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 enum ao_flight_state    ao_flight_state;
  27
  28 static uint8_t  ao_tracker_force_telem;
  29 static uint8_t  ao_tracker_force_launch;
  30
  31 #if HAS_USB_CONNECT
  32 static inline uint8_t
  33 ao_usb_connected(void)
  34 {
  35         return ao_gpio_get(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, AO_USB_CONNECT) != 0;
  36 }
  37 #else
  38 #define ao_usb_connected()      1
  39 #endif
  40
  41 #define STARTUP_AVERAGE 5
  42
  43 int32_t ao_tracker_start_latitude;
  44 int32_t ao_tracker_start_longitude;
  45 int16_t ao_tracker_start_altitude;
  46
  47 struct ao_tracker_data  ao_tracker_data[AO_TRACKER_RING];
  48 uint8_t                 ao_tracker_head;
  49 static uint8_t          ao_tracker_log_pos;
  50
  51 static uint16_t telem_rate;
  52 static uint8_t  gps_rate;
  53 static uint8_t  telem_enabled;
  54
  55 static int64_t  lat_sum, lon_sum;
  56 static int32_t  alt_sum;
  57 static int      nsamples;
  58
  59 static void
  60 ao_tracker_state_update(struct ao_tracker_data *tracker)
  61 {
  62         uint16_t        new_telem_rate;
  63         uint8_t         new_gps_rate;
  64         uint8_t         new_telem_enabled;
  65         uint32_t        ground_distance;
  66         int16_t         height;
  67         uint16_t        speed;
  68
  69         new_gps_rate = gps_rate;
  70         new_telem_rate = telem_rate;
  71
  72         new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected();
  73
  74         /* Don't change anything if GPS isn't locked */
  75         if ((tracker->new & AO_GPS_NEW_DATA) &&
  76             (tracker->gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
  77             (AO_GPS_VALID|AO_GPS_COURSE_VALID))
  78         {
  79                 switch (ao_flight_state) {
  80                 case ao_flight_startup:
  81                         new_telem_rate = AO_SEC_TO_TICKS(1);
  82                         new_gps_rate = 1;
  83
  84                         /* startup to pad when GPS locks */
  85
  86                         lat_sum += tracker->gps_data.latitude;
  87                         lon_sum += tracker->gps_data.longitude;
  88                         alt_sum += tracker->gps_data.altitude;
  89
  90                         ++nsamples;
  91
  92                         if (nsamples >= STARTUP_AVERAGE) {
  93                                 ao_flight_state = ao_flight_pad;
  94                                 ao_wakeup(&ao_flight_state);
  95                                 ao_tracker_start_latitude = lat_sum / nsamples;
  96                                 ao_tracker_start_longitude = lon_sum / nsamples;
  97                                 ao_tracker_start_altitude = alt_sum / nsamples;
  98                         }
  99                         break;
 100                 case ao_flight_pad:
 101                         new_telem_rate = AO_SEC_TO_TICKS(1);
 102                         new_gps_rate = 1;
 103
 104                         ground_distance = ao_distance(tracker->gps_data.latitude,
 105                                                       tracker->gps_data.longitude,
 106                                                       ao_tracker_start_latitude,
 107                                                       ao_tracker_start_longitude);
 108                         height = tracker->gps_data.altitude - ao_tracker_start_altitude;
 109                         if (height < 0)
 110                                 height = -height;
 111
 112                         if (ground_distance >= ao_config.tracker_start_horiz ||
 113                             height >= ao_config.tracker_start_vert ||
 114                             ao_tracker_force_launch)
 115                         {
 116                                 ao_flight_state = ao_flight_drogue;
 117                                 ao_wakeup(&ao_flight_state);
 118                                 ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_head);
 119                                 ao_log_start();
 120                                 ao_log_gps_flight();
 121                         }
 122                         break;
 123                 case ao_flight_drogue:
 124                         /* Modulate data rates based on speed (in cm/s) */
 125                         if (tracker->gps_data.climb_rate < 0)
 126                                 speed = -tracker->gps_data.climb_rate;
 127                         else
 128                                 speed = tracker->gps_data.climb_rate;
 129                         speed += tracker->gps_data.ground_speed;
 130
 131                         if (speed < AO_TRACKER_NOT_MOVING) {
 132                                 new_telem_rate = AO_SEC_TO_TICKS(10);
 133                                 new_gps_rate = 10;
 134                         } else {
 135                                 new_telem_rate = AO_SEC_TO_TICKS(1);
 136                                 new_gps_rate = 1;
 137                         }
 138                         break;
 139                 default:
 140                         break;
 141                 }
 142         }
 143
 144         if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) {
 145                 if (new_telem_enabled)
 146                         ao_telemetry_set_interval(new_telem_rate);
 147                 else
 148                         ao_telemetry_set_interval(0);
 149                 telem_rate = new_telem_rate;
 150                 telem_enabled = new_telem_enabled;
 151         }
 152
 153         if (new_gps_rate != gps_rate) {
 154                 ao_gps_set_rate(new_gps_rate);
 155                 gps_rate = new_gps_rate;
 156         }
 157 }
 158
 159 #if HAS_LOG
 160 static uint8_t  ao_tracker_should_log;
 161
 162 static void
 163 ao_tracker_log(void)
 164 {
 165         struct ao_tracker_data  *tracker;
 166
 167         if (ao_log_running) {
 168                 while (ao_tracker_log_pos != ao_tracker_head) {
 169                         tracker = &ao_tracker_data[ao_tracker_log_pos];
 170                         if (tracker->new & AO_GPS_NEW_DATA) {
 171                                 ao_tracker_should_log = ao_log_gps_should_log(tracker->gps_data.latitude,
 172                                                                               tracker->gps_data.longitude,
 173                                                                               tracker->gps_data.altitude);
 174                                 if (ao_tracker_should_log)
 175                                         ao_log_gps_data(tracker->tick, tracker->state, &tracker->gps_data);
 176                         }
 177                         if (tracker->new & AO_GPS_NEW_TRACKING) {
 178                                 if (ao_tracker_should_log)
 179                                         ao_log_gps_tracking(tracker->tick, &tracker->gps_tracking_data);
 180                         }
 181                         ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_log_pos);
 182                 }
 183         }
 184 }
 185 #endif
 186
 187 static void
 188 ao_tracker(void)
 189 {
 190         uint8_t         new;
 191         struct ao_tracker_data  *tracker;
 192
 193 #if HAS_ADC
 194         ao_timer_set_adc_interval(100);
 195 #endif
 196
 197 #if !HAS_USB_CONNECT
 198         ao_tracker_force_telem = 1;
 199 #endif
 200
 201         nsamples = 0;
 202         lat_sum = 0;
 203         lon_sum = 0;
 204         alt_sum = 0;
 205
 206         ao_log_scan();
 207
 208         ao_rdf_set(1);
 209         ao_telemetry_set_interval(0);
 210         telem_rate = AO_SEC_TO_TICKS(1);
 211         telem_enabled = 0;
 212         gps_rate = 1;
 213
 214         ao_flight_state = ao_flight_startup;
 215         for (;;) {
 216                 while (!(new = ao_gps_new))
 217                         ao_sleep(&ao_gps_new);
 218
 219                 /* Stick GPS data into the ring */
 220                 ao_mutex_get(&ao_gps_mutex);
 221                 tracker = &ao_tracker_data[ao_tracker_head];
 222                 tracker->tick = ao_gps_tick;
 223                 tracker->new = new;
 224                 tracker->state = ao_flight_state;
 225                 tracker->gps_data = ao_gps_data;
 226                 tracker->gps_tracking_data = ao_gps_tracking_data;
 227                 ao_tracker_head = ao_tracker_ring_next(ao_tracker_head);
 228
 229                 ao_gps_new = 0;
 230                 ao_mutex_put(&ao_gps_mutex);
 231
 232                 /* Update state based on current GPS data */
 233                 ao_tracker_state_update(tracker);
 234
 235 #if HAS_LOG
 236                 /* Log all gps data */
 237                 ao_tracker_log();
 238 #endif
 239         }
 240 }
 241
 242 static struct ao_task ao_tracker_task;
 243
 244 static void
 245 ao_tracker_set_telem(void)
 246 {
 247         uint8_t telem, launch;
 248         ao_cmd_hex();
 249         telem = ao_cmd_lex_i;
 250         ao_cmd_hex();
 251         launch = ao_cmd_lex_i;
 252         if (ao_cmd_status == ao_cmd_success) {
 253                 ao_tracker_force_telem = telem;
 254                 ao_tracker_force_launch = launch;
 255         }
 256         ao_cmd_status = ao_cmd_success;
 257         printf ("flight %d force telem %d force launch %d\n",
 258                 ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch);
 259 }
 260
 261 static const struct ao_cmds ao_tracker_cmds[] = {
 262         { ao_tracker_set_telem, "t <d>\0Set telem on USB" },
 263         { 0, NULL },
 264 };
 265
 266 void
 267 ao_tracker_init(void)
 268 {
 269 #if HAS_USB_CONNECT
 270         ao_enable_input(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, 0);
 271 #endif
 272         ao_cmd_register(&ao_tracker_cmds[0]);
 273         ao_add_task(&ao_tracker_task, ao_tracker, "tracker");
 274 }