git.gag.com Git - fw/altos/blob - src/kernel/ao_tracker.c

   1 /*
   2  * Copyright © 2014 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #include <ao.h>
  20 #include <ao_flight.h>
  21 #include <ao_log.h>
  22 #include <ao_log_gps.h>
  23 #include <ao_distance.h>
  24 #include <ao_tracker.h>
  25 #include <ao_exti.h>
  26
  27 static uint8_t          ao_tracker_force_telem;
  28
  29 #if HAS_USB_CONNECT
  30 static inline uint8_t
  31 ao_usb_connected(void)
  32 {
  33         return ao_gpio_get(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN) != 0;
  34 }
  35 #else
  36 #define ao_usb_connected()      1
  37 #endif
  38
  39 struct gps_position {
  40         int32_t         latitude;
  41         int32_t         longitude;
  42         gps_alt_t       altitude;
  43 };
  44
  45 #define GPS_RING        16
  46
  47 struct gps_position     gps_position[GPS_RING];
  48
  49 #define ao_gps_ring_next(n)     (((n) + 1) & (GPS_RING - 1))
  50 #define ao_gps_ring_prev(n)     (((n) - 1) & (GPS_RING - 1))
  51
  52 static uint8_t  gps_head;
  53
  54 static uint8_t  tracker_mutex;
  55 static uint8_t  log_started;
  56 static struct ao_telemetry_location gps_data;
  57 static uint8_t  tracker_running;
  58 static uint16_t tracker_interval;
  59
  60 static void
  61 ao_tracker(void)
  62 {
  63         uint8_t new;
  64         int32_t ground_distance;
  65         int16_t height;
  66         uint16_t gps_tick;
  67         uint8_t new_tracker_running;
  68
  69 #if HAS_ADC
  70         ao_timer_set_adc_interval(100);
  71 #endif
  72
  73 #if !HAS_USB_CONNECT
  74         ao_tracker_force_telem = 1;
  75 #endif
  76         log_started = ao_log_scan();
  77
  78         ao_rdf_set(1);
  79
  80         tracker_interval = ao_config.tracker_interval;
  81         ao_gps_set_rate(tracker_interval);
  82
  83         for (;;) {
  84
  85                 /** Wait for new GPS data
  86                  */
  87                 while (!(new = ao_gps_new))
  88                         ao_sleep(&ao_gps_new);
  89                 ao_mutex_get(&ao_gps_mutex);
  90                 gps_data = ao_gps_data;
  91                 gps_tick = ao_gps_tick;
  92                 ao_gps_new = 0;
  93                 ao_mutex_put(&ao_gps_mutex);
  94
  95                 new_tracker_running = ao_tracker_force_telem || !ao_usb_connected();
  96
  97                 if (ao_config.tracker_interval != tracker_interval) {
  98                         tracker_interval = ao_config.tracker_interval;
  99                         ao_gps_set_rate(tracker_interval);
 100
 101                         /* force telemetry interval to be reset */
 102                         tracker_running = 0;
 103                 }
 104
 105                 if (new_tracker_running && !tracker_running)
 106                         ao_telemetry_set_interval(AO_SEC_TO_TICKS(tracker_interval));
 107                 else if (!new_tracker_running && tracker_running)
 108                         ao_telemetry_set_interval(0);
 109
 110                 tracker_running = new_tracker_running;
 111
 112                 if (new_tracker_running && !ao_log_running)
 113                         ao_log_start();
 114                 else if (!new_tracker_running && ao_log_running)
 115                         ao_log_stop();
 116
 117                 if (!ao_log_running)
 118                         continue;
 119
 120                 if (new & AO_GPS_NEW_DATA) {
 121                         if ((gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
 122                             (AO_GPS_VALID|AO_GPS_COURSE_VALID))
 123                         {
 124                                 uint8_t ring;
 125                                 uint8_t moving = 0;
 126                                 gps_alt_t altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data);
 127
 128                                 for (ring = ao_gps_ring_next(gps_head); ring != gps_head; ring = ao_gps_ring_next(ring)) {
 129                                         ground_distance = ao_distance(gps_data.latitude, gps_data.longitude,
 130                                                                       gps_position[ring].latitude,
 131                                                                       gps_position[ring].longitude);
 132                                         height = gps_position[ring].altitude - altitude;
 133                                         if (height < 0)
 134                                                 height = -height;
 135
 136                                         if (ao_tracker_force_telem > 1)
 137                                                 printf("head %d ring %d ground_distance %d height %d\n", gps_head, ring, ground_distance, height);
 138                                         if (ground_distance > ao_config.tracker_motion ||
 139                                             height > (ao_config.tracker_motion << 1))
 140                                         {
 141                                                 moving = 1;
 142                                                 break;
 143                                         }
 144                                 }
 145                                 if (ao_tracker_force_telem > 1) {
 146                                         printf ("moving %d started %d\n", moving, log_started);
 147                                         flush();
 148                                 }
 149                                 if (moving) {
 150                                         ao_mutex_get(&tracker_mutex);
 151                                         if (!log_started) {
 152                                                 ao_log_gps_flight();
 153                                                 log_started = 1;
 154                                         }
 155                                         ao_log_gps_data(gps_tick, &gps_data);
 156                                         gps_position[gps_head].latitude = gps_data.latitude;
 157                                         gps_position[gps_head].longitude = gps_data.longitude;
 158                                         gps_position[gps_head].altitude = altitude;
 159                                         gps_head = ao_gps_ring_next(gps_head);
 160                                         ao_mutex_put(&tracker_mutex);
 161                                 }
 162                         }
 163                 }
 164         }
 165 }
 166
 167 #ifdef AO_LED_GPS_LOCK
 168
 169 static struct ao_task ao_gps_lock_task;
 170
 171 static void
 172 ao_gps_lock(void)
 173 {
 174         for (;;) {
 175                 if ((gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
 176                     (AO_GPS_VALID|AO_GPS_COURSE_VALID))
 177                 {
 178                         ao_led_for(AO_LED_GPS_LOCK, AO_MS_TO_TICKS(20));
 179                 }
 180                 ao_delay(AO_SEC_TO_TICKS(3));
 181         }
 182 }
 183 #endif
 184
 185
 186 static uint8_t erasing_current;
 187
 188 void
 189 ao_tracker_erase_start(uint16_t flight)
 190 {
 191         erasing_current = flight == ao_flight_number;
 192         if (erasing_current) {
 193                 ao_mutex_get(&tracker_mutex);
 194                 ao_log_stop();
 195                 if (++ao_flight_number == 0)
 196                         ao_flight_number = 1;
 197         }
 198 }
 199
 200 void
 201 ao_tracker_erase_end(void)
 202 {
 203         if (erasing_current) {
 204                 log_started = ao_log_scan();
 205                 ao_mutex_put(&tracker_mutex);
 206         }
 207 }
 208
 209 static struct ao_task ao_tracker_task;
 210
 211 static void
 212 ao_tracker_set_telem(void)
 213 {
 214         uint16_t r = ao_cmd_hex();
 215         if (ao_cmd_status == ao_cmd_success)
 216                 ao_tracker_force_telem = r;
 217         ao_cmd_status = ao_cmd_success;
 218         printf ("flight: %d\n", ao_flight_number);
 219         printf ("force_telem: %d\n", ao_tracker_force_telem);
 220         printf ("tracker_running: %d\n", tracker_running);
 221         printf ("log_started: %d\n", log_started);
 222         printf ("latitude: %ld\n", (long) gps_data.latitude);
 223         printf ("longitude: %ld\n", (long) gps_data.longitude);
 224         printf ("altitude: %ld\n", AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data));
 225         printf ("log_running: %d\n", ao_log_running);
 226         printf ("log_start_pos: %ld\n", (long) ao_log_start_pos);
 227         printf ("log_cur_pos: %ld\n", (long) ao_log_current_pos);
 228         printf ("log_end_pos: %ld\n", (long) ao_log_end_pos);
 229 }
 230
 231 static const struct ao_cmds ao_tracker_cmds[] = {
 232         { ao_tracker_set_telem, "t <d>\0Set telem on USB (0 off, 1 on, 2 dbg)" },
 233         { 0, NULL },
 234 };
 235
 236 void
 237 ao_tracker_init(void)
 238 {
 239 #if HAS_USB_CONNECT
 240         ao_enable_input(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, 0);
 241 #endif
 242         ao_cmd_register(&ao_tracker_cmds[0]);
 243         ao_add_task(&ao_tracker_task, ao_tracker, "tracker");
 244 #ifdef AO_LED_GPS_LOCK
 245         ao_add_task(&ao_gps_lock_task, ao_gps_lock, "gps lock");
 246 #endif
 247 }