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 int16_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                         /* startup to pad when GPS locks */
  82
  83                         lat_sum += tracker->gps_data.latitude;
  84                         lon_sum += tracker->gps_data.longitude;
  85                         alt_sum += tracker->gps_data.altitude;
  86
  87                         if (++nsamples >= STARTUP_AVERAGE) {
  88                                 ao_flight_state = ao_flight_pad;
  89                                 ao_wakeup(&ao_flight_state);
  90                                 ao_tracker_start_latitude = lat_sum / nsamples;
  91                                 ao_tracker_start_longitude = lon_sum / nsamples;
  92                                 ao_tracker_start_altitude = alt_sum / nsamples;
  93                         }
  94                         break;
  95                 case ao_flight_pad:
  96                         ground_distance = ao_distance(tracker->gps_data.latitude,
  97                                                       tracker->gps_data.longitude,
  98                                                       ao_tracker_start_latitude,
  99                                                       ao_tracker_start_longitude);
 100                         height = tracker->gps_data.altitude - ao_tracker_start_altitude;
 101                         if (height < 0)
 102                                 height = -height;
 103
 104                         if (ground_distance >= ao_config.tracker_start_horiz ||
 105                             height >= ao_config.tracker_start_vert ||
 106                             ao_tracker_force_launch)
 107                         {
 108                                 ao_flight_state = ao_flight_drogue;
 109                                 ao_wakeup(&ao_flight_state);
 110                                 ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_head);
 111                                 ao_log_start();
 112                                 ao_log_gps_flight();
 113                         }
 114                         break;
 115                 case ao_flight_drogue:
 116                         /* Modulate data rates based on speed (in cm/s) */
 117                         if (tracker->gps_data.climb_rate < 0)
 118                                 speed = -tracker->gps_data.climb_rate;
 119                         else
 120                                 speed = tracker->gps_data.climb_rate;
 121                         speed += tracker->gps_data.ground_speed;
 122
 123                         if (speed < AO_TRACKER_NOT_MOVING) {
 124                                 new_telem_rate = AO_SEC_TO_TICKS(10);
 125                                 new_gps_rate = 10;
 126                         } else {
 127                                 new_telem_rate = AO_SEC_TO_TICKS(1);
 128                                 new_gps_rate = 1;
 129                         }
 130                         break;
 131                 default:
 132                         break;
 133                 }
 134         }
 135
 136         if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) {
 137                 if (new_telem_enabled)
 138                         ao_telemetry_set_interval(new_telem_rate);
 139                 else
 140                         ao_telemetry_set_interval(0);
 141                 telem_rate = new_telem_rate;
 142                 telem_enabled = new_telem_enabled;
 143         }
 144
 145         if (new_gps_rate != gps_rate) {
 146                 ao_gps_set_rate(new_gps_rate);
 147                 gps_rate = new_gps_rate;
 148         }
 149 }
 150
 151 #if HAS_LOG
 152 static uint8_t  ao_tracker_should_log;
 153
 154 static void
 155 ao_tracker_log(void)
 156 {
 157         struct ao_tracker_data  *tracker;
 158
 159         if (ao_log_running) {
 160                 while (ao_tracker_log_pos != ao_tracker_head) {
 161                         tracker = &ao_tracker_data[ao_tracker_log_pos];
 162                         if (tracker->new & AO_GPS_NEW_DATA) {
 163                                 ao_tracker_should_log = ao_log_gps_should_log(tracker->gps_data.latitude,
 164                                                                               tracker->gps_data.longitude,
 165                                                                               tracker->gps_data.altitude);
 166                                 if (ao_tracker_should_log)
 167                                         ao_log_gps_data(tracker->tick, tracker->state, &tracker->gps_data);
 168                         }
 169                         if (tracker->new & AO_GPS_NEW_TRACKING) {
 170                                 if (ao_tracker_should_log)
 171                                         ao_log_gps_tracking(tracker->tick, &tracker->gps_tracking_data);
 172                         }
 173                         ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_log_pos);
 174                 }
 175         }
 176 }
 177 #endif
 178
 179 static void
 180 ao_tracker(void)
 181 {
 182         uint8_t         new;
 183         struct ao_tracker_data  *tracker;
 184
 185 #if HAS_ADC
 186         ao_timer_set_adc_interval(100);
 187 #endif
 188
 189 #if !HAS_USB_CONNECT
 190         ao_tracker_force_telem = 1;
 191 #endif
 192
 193         ao_log_scan();
 194
 195         ao_rdf_set(1);
 196         ao_telemetry_set_interval(0);
 197         telem_rate = AO_SEC_TO_TICKS(1);
 198         telem_enabled = 0;
 199         gps_rate = 1;
 200
 201         ao_flight_state = ao_flight_startup;
 202         for (;;) {
 203                 while (!(new = ao_gps_new))
 204                         ao_sleep(&ao_gps_new);
 205
 206                 /* Stick GPS data into the ring */
 207                 ao_mutex_get(&ao_gps_mutex);
 208                 tracker = &ao_tracker_data[ao_tracker_head];
 209                 tracker->tick = ao_gps_tick;
 210                 tracker->new = new;
 211                 tracker->state = ao_flight_state;
 212                 tracker->gps_data = ao_gps_data;
 213                 tracker->gps_tracking_data = ao_gps_tracking_data;
 214                 ao_tracker_head = ao_tracker_ring_next(ao_tracker_head);
 215
 216                 ao_gps_new = 0;
 217                 ao_mutex_put(&ao_gps_mutex);
 218
 219                 /* Update state based on current GPS data */
 220                 ao_tracker_state_update(tracker);
 221
 222 #if HAS_LOG
 223                 /* Log all gps data */
 224                 ao_tracker_log();
 225 #endif
 226         }
 227 }
 228
 229 static struct ao_task ao_tracker_task;
 230
 231 static void
 232 ao_tracker_set_telem(void)
 233 {
 234         ao_cmd_hex();
 235         if (ao_cmd_status == ao_cmd_success) {
 236                 ao_tracker_force_telem = (ao_cmd_lex_i & 1) != 0;
 237                 ao_tracker_force_launch = (ao_cmd_lex_i & 2) != 0;
 238         }
 239         printf ("flight %d force telem %d force launch %d\n",
 240                 ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch);
 241 }
 242
 243 static const struct ao_cmds ao_tracker_cmds[] = {
 244         { ao_tracker_set_telem, "t <d>\0Set telem on USB" },
 245         { 0, NULL },
 246 };
 247
 248 void
 249 ao_tracker_init(void)
 250 {
 251 #if HAS_USB_CONNECT
 252         ao_enable_input(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, 0);
 253 #endif
 254         ao_cmd_register(&ao_tracker_cmds[0]);
 255         ao_add_task(&ao_tracker_task, ao_tracker, "tracker");
 256 }