altos: Simplify tracker logic, removing boost detect

This removes the ao_flight_state value from the tracker code and makes
it simply log position information when the device has moved within
the last 10 log intervals. This also changes the configuration
parameters to define what 'motionless' means, and what interval to
configure the GPS receiver for, log data and send telemetry.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
index 721705551f66ec32bb02d10b7ffdeee888e6d01b..71445335a7a39901c98b91c4346f1cbd1e6686e1 100644 (file)
--- a/src/kernel/ao_config.c
+++ b/src/kernel/ao_config.c
@@ -184,8 +184,8 @@ _ao_config_get(void)
 #endif
 #if HAS_TRACKER
                if (minor < 17) {
-                       ao_config.tracker_start_horiz = AO_CONFIG_DEFAULT_TRACKER_START_HORIZ;
-                       ao_config.tracker_start_vert = AO_CONFIG_DEFAULT_TRACKER_START_VERT;
+                       ao_config.tracker_motion = AO_TRACKER_MOTION_DEFAULT;
+                       ao_config.tracker_interval = AO_TRACKER_INTERVAL_DEFAULT;
                }
 #endif
 #if AO_PYRO_NUM
@@ -695,25 +695,25 @@ void
 ao_config_tracker_show(void)
 {
        printf ("Tracker setting: %d %d\n",
-               ao_config.tracker_start_horiz,
-               ao_config.tracker_start_vert);
+               ao_config.tracker_motion,
+               ao_config.tracker_interval);
 }
 
 void
 ao_config_tracker_set(void)
 {
-       uint16_t        h, v;
+       uint16_t        m, i;
        ao_cmd_decimal();
        if (ao_cmd_status != ao_cmd_success)
                return;
-       h = ao_cmd_lex_i;
+       m = ao_cmd_lex_i;
        ao_cmd_decimal();
        if (ao_cmd_status != ao_cmd_success)
                return;
-       v = ao_cmd_lex_i;
+       i = ao_cmd_lex_i;
        _ao_config_edit_start();
-       ao_config.tracker_start_horiz = h;
-       ao_config.tracker_start_vert = v;
+       ao_config.tracker_motion = m;
+       ao_config.tracker_interval = i;
        _ao_config_edit_finish();
 }
 #endif /* HAS_TRACKER */
@@ -814,7 +814,7 @@ __code struct ao_config_var ao_config_vars[] = {
          ao_config_beep_set, ao_config_beep_show },
 #endif
 #if HAS_TRACKER
-       { "t <horiz> <vert>\0Tracker start trigger distances",
+       { "t <motion> <interval>\0Tracker configuration",
          ao_config_tracker_set, ao_config_tracker_show },
 #endif
        { "s\0Show",

diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h
index 77f73fbe6014749b054949f82c46937272bfc516..2b5cd352300fed22d9c89ceb7fb0b4dbdeb73aee 100644 (file)
--- a/src/kernel/ao_config.h
+++ b/src/kernel/ao_config.h
@@ -96,8 +96,8 @@ struct ao_config {
        uint8_t         mid_beep;               /* minor version 16 */
 #endif
 #if HAS_TRACKER
-       uint16_t        tracker_start_horiz;    /* minor version 17 */
-       uint16_t        tracker_start_vert;     /* minor version 17 */
+       uint16_t        tracker_motion;         /* minor version 17 */
+       uint8_t         tracker_interval;       /* minor version 17 */
 #endif
 #if AO_PYRO_NUM
        uint16_t        pyro_time;              /* minor version 18 */

diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c
index e9e573a52fc5a0c1f6c21483feaa14d48a280b61..8bf529f40b862a2eff145338bd2e83346aee08bb 100644 (file)
--- a/src/kernel/ao_log_gps.c
+++ b/src/kernel/ao_log_gps.c
@@ -60,51 +60,17 @@ ao_log_gps(__xdata struct ao_log_gps *log) __reentrant
        return wrote;
 }
 
-
-static int32_t prev_lat, prev_lon;
-static int16_t  prev_alt;
-static uint8_t has_prev, unmoving;
-
-#define GPS_SPARSE_UNMOVING_REPORTS    10
-#define GPS_SPARSE_UNMOVING_GROUND     10
-#define GPS_SPARSE_UNMOVING_AIR                10
-
-uint8_t
-ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt)
-{
-       if (has_prev && ao_log_running) {
-               uint32_t        h = ao_distance(prev_lat, prev_lon, lat, lon);
-               uint16_t        v = alt > prev_alt ? (alt - prev_alt) : (prev_alt - alt);
-
-               if (h < GPS_SPARSE_UNMOVING_GROUND && v < GPS_SPARSE_UNMOVING_AIR) {
-                       if (unmoving < GPS_SPARSE_UNMOVING_REPORTS)
-                               ++unmoving;
-               } else
-                       unmoving = 0;
-       } else
-               unmoving = 0;
-
-       prev_lat = lat;
-       prev_lon = lon;
-       prev_alt = alt;
-       has_prev = 1;
-       return unmoving >= GPS_SPARSE_UNMOVING_REPORTS;
-}
-
 void
 ao_log_gps_flight(void)
 {
        log.type = AO_LOG_FLIGHT;
        log.tick = ao_time();
        log.u.flight.flight = ao_flight_number;
-       log.u.flight.start_altitude = ao_tracker_start_altitude;
-       log.u.flight.start_latitude = ao_tracker_start_latitude;
-       log.u.flight.start_longitude = ao_tracker_start_longitude;
        ao_log_gps(&log);
 }
 
 void
-ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data)
+ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data)
 {
        log.tick = tick;
        log.type = AO_LOG_GPS_TIME;
@@ -126,7 +92,6 @@ ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_
        log.u.gps.hdop = gps_data->hdop;
        log.u.gps.vdop = gps_data->vdop;
        log.u.gps.mode = gps_data->mode;
-       log.u.gps.state = state;
        ao_log_gps(&log);
 }
 

