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