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         int16_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  log_started;
  54 static struct ao_telemetry_location gps_data;
  55 static uint8_t  tracker_running;
  56 static uint16_t tracker_interval;
  57
  58 static void
  59 ao_tracker(void)
  60 {
  61         uint8_t new;
  62         int32_t ground_distance;
  63         int16_t height;
  64         uint16_t gps_tick;
  65         uint8_t new_tracker_running;
  66
  67 #if HAS_ADC
  68         ao_timer_set_adc_interval(100);
  69 #endif
  70
  71 #if !HAS_USB_CONNECT
  72         ao_tracker_force_telem = 1;
  73 #endif
  74         ao_log_scan();
  75
  76         ao_rdf_set(1);
  77
  78         tracker_interval = ao_config.tracker_interval;
  79         ao_gps_set_rate(tracker_interval);
  80
  81         for (;;) {
  82
  83                 /** Wait for new GPS data
  84                  */
  85                 while (!(new = ao_gps_new))
  86                         ao_sleep(&ao_gps_new);
  87                 ao_mutex_get(&ao_gps_mutex);
  88                 gps_data = ao_gps_data;
  89                 gps_tick = ao_gps_tick;
  90                 ao_gps_new = 0;
  91                 ao_mutex_put(&ao_gps_mutex);
  92
  93                 new_tracker_running = ao_tracker_force_telem || !ao_usb_connected();
  94
  95                 if (ao_config.tracker_interval != tracker_interval) {
  96                         tracker_interval = ao_config.tracker_interval;
  97                         ao_gps_set_rate(tracker_interval);
  98
  99                         /* force telemetry interval to be reset */
 100                         tracker_running = 0;
 101                 }
 102
 103                 if (new_tracker_running && !tracker_running) {
 104                         ao_telemetry_set_interval(AO_SEC_TO_TICKS(tracker_interval));
 105                         ao_log_start();
 106                 } else if (!new_tracker_running && tracker_running) {
 107                         ao_telemetry_set_interval(0);
 108                         ao_log_stop();
 109                 }
 110
 111                 tracker_running = new_tracker_running;
 112
 113                 if (!tracker_running)
 114                         continue;
 115
 116                 if (new & AO_GPS_NEW_DATA) {
 117                         if ((gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
 118                             (AO_GPS_VALID|AO_GPS_COURSE_VALID))
 119                         {
 120                                 uint8_t ring;
 121                                 uint8_t moving = 0;
 122
 123                                 for (ring = ao_gps_ring_next(gps_head); ring != gps_head; ring = ao_gps_ring_next(ring)) {
 124                                         ground_distance = ao_distance(gps_data.latitude, gps_data.longitude,
 125                                                                       gps_position[ring].latitude,
 126                                                                       gps_position[ring].longitude);
 127                                         height = gps_position[ring].altitude - gps_data.altitude;
 128                                         if (height < 0)
 129                                                 height = -height;
 130
 131                                         if (ao_tracker_force_telem)
 132                                                 printf("head %d ring %d ground_distance %d height %d\n", gps_head, ring, ground_distance, height);
 133                                         if (ground_distance > ao_config.tracker_motion ||
 134                                             height > (ao_config.tracker_motion << 1))
 135                                         {
 136                                                 moving = 1;
 137                                                 break;
 138                                         }
 139                                 }
 140                                 if (ao_tracker_force_telem) {
 141                                         printf ("moving %d\n", moving);
 142                                         flush();
 143                                 }
 144                                 if (moving) {
 145                                         if (!log_started) {
 146                                                 ao_log_gps_flight();
 147                                                 log_started = 1;
 148                                         }
 149                                         ao_log_gps_data(gps_tick, &gps_data);
 150                                         gps_position[gps_head].latitude = gps_data.latitude;
 151                                         gps_position[gps_head].longitude = gps_data.longitude;
 152                                         gps_position[gps_head].altitude = gps_data.altitude;
 153                                         gps_head = ao_gps_ring_next(gps_head);
 154                                 }
 155                         }
 156                 }
 157         }
 158 }
 159
 160 static struct ao_task ao_tracker_task;
 161
 162 static void
 163 ao_tracker_set_telem(void)
 164 {
 165         uint8_t telem;
 166         ao_cmd_hex();
 167         telem = ao_cmd_lex_i;
 168         if (ao_cmd_status == ao_cmd_success)
 169                 ao_tracker_force_telem = telem;
 170         ao_cmd_status = ao_cmd_success;
 171         printf ("flight: %d\n", ao_flight_number);
 172         printf ("force_telem: %d\n", ao_tracker_force_telem);
 173         printf ("tracker_running: %d\n", tracker_running);
 174         printf ("log_started: %d\n", log_started);
 175         printf ("latitude: %ld\n", (long) gps_data.latitude);
 176         printf ("longitude: %ld\n", (long) gps_data.longitude);
 177         printf ("altitude: %d\n", gps_data.altitude);
 178         printf ("log_running: %d\n", ao_log_running);
 179         printf ("log_start_pos: %ld\n", (long) ao_log_start_pos);
 180         printf ("log_cur_pos: %ld\n", (long) ao_log_current_pos);
 181         printf ("log_end_pos: %ld\n", (long) ao_log_end_pos);
 182 }
 183
 184 static const struct ao_cmds ao_tracker_cmds[] = {
 185         { ao_tracker_set_telem, "t <d>\0Set telem on USB" },
 186         { 0, NULL },
 187 };
 188
 189 void
 190 ao_tracker_init(void)
 191 {
 192 #if HAS_USB_CONNECT
 193         ao_enable_input(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, 0);
 194 #endif
 195         ao_cmd_register(&ao_tracker_cmds[0]);
 196         ao_add_task(&ao_tracker_task, ao_tracker, "tracker");
 197 }