diff --git a/src/kernel/ao_log_gps.h b/src/kernel/ao_log_gps.h
index 733db19b92110b4dd7eb5a846c235e2c9963388d..5851f4d1fadbeedbe83d57a9fa797cab85cbf3a3 100644 (file)
--- a/src/kernel/ao_log_gps.h
+++ b/src/kernel/ao_log_gps.h
@@ -18,6 +18,9 @@
 #include "ao.h"
 #include "ao_telemetry.h"
 
+#ifndef _AO_LOG_GPS_H_
+#define _AO_LOG_GPS_H_
+
 uint8_t
 ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt);
 
@@ -25,8 +28,6 @@ void
 ao_log_gps_flight(void);
 
 void
-ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data);
-
-void
-ao_log_gps_tracking(uint16_t tick, struct ao_telemetry_satellite *gps_tracking_data);
+ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data);
 
+#endif /* _AO_LOG_GPS_H_ */

diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c
index cdf147cd35bea750819d545f2f7c9c396f56dc6d..fb9e75d06d376db77b850ca66e907c44d7c72fd4 100644 (file)
--- a/src/kernel/ao_tracker.c
+++ b/src/kernel/ao_tracker.c
@@ -23,10 +23,7 @@
 #include <ao_tracker.h>
 #include <ao_exti.h>
 
-enum ao_flight_state   ao_flight_state;
-
-static uint8_t ao_tracker_force_telem;
-static uint8_t ao_tracker_force_launch;
+static uint8_t         ao_tracker_force_telem;
 
 #if HAS_USB_CONNECT
 static inline uint8_t
@@ -38,157 +35,22 @@ ao_usb_connected(void)
 #define ao_usb_connected()     1
 #endif
 
