altos: Add AO_LOG_FLIGHT packet to TeleGPS logs

[fw/altos] / src / product / ao_tracker.c

diff --git a/src/product/ao_tracker.c b/src/product/ao_tracker.c
index a710d49d5ac611078f1f4bc996d43e3de30d1a66..9666528b5a2475426355ff26a4ef7321557bc757 100644 (file)
--- a/src/product/ao_tracker.c
+++ b/src/product/ao_tracker.c
@@ -22,10 +22,11 @@
 
 enum ao_flight_state   ao_flight_state;
 
-/* Speeds for the various modes */
-#define AO_TRACKER_NOT_MOVING  100
+/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */
+#define AO_TRACKER_NOT_MOVING  200
 
 static uint8_t ao_tracker_force_telem;
+static uint8_t ao_tracker_force_launch;
 
 #if HAS_USB_CONNECT
 static inline uint8_t
@@ -37,6 +38,33 @@ ao_usb_connected(void)
 #define ao_usb_connected()     1
 #endif
 
+#define STARTUP_AVERAGE        5
+
+static void
+ao_tracker_start_flight(void)
+{
+       struct ao_log_mega log;
+       ao_log_start();
+       log.type = AO_LOG_FLIGHT;
+       log.tick = ao_time();
+#if HAS_ACCEL
+       log.u.flight.ground_accel = ao_ground_accel;
+#endif
+#if HAS_GYRO
+       log.u.flight.ground_accel_along = ao_ground_accel_along;
+       log.u.flight.ground_accel_across = ao_ground_accel_across;
+       log.u.flight.ground_accel_through = ao_ground_accel_through;
+       log.u.flight.ground_roll = ao_ground_roll;
+       log.u.flight.ground_pitch = ao_ground_pitch;
+       log.u.flight.ground_yaw = ao_ground_yaw;
+#endif
+#if HAS_FLIGHT
+       log.u.flight.ground_pres = ao_ground_pres;
+#endif
+       log.u.flight.flight = ao_flight_number;
+       ao_log_mega(&log);
+}
+
 static void
 ao_tracker(void)
 {
@@ -48,8 +76,19 @@ ao_tracker(void)
        uint32_t        ground_distance;
        int16_t         height;
        uint16_t        speed;
+       int64_t         lat_sum = 0, lon_sum = 0;
+       int32_t         alt_sum = 0;
+       int             nsamples = 0;
 
+#if HAS_ADC
        ao_timer_set_adc_interval(100);
+#endif
+
+       ao_log_scan();
+
+       ao_rdf_set(1);
+
+       ao_telemetry_set_interval(0);
 
        ao_flight_state = ao_flight_startup;
        for (;;) {
@@ -69,28 +108,39 @@ ao_tracker(void)
                        switch (ao_flight_state) {
                        case ao_flight_startup:
                                /* startup to pad when GPS locks */
-                               ao_flight_state = ao_flight_pad;
-                               start_latitude = ao_gps_data.longitude;
-                               start_longitude = ao_gps_data.latitude;
-                               start_altitude = ao_gps_data.altitude;
+
+                               lat_sum += ao_gps_data.latitude;
+                               lon_sum += ao_gps_data.longitude;
+                               alt_sum += ao_gps_data.altitude;
+
+                               if (++nsamples >= STARTUP_AVERAGE) {
+                                       ao_flight_state = ao_flight_pad;
+                                       ao_wakeup(&ao_flight_state);
+                                       start_latitude = lat_sum / nsamples;
+                                       start_longitude = lon_sum / nsamples;
+                                       start_altitude = alt_sum / nsamples;
+                               }
                                break;
                        case ao_flight_pad:
                                ground_distance = ao_distance(ao_gps_data.latitude,
-                                                             start_latitude,
                                                              ao_gps_data.longitude,
+                                                             start_latitude,
                                                              start_longitude);
                                height = ao_gps_data.altitude - start_altitude;
                                if (height < 0)
                                        height = -height;
+
                                if (ground_distance >= ao_config.tracker_start_horiz ||
-                                   height >= ao_config.tracker_start_vert)
+                                   height >= ao_config.tracker_start_vert ||
+                                   ao_tracker_force_launch)
                                {
                                        ao_flight_state = ao_flight_drogue;
+                                       ao_wakeup(&ao_flight_state);
                                        ao_log_start();
+                                       ao_tracker_start_flight();
                                }
                                break;
                        case ao_flight_drogue:
-
                                /* Modulate data rates based on speed (in cm/s) */
                                if (ao_gps_data.climb_rate < 0)
                                        speed = -ao_gps_data.climb_rate;
@@ -134,8 +184,11 @@ static void
 ao_tracker_set_telem(void)
 {
        ao_cmd_hex();
-       if (ao_cmd_status == ao_cmd_success)
-               ao_tracker_force_telem = ao_cmd_lex_i;
+       if (ao_cmd_status == ao_cmd_success) {
+               ao_tracker_force_telem = (ao_cmd_lex_i & 1) != 0;
+               ao_tracker_force_launch = (ao_cmd_lex_i & 2) != 0;
+               printf ("force telem %d force launch %d\n", ao_tracker_force_telem, ao_tracker_force_launch);
+       }
 }
 
 static const struct ao_cmds ao_tracker_cmds[] = {