-#define STARTUP_AVERAGE        5
-
-int32_t        ao_tracker_start_latitude;
-int32_t        ao_tracker_start_longitude;
-int16_t        ao_tracker_start_altitude;
-
-struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING];
-uint8_t                        ao_tracker_head;
-static uint8_t         ao_tracker_log_pos;
-
-static uint16_t        telem_rate;
-static uint8_t gps_rate;
-static uint8_t telem_enabled;
-
-static int64_t lat_sum, lon_sum;
-static int32_t alt_sum;
-static int     nsamples;
-
-static void
-ao_tracker_state_update(struct ao_tracker_data *tracker)
-{
-       uint16_t        new_telem_rate;
-       uint8_t         new_gps_rate;
-       uint8_t         new_telem_enabled;
-       uint32_t        ground_distance;
-       int16_t         height;
-       uint16_t        speed;
-
-       new_gps_rate = gps_rate;
-       new_telem_rate = telem_rate;
-
-       new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected();
-
-       /* Don't change anything if GPS isn't locked */
-       if ((tracker->new & AO_GPS_NEW_DATA) &&
-           (tracker->gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
-           (AO_GPS_VALID|AO_GPS_COURSE_VALID))
-       {
-               switch (ao_flight_state) {
-               case ao_flight_startup:
-                       new_telem_rate = AO_SEC_TO_TICKS(1);
-                       new_gps_rate = 1;
-
-                       /* startup to pad when GPS locks */
-
-                       lat_sum += tracker->gps_data.latitude;
-                       lon_sum += tracker->gps_data.longitude;
-                       alt_sum += tracker->gps_data.altitude;
-
-                       ++nsamples;
-
-                       if (nsamples >= STARTUP_AVERAGE) {
-                               ao_flight_state = ao_flight_pad;
-                               ao_wakeup(&ao_flight_state);
-                               ao_tracker_start_latitude = lat_sum / nsamples;
-                               ao_tracker_start_longitude = lon_sum / nsamples;
-                               ao_tracker_start_altitude = alt_sum / nsamples;
-                       }
-                       break;
-               case ao_flight_pad:
-                       new_telem_rate = AO_SEC_TO_TICKS(1);
-                       new_gps_rate = 1;
-
-                       ground_distance = ao_distance(tracker->gps_data.latitude,
-                                                     tracker->gps_data.longitude,
-                                                     ao_tracker_start_latitude,
-                                                     ao_tracker_start_longitude);
-                       height = tracker->gps_data.altitude - ao_tracker_start_altitude;
-                       if (height < 0)
-                               height = -height;
-
-                       if (ground_distance >= ao_config.tracker_start_horiz ||
-                           height >= ao_config.tracker_start_vert ||
-                           ao_tracker_force_launch)
-                       {
-                               ao_flight_state = ao_flight_drogue;
-                               ao_wakeup(&ao_flight_state);
-                               ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_head);
-                               ao_log_start();
-                               ao_log_gps_flight();
-                       }
-                       break;
-               case ao_flight_drogue:
-                       /* Modulate data rates based on speed (in cm/s) */
-                       if (tracker->gps_data.climb_rate < 0)
-                               speed = -tracker->gps_data.climb_rate;
-                       else
-                               speed = tracker->gps_data.climb_rate;
-                       speed += tracker->gps_data.ground_speed;
-
-                       if (speed < AO_TRACKER_NOT_MOVING) {
-                               new_telem_rate = AO_SEC_TO_TICKS(10);
-                               new_gps_rate = 10;
-                       } else {
-                               new_telem_rate = AO_SEC_TO_TICKS(1);
-                               new_gps_rate = 1;
-                       }
-                       break;
-               default:
-                       break;
-               }
-       }
-
-       if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) {
-               if (new_telem_enabled)
-                       ao_telemetry_set_interval(new_telem_rate);
-               else
-                       ao_telemetry_set_interval(0);
-               telem_rate = new_telem_rate;
-               telem_enabled = new_telem_enabled;
-       }
-
-       if (new_gps_rate != gps_rate) {
-               ao_gps_set_rate(new_gps_rate);
-               gps_rate = new_gps_rate;
-       }
-}
-
-#if HAS_LOG
-static uint8_t ao_tracker_should_log;
-
-static void
-ao_tracker_log(void)
-{
-       struct ao_tracker_data  *tracker;
-
-       if (ao_log_running) {
-               while (ao_tracker_log_pos != ao_tracker_head) {
-                       tracker = &ao_tracker_data[ao_tracker_log_pos];
-                       if (tracker->new & AO_GPS_NEW_DATA) {
-                               ao_tracker_should_log = ao_log_gps_should_log(tracker->gps_data.latitude,
-                                                                             tracker->gps_data.longitude,
-                                                                             tracker->gps_data.altitude);
-                               if (ao_tracker_should_log)
-                                       ao_log_gps_data(tracker->tick, tracker->state, &tracker->gps_data);
-                       }
-                       if (tracker->new & AO_GPS_NEW_TRACKING) {
-                               if (ao_tracker_should_log)
-                                       ao_log_gps_tracking(tracker->tick, &tracker->gps_tracking_data);
-                       }
-                       ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_log_pos);
-               }
-       }
-}
-#endif
+static int32_t last_log_latitude, last_log_longitude;
+static int16_t last_log_altitude;
+static uint8_t unmoving;
+static uint8_t log_started;
+static struct ao_telemetry_location gps_data;
+static uint8_t tracker_running;
+static uint16_t tracker_interval;
 
 static void
 ao_tracker(void)
 {
-       uint8_t         new;
-       struct ao_tracker_data  *tracker;
+       uint8_t new;
+       int32_t ground_distance;
+       int16_t height;
+       uint16_t gps_tick;
+       uint8_t new_tracker_running;
 
 #if HAS_ADC
        ao_timer_set_adc_interval(100);
@@ -197,45 +59,83 @@ ao_tracker(void)
 #if !HAS_USB_CONNECT
        ao_tracker_force_telem = 1;
 #endif
-
-       nsamples = 0;
-       lat_sum = 0;
-       lon_sum = 0;
-       alt_sum = 0;
-
        ao_log_scan();
+       ao_log_start();
 
        ao_rdf_set(1);
-       ao_telemetry_set_interval(0);
-       telem_rate = AO_SEC_TO_TICKS(1);
-       telem_enabled = 0;
-       gps_rate = 1;
 
-       ao_flight_state = ao_flight_startup;
+       tracker_interval = ao_config.tracker_interval;
+       ao_gps_set_rate(tracker_interval);
+
        for (;;) {
+
+               /** Wait for new GPS data
+                */
                while (!(new = ao_gps_new))
                        ao_sleep(&ao_gps_new);
-
-               /* Stick GPS data into the ring */
                ao_mutex_get(&ao_gps_mutex);
-               tracker = &ao_tracker_data[ao_tracker_head];
-               tracker->tick = ao_gps_tick;
-               tracker->new = new;
-               tracker->state = ao_flight_state;
-               tracker->gps_data = ao_gps_data;
-               tracker->gps_tracking_data = ao_gps_tracking_data;
-               ao_tracker_head = ao_tracker_ring_next(ao_tracker_head);
-
+               gps_data = ao_gps_data;
+               gps_tick = ao_gps_tick;
                ao_gps_new = 0;
                ao_mutex_put(&ao_gps_mutex);
 
-               /* Update state based on current GPS data */
-               ao_tracker_state_update(tracker);
+               new_tracker_running = ao_tracker_force_telem || !ao_usb_connected();
 
-#if HAS_LOG
-               /* Log all gps data */
-               ao_tracker_log();
-#endif
+               if (ao_config.tracker_interval != tracker_interval) {
+                       tracker_interval = ao_config.tracker_interval;
+                       ao_gps_set_rate(tracker_interval);
+
+                       /* force telemetry interval to be reset */
+                       tracker_running = 0;
+               }
+
+               if (new_tracker_running && !tracker_running) {
+                       ao_telemetry_set_interval(AO_SEC_TO_TICKS(tracker_interval));
+               } else if (!new_tracker_running && tracker_running) {
+                       ao_telemetry_set_interval(0);
+               }
+
+               tracker_running = new_tracker_running;
+
+               if (!tracker_running)
+                       continue;
+
+               if (new & AO_GPS_NEW_DATA) {
+                       if ((gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
+                           (AO_GPS_VALID|AO_GPS_COURSE_VALID))
+                       {
+                               if (log_started) {
+                                       ground_distance = ao_distance(gps_data.latitude, gps_data.longitude,
+                                                                     last_log_latitude, last_log_longitude);
+                                       height = last_log_altitude - gps_data.altitude;
+                                       if (height < 0)
+                                               height = -height;
+                                       if (ground_distance <= ao_config.tracker_motion &&
+                                           height <= ao_config.tracker_motion)
+                                       {
+                                               if (unmoving < AO_TRACKER_MOTION_COUNT)
+                                                       unmoving++;
+                                       } else
+                                               unmoving = 0;
+                               }
+                       } else {
+                               if (!log_started)
+                                       continue;
+                               if (unmoving < AO_TRACKER_MOTION_COUNT)
+                                       unmoving++;
+                       }
+
+                       if (unmoving < AO_TRACKER_MOTION_COUNT) {
+                               if (!log_started) {
+                                       ao_log_gps_flight();
+                                       log_started = 1;
+                               }
+                               ao_log_gps_data(gps_tick, &gps_data);
+                               last_log_latitude = gps_data.latitude;
+                               last_log_longitude = gps_data.longitude;
+                               last_log_altitude = gps_data.altitude;
+                       }
+               }
        }
 }
 
@@ -244,18 +144,23 @@ static struct ao_task ao_tracker_task;
 static void
 ao_tracker_set_telem(void)
 {
-       uint8_t telem, launch;
+       uint8_t telem;
        ao_cmd_hex();
        telem = ao_cmd_lex_i;
-       ao_cmd_hex();
-       launch = ao_cmd_lex_i;
-       if (ao_cmd_status == ao_cmd_success) {
+       if (ao_cmd_status == ao_cmd_success)
                ao_tracker_force_telem = telem;
-               ao_tracker_force_launch = launch;
-       }
        ao_cmd_status = ao_cmd_success;
-       printf ("flight %d force telem %d force launch %d\n",
-               ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch);
+       printf ("flight: %d\n", ao_flight_number);
+       printf ("force_telem: %d\n", ao_tracker_force_telem);
+       printf ("log_started: %d\n", log_started);
+       printf ("unmoving: %d\n", unmoving);
+       printf ("latitude: %ld\n", (long) gps_data.latitude);
+       printf ("longitude: %ld\n", (long) gps_data.longitude);
+       printf ("altitude: %d\n", gps_data.altitude);
+       printf ("log_running: %d\n", ao_log_running);
+       printf ("log_start_pos: %ld\n", (long) ao_log_start_pos);
+       printf ("log_cur_pos: %ld\n", (long) ao_log_current_pos);
+       printf ("log_end_pos: %ld\n", (long) ao_log_end_pos);
 }
 
 static const struct ao_cmds ao_tracker_cmds[] = {

diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h
index 63bdaf2fc6e91886917887a67d9c81c12da0f703..a0fd2f49294ab9a8e0cb18d26c9a5afbc299f02f 100644 (file)
--- a/src/kernel/ao_tracker.h
+++ b/src/kernel/ao_tracker.h
@@ -18,31 +18,12 @@
 #ifndef _AO_TRACKER_H_
 #define _AO_TRACKER_H_
 
-#define AO_CONFIG_DEFAULT_TRACKER_START_HORIZ  1000
-#define AO_CONFIG_DEFAULT_TRACKER_START_VERT   100
+/* Any motion more than this will result in a log entry */
 
-/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */
-#define AO_TRACKER_NOT_MOVING  200
+#define AO_TRACKER_MOTION_DEFAULT      10
+#define AO_TRACKER_INTERVAL_DEFAULT    1
 
-extern int32_t ao_tracker_start_latitude;
-extern int32_t ao_tracker_start_longitude;
-extern int16_t ao_tracker_start_altitude;
-
-#define AO_TRACKER_RING        4
-
-struct ao_tracker_data {
-       uint16_t                        tick;
-       uint8_t                         new;
-       uint8_t                         state;
-       struct ao_telemetry_location    gps_data;
-       struct ao_telemetry_satellite   gps_tracking_data;
-};
-
-extern struct ao_tracker_data  ao_tracker_data[AO_TRACKER_RING];
-extern uint8_t                 ao_tracker_head;
-
-#define ao_tracker_ring_next(n)        (((n) + 1) & (AO_TRACKER_RING-1))
-#define ao_tracker_ring_prev(n)        (((n) - 1) & (AO_TRACKER_RING-1))
+#define AO_TRACKER_MOTION_COUNT                10
 
 void
 ao_tracker_init(void);

diff --git a/src/telegps-v0.3/ao_telegps.c b/src/telegps-v0.3/ao_telegps.c
index bc7eeea80516b77c03c242d14eb9dbf81c29838f..dd699ecff6336a27cc0578fdd925f8780186013a 100644 (file)
--- a/src/telegps-v0.3/ao_telegps.c
+++ b/src/telegps-v0.3/ao_telegps.c
@@ -52,7 +52,6 @@ main(void)
        ao_tracker_init();
 
        ao_telemetry_init();
-       ao_telemetry_set_interval(AO_SEC_TO_TICKS(1));
 
 #if HAS_SAMPLE_PROFILE
        ao_sample_profile_init();

diff --git a/src/telegps-v1.0/ao_telegps.c b/src/telegps-v1.0/ao_telegps.c
index 1185a5ddf64607cb24cfe0f21f9fb2506167c9cc..7a71699b425c91a0b779bd7c63596018adabf58e 100644 (file)
--- a/src/telegps-v1.0/ao_telegps.c
+++ b/src/telegps-v1.0/ao_telegps.c
@@ -55,7 +55,6 @@ main(void)
        ao_tracker_init();
 
        ao_telemetry_init();
-       ao_telemetry_set_interval(AO_SEC_TO_TICKS(1));
 
 #if HAS_SAMPLE_PROFILE
        ao_sample_profile_init();