altos: Complete new telemetry switchover
authorKeith Packard <keithp@keithp.com>
Tue, 5 Jul 2011 06:39:21 +0000 (23:39 -0700)
committerKeith Packard <keithp@keithp.com>
Tue, 5 Jul 2011 06:39:21 +0000 (23:39 -0700)
This involved rewriting the GPS code to use the telemetry structures
directly so that a memcpy could be used to transfer the data to the
telemetry packets, saving a bunch of code space, along with fixing up
the gps testing programs to deal with the structure changes.

In addition, the teledongle code needed to have the monitoring code
split into separate radio receiver and USB writer threads as the
packets are now back-to-back, and hence come too fast to wait for the
USB data to be sent to the host after each one.

Signed-off-by: Keith Packard <keithp@keithp.com>
src/ao.h
src/ao_gps_print.c
src/ao_gps_report.c
src/ao_gps_sirf.c
src/ao_gps_skytraq.c
src/ao_gps_test.c
src/ao_gps_test_skytraq.c
src/ao_monitor.c
src/ao_telemetry.c

index 4d31f4f..08f4527 100644 (file)
--- a/src/ao.h
+++ b/src/ao.h
@@ -965,80 +965,8 @@ void
 ao_spi_init(void);
 
 /*
- * ao_gps.c
+ * ao_telemetry.c
  */
-
-#define AO_GPS_NUM_SAT_MASK    (0xf << 0)
-#define AO_GPS_NUM_SAT_SHIFT   (0)
-
-#define AO_GPS_VALID           (1 << 4)
-#define AO_GPS_RUNNING         (1 << 5)
-#define AO_GPS_DATE_VALID      (1 << 6)
-#define AO_GPS_COURSE_VALID    (1 << 7)
-
-extern __xdata uint16_t ao_gps_tick;
-
-struct ao_gps_data {
-       uint8_t                 year;
-       uint8_t                 month;
-       uint8_t                 day;
-       uint8_t                 hour;
-       uint8_t                 minute;
-       uint8_t                 second;
-       uint8_t                 flags;
-       int32_t                 latitude;       /* degrees * 10⁷ */
-       int32_t                 longitude;      /* degrees * 10⁷ */
-       int16_t                 altitude;       /* m */
-       uint16_t                ground_speed;   /* cm/s */
-       uint8_t                 course;         /* degrees / 2 */
-       uint8_t                 hdop;           /* * 5 */
-       int16_t                 climb_rate;     /* cm/s */
-       uint16_t                h_error;        /* m */
-       uint16_t                v_error;        /* m */
-};
-
-struct ao_gps_sat_data {
-       uint8_t         svid;
-       uint8_t         c_n_1;
-};
-
-#define AO_MAX_GPS_TRACKING    12
-
-struct ao_gps_tracking_data {
-       uint8_t                 channels;
-       struct ao_gps_sat_data  sats[AO_MAX_GPS_TRACKING];
-};
-
-extern __xdata uint8_t ao_gps_mutex;
-extern __xdata struct ao_gps_data ao_gps_data;
-extern __xdata struct ao_gps_tracking_data ao_gps_tracking_data;
-
-void
-ao_gps(void);
-
-void
-ao_gps_print(__xdata struct ao_gps_data *gps_data);
-
-void
-ao_gps_tracking_print(__xdata struct ao_gps_tracking_data *gps_tracking_data);
-
-void
-ao_gps_init(void);
-
-/*
- * ao_gps_report.c
- */
-
-void
-ao_gps_report(void);
-
-void
-ao_gps_report_init(void);
-
-/*
- * ao_telemetry_orig.c
- */
-
 #define AO_MAX_CALLSIGN                        8
 #define AO_MAX_VERSION                 8
 #define AO_MAX_TELEMETRY               128
@@ -1126,12 +1054,13 @@ struct ao_telemetry_location {
        uint8_t         vdop;           /* 24 (m * 5) */
        uint8_t         mode;           /* 25 */
        uint16_t        ground_speed;   /* 26 cm/s */
-       uint8_t         course;         /* 28 degrees / 2 */
-       uint8_t         unused[3];      /* 29 */
+       int16_t         climb_rate;     /* 28 cm/s */
+       uint8_t         course;         /* 30 degrees / 2 */
+       uint8_t         unused[1];      /* 31 */
        /* 32 */
 };
 
-#define AO_TELEMETRY_SATELLITE         0x12
+#define AO_TELEMETRY_SATELLITE         0x06
 
 struct ao_telemetry_satellite_info {
        uint8_t         svid;
@@ -1157,10 +1086,79 @@ union ao_telemetry_all {
        struct ao_telemetry_satellite           satellite;
 };
 
-#define AO_SAT_0_SSID(s)       ((s)[0] & 0x3f)
-#define AO_SAT_0_C_N_1(s)      ((((s)[0] & 0xc0) >> 2) | ((s)[1] & 0x0f))
-#define AO_SAT_1_SSID(s)       ((((s)[1] & 0xf0) >> 2) | ((s)[2] & 0x03))
-#define AO_SAT_1_C_N_1(s)      (((s)[2] & 0xfc) >> 2)
+/*
+ * ao_gps.c
+ */
+
+#define AO_GPS_NUM_SAT_MASK    (0xf << 0)
+#define AO_GPS_NUM_SAT_SHIFT   (0)
+
+#define AO_GPS_VALID           (1 << 4)
+#define AO_GPS_RUNNING         (1 << 5)
+#define AO_GPS_DATE_VALID      (1 << 6)
+#define AO_GPS_COURSE_VALID    (1 << 7)
+
+extern __xdata uint16_t ao_gps_tick;
+extern __xdata uint8_t ao_gps_mutex;
+extern __xdata struct ao_telemetry_location ao_gps_data;
+extern __xdata struct ao_telemetry_satellite ao_gps_tracking_data;
+
+struct ao_gps_orig {
+       uint8_t                 year;
+       uint8_t                 month;
+       uint8_t                 day;
+       uint8_t                 hour;
+       uint8_t                 minute;
+       uint8_t                 second;
+       uint8_t                 flags;
+       int32_t                 latitude;       /* degrees * 10⁷ */
+       int32_t                 longitude;      /* degrees * 10⁷ */
+       int16_t                 altitude;       /* m */
+       uint16_t                ground_speed;   /* cm/s */
+       uint8_t                 course;         /* degrees / 2 */
+       uint8_t                 hdop;           /* * 5 */
+       int16_t                 climb_rate;     /* cm/s */
+       uint16_t                h_error;        /* m */
+       uint16_t                v_error;        /* m */
+};
+
+struct ao_gps_sat_orig {
+       uint8_t         svid;
+       uint8_t         c_n_1;
+};
+
+#define AO_MAX_GPS_TRACKING    12
+
+struct ao_gps_tracking_orig {
+       uint8_t                 channels;
+       struct ao_gps_sat_orig  sats[AO_MAX_GPS_TRACKING];
+};
+
+void
+ao_gps(void);
+
+void
+ao_gps_print(__xdata struct ao_gps_orig *gps_data);
+
+void
+ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data);
+
+void
+ao_gps_init(void);
+
+/*
+ * ao_gps_report.c
+ */
+
+void
+ao_gps_report(void);
+
+void
+ao_gps_report_init(void);
+
+/*
+ * ao_telemetry_orig.c
+ */
 
 struct ao_telemetry_orig {
        uint16_t                serial;
@@ -1180,9 +1178,9 @@ struct ao_telemetry_orig {
        int16_t                 accel_plus_g;
        int16_t                 accel_minus_g;
        struct ao_adc           adc;
-       struct ao_gps_data      gps;
+       struct ao_gps_orig      gps;
        char                    callsign[AO_MAX_CALLSIGN];
-       struct ao_gps_tracking_data     gps_tracking;
+       struct ao_gps_tracking_orig     gps_tracking;
 };
 
 struct ao_telemetry_tiny {
index ca071b4..fcdedd3 100644 (file)
@@ -21,7 +21,7 @@
 #include "ao_telem.h"
 
 void
-ao_gps_print(__xdata struct ao_gps_data *gps_data) __reentrant
+ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant
 {
        char    state;
 
@@ -77,10 +77,10 @@ ao_gps_print(__xdata struct ao_gps_data *gps_data) __reentrant
 }
 
 void
-ao_gps_tracking_print(__xdata struct ao_gps_tracking_data *gps_tracking_data) __reentrant
+ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data) __reentrant
 {
        uint8_t c, n, v;
-       __xdata struct ao_gps_sat_data  *sat;
+       __xdata struct ao_gps_sat_orig  *sat;
 
        n = gps_tracking_data->channels;
        if (n == 0)
index 7abc93f..e57f874 100644 (file)
 void
 ao_gps_report(void)
 {
-       static __xdata struct ao_log_record     gps_log;
-       static __xdata struct ao_gps_data       gps_data;
+       static __xdata struct ao_log_record             gps_log;
+       static __xdata struct ao_telemetry_location     gps_data;
        uint8_t date_reported = 0;
 
        for (;;) {
                ao_sleep(&ao_gps_data);
                ao_mutex_get(&ao_gps_mutex);
-               memcpy(&gps_data, &ao_gps_data, sizeof (struct ao_gps_data));
+               memcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data));
                ao_mutex_put(&ao_gps_mutex);
 
                if (!(gps_data.flags & AO_GPS_VALID))
@@ -64,15 +64,15 @@ ao_gps_report(void)
 void
 ao_gps_tracking_report(void)
 {
-       static __xdata struct ao_log_record     gps_log;
-       static __xdata struct ao_gps_tracking_data      gps_tracking_data;
+       static __xdata struct ao_log_record             gps_log;
+       static __xdata struct ao_telemetry_satellite    gps_tracking_data;
        uint8_t c, n;
 
        for (;;) {
                ao_sleep(&ao_gps_tracking_data);
                ao_mutex_get(&ao_gps_mutex);
                gps_log.tick = ao_gps_tick;
-               memcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (struct ao_gps_tracking_data));
+               memcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data));
                ao_mutex_put(&ao_gps_mutex);
 
                if (!(n = gps_tracking_data.channels))
index 87b1d69..5827c68 100644 (file)
@@ -21,8 +21,8 @@
 
 __xdata uint8_t ao_gps_mutex;
 __xdata uint16_t ao_gps_tick;
-__xdata struct ao_gps_data     ao_gps_data;
-__xdata struct ao_gps_tracking_data    ao_gps_tracking_data;
+__xdata struct ao_telemetry_location   ao_gps_data;
+__xdata struct ao_telemetry_satellite  ao_gps_tracking_data;
 
 static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n";
 
@@ -406,6 +406,7 @@ ao_gps(void) __reentrant
                        ao_gps_data.hdop = ao_sirf_data.hdop;
                        ao_gps_data.climb_rate = ao_sirf_data.climb_rate;
                        ao_gps_data.flags |= AO_GPS_COURSE_VALID;
+#if 0
                        if (ao_sirf_data.h_error > 6553500)
                                ao_gps_data.h_error = 65535;
                        else
@@ -414,6 +415,7 @@ ao_gps(void) __reentrant
                                ao_gps_data.v_error = 65535;
                        else
                                ao_gps_data.v_error = ao_sirf_data.v_error / 100;
+#endif
                        ao_mutex_put(&ao_gps_mutex);
                        ao_wakeup(&ao_gps_data);
                        break;
index 6099ca9..84743ff 100644 (file)
@@ -29,13 +29,13 @@ static __xdata uint8_t ao_gps_cksum;
 static __xdata uint8_t ao_gps_error;
 
 __xdata uint16_t ao_gps_tick;
-__xdata struct ao_gps_data     ao_gps_data;
-__xdata struct ao_gps_tracking_data    ao_gps_tracking_data;
+__xdata struct ao_telemetry_location   ao_gps_data;
+__xdata struct ao_telemetry_satellite  ao_gps_tracking_data;
 
 static __xdata uint16_t                                ao_gps_next_tick;
-static __xdata struct ao_gps_data              ao_gps_next;
+static __xdata struct ao_telemetry_location    ao_gps_next;
 static __xdata uint8_t                         ao_gps_date_flags;
-static __xdata struct ao_gps_tracking_data     ao_gps_tracking_next;
+static __xdata struct ao_telemetry_satellite   ao_gps_tracking_next;
 
 #define STQ_S 0xa0, 0xa1
 #define STQ_E 0x0d, 0x0a
@@ -265,7 +265,7 @@ ao_nmea_gga()
        if (!ao_gps_error) {
                ao_mutex_get(&ao_gps_mutex);
                ao_gps_tick = ao_gps_next_tick;
-               memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data));
+               memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data));
                ao_mutex_put(&ao_gps_mutex);
                ao_wakeup(&ao_gps_data);
        }
index 44efb50..93d7a9a 100644 (file)
@@ -30,7 +30,7 @@
 #define AO_GPS_DATE_VALID      (1 << 6)
 #define AO_GPS_COURSE_VALID    (1 << 7)
 
-struct ao_gps_data {
+struct ao_gps_orig {
        uint8_t                 year;
        uint8_t                 month;
        uint8_t                 day;
@@ -58,18 +58,22 @@ struct ao_gps_data {
 #define SIRF_SAT_ACQUISITION_FAILED            (1 << 6)
 #define SIRF_SAT_EPHEMERIS_AVAILABLE           (1 << 7)
 
-struct ao_gps_sat_data {
+struct ao_gps_sat_orig {
        uint8_t         svid;
        uint8_t         c_n_1;
 };
 
 #define AO_MAX_GPS_TRACKING    12
 
-struct ao_gps_tracking_data {
+struct ao_gps_tracking_orig {
        uint8_t                 channels;
-       struct ao_gps_sat_data  sats[AO_MAX_GPS_TRACKING];
+       struct ao_gps_sat_orig  sats[AO_MAX_GPS_TRACKING];
 };
 
+#define ao_telemetry_location ao_gps_orig
+#define ao_telemetry_satellite ao_gps_tracking_orig
+#define ao_telemetry_satellite_info ao_gps_sat_orig
+
 void
 ao_mutex_get(uint8_t *mutex)
 {
index b94e9bd..a78fae0 100644 (file)
@@ -30,7 +30,7 @@
 #define AO_GPS_DATE_VALID      (1 << 6)
 #define AO_GPS_COURSE_VALID    (1 << 7)
 
-struct ao_gps_data {
+struct ao_gps_orig {
        uint8_t                 year;
        uint8_t                 month;
        uint8_t                 day;
@@ -58,19 +58,22 @@ struct ao_gps_data {
 #define SIRF_SAT_ACQUISITION_FAILED            (1 << 6)
 #define SIRF_SAT_EPHEMERIS_AVAILABLE           (1 << 7)
 
-struct ao_gps_sat_data {
+struct ao_gps_sat_orig {
        uint8_t         svid;
-       uint8_t         state;
        uint8_t         c_n_1;
 };
 
 #define AO_MAX_GPS_TRACKING    12
 
-struct ao_gps_tracking_data {
+struct ao_gps_tracking_orig {
        uint8_t                 channels;
-       struct ao_gps_sat_data  sats[AO_MAX_GPS_TRACKING];
+       struct ao_gps_sat_orig  sats[AO_MAX_GPS_TRACKING];
 };
 
+#define ao_telemetry_location ao_gps_orig
+#define ao_telemetry_satellite ao_gps_tracking_orig
+#define ao_telemetry_satellite_info ao_gps_sat_orig
+
 void
 ao_mutex_get(uint8_t *mutex)
 {
index e5e9159..8f1b9e1 100644 (file)
 __xdata uint8_t ao_monitoring;
 __pdata uint8_t ao_monitor_led;
 
-void
-ao_monitor(void)
-{
-       __xdata char callsign[AO_MAX_CALLSIGN+1];
-       __xdata union {
+#define AO_MONITOR_RING        8
+
+__xdata union ao_monitor {
                struct ao_telemetry_raw_recv    raw;
                struct ao_telemetry_orig_recv   orig;
                struct ao_telemetry_tiny_recv   tiny;
-       } u;
+} ao_monitor_ring[AO_MONITOR_RING];
+
+#define ao_monitor_ring_next(n)        (((n) + 1) & (AO_MONITOR_RING - 1))
 
-#define recv_raw       (u.raw)
-#define recv_orig      (u.orig)
-#define recv_tiny      (u.tiny)
+__data uint8_t ao_monitor_head;
 
+void
+ao_monitor_get(void)
+{
+       uint8_t size;
+
+       for (;;) {
+               switch (ao_monitoring) {
+               case 0:
+                       ao_sleep(&ao_monitoring);
+                       continue;
+               case AO_MONITORING_ORIG:
+                       size = sizeof (struct ao_telemetry_orig_recv);
+                       break;
+               case AO_MONITORING_TINY:
+                       size = sizeof (struct ao_telemetry_tiny_recv);
+                       break;
+               default:
+                       if (ao_monitoring > AO_MAX_TELEMETRY)
+                               ao_monitoring = AO_MAX_TELEMETRY;
+                       size = ao_monitoring;
+                       break;
+               }
+               if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2))
+                       continue;
+               ao_monitor_head = ao_monitor_ring_next(ao_monitor_head);
+               ao_wakeup(DATA_TO_XDATA(&ao_monitor_head));
+               ao_led_toggle(ao_monitor_led);
+       }
+}
+
+void
+ao_monitor_put(void)
+{
+       __xdata char callsign[AO_MAX_CALLSIGN+1];
+
+       uint8_t ao_monitor_tail;
        uint8_t state;
        uint8_t sum, byte;
        int16_t rssi;
+       __xdata union ao_monitor        *m;
+
+#define recv_raw       ((m->raw))
+#define recv_orig      ((m->orig))
+#define recv_tiny      ((m->tiny))
 
+       ao_monitor_tail = ao_monitor_head;
        for (;;) {
-               __critical while (!ao_monitoring)
-                       ao_sleep(&ao_monitoring);
+               while (ao_monitor_tail == ao_monitor_head)
+                       ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
+               m = &ao_monitor_ring[ao_monitor_tail];
+               ao_monitor_tail = ao_monitor_ring_next(ao_monitor_tail);
                switch (ao_monitoring) {
                case AO_MONITORING_ORIG:
-                       if (!ao_radio_recv(&recv_orig, sizeof (struct ao_telemetry_orig_recv)))
-                               continue;
-
                        state = recv_orig.telemetry_orig.flight_state;
 
                        /* Typical RSSI offset for 38.4kBaud at 433 MHz is 74 */
@@ -122,9 +161,6 @@ ao_monitor(void)
                        }
                        break;
                case AO_MONITORING_TINY:
-                       if (!ao_radio_recv(&recv_tiny, sizeof (struct ao_telemetry_tiny_recv)))
-                               continue;
-
                        state = recv_tiny.telemetry_tiny.flight_state;
 
                        /* Typical RSSI offset for 38.4kBaud at 433 MHz is 74 */
@@ -188,10 +224,6 @@ ao_monitor(void)
                        }
                        break;
                default:
-                       if (ao_monitoring > AO_MAX_TELEMETRY)
-                               ao_monitoring = AO_MAX_TELEMETRY;
-                       if (!ao_radio_recv(&recv_raw, ao_monitoring + 2))
-                               continue;
                        printf ("TELEM %02x", ao_monitoring + 2);
                        sum = 0x5a;
                        for (state = 0; state < ao_monitoring + 2; state++) {
@@ -203,11 +235,11 @@ ao_monitor(void)
                        break;
                }
                ao_usb_flush();
-               ao_led_toggle(ao_monitor_led);
        }
 }
 
-__xdata struct ao_task ao_monitor_task;
+__xdata struct ao_task ao_monitor_get_task;
+__xdata struct ao_task ao_monitor_put_task;
 
 void
 ao_set_monitor(uint8_t monitoring)
@@ -236,5 +268,6 @@ ao_monitor_init(uint8_t monitor_led, uint8_t monitoring) __reentrant
        ao_monitor_led = monitor_led;
        ao_monitoring = monitoring;
        ao_cmd_register(&ao_monitor_cmds[0]);
-       ao_add_task(&ao_monitor_task, ao_monitor, "monitor");
+       ao_add_task(&ao_monitor_get_task, ao_monitor_get, "monitor_get");
+       ao_add_task(&ao_monitor_put_task, ao_monitor_put, "monitor_put");
 }
index 024ac6f..94ea0b2 100644 (file)
 #include "ao.h"
 #include "ao_product.h"
 
-__xdata uint16_t ao_telemetry_interval = 0;
+__xdata uint16_t ao_telemetry_interval;
+__xdata int8_t ao_telemetry_config_max;
+__xdata int8_t ao_telemetry_config_cur;
+#if HAS_GPS
+__xdata int8_t ao_telemetry_loc_cur;
+__xdata int8_t ao_telemetry_sat_cur;
+#endif
 __xdata uint8_t ao_rdf = 0;
 __xdata uint16_t ao_rdf_time;
 
@@ -37,13 +43,110 @@ __xdata uint16_t ao_rdf_time;
 #define AO_TELEMETRY_SENSOR    AO_TELEMETRY_SENSOR_TELENANO
 #endif
 
+static __xdata union ao_telemetry_all  telemetry;
+
+/* Send sensor packet */
+static void
+ao_send_sensor(void)
+{
+       uint8_t         sample;
+       sample = ao_sample_adc;
+                       
+       telemetry.generic.tick = ao_adc_ring[sample].tick;
+       telemetry.generic.type = AO_TELEMETRY_SENSOR;
+
+       telemetry.sensor.state = ao_flight_state;
+#if HAS_ACCEL
+       telemetry.sensor.accel = ao_adc_ring[sample].accel;
+#else
+       telemetry.sensor.accel = 0;
+#endif
+       telemetry.sensor.pres = ao_adc_ring[sample].pres;
+       telemetry.sensor.temp = ao_adc_ring[sample].temp;
+       telemetry.sensor.v_batt = ao_adc_ring[sample].v_batt;
+#if HAS_IGNITE
+       telemetry.sensor.sense_d = ao_adc_ring[sample].sense_d;
+       telemetry.sensor.sense_m = ao_adc_ring[sample].sense_m;
+#else
+       telemetry.sensor.sense_d = 0;
+       telemetry.sensor.sense_m = 0;
+#endif
+
+       telemetry.sensor.acceleration = ao_accel;
+       telemetry.sensor.speed = ao_speed;
+       telemetry.sensor.height = ao_height;
+
+       telemetry.sensor.ground_pres = ao_ground_pres;
+       telemetry.sensor.ground_accel = ao_ground_accel;
+       telemetry.sensor.accel_plus_g = ao_config.accel_plus_g;
+       telemetry.sensor.accel_minus_g = ao_config.accel_minus_g;
+
+       ao_radio_send(&telemetry, sizeof (telemetry));
+}
+
+static void
+ao_send_configuration(void)
+{
+       if (--ao_telemetry_config_cur <= 0)
+       {
+               telemetry.generic.type = AO_TELEMETRY_CONFIGURATION;
+               telemetry.configuration.device = AO_idProduct_NUMBER;
+               telemetry.configuration.flight = ao_flight_number;
+               telemetry.configuration.config_major = AO_CONFIG_MAJOR;
+               telemetry.configuration.config_minor = AO_CONFIG_MINOR;
+               telemetry.configuration.main_deploy = ao_config.main_deploy;
+               telemetry.configuration.flight_log_max = ao_config.flight_log_max;
+               memcpy (telemetry.configuration.callsign,
+                       ao_config.callsign,
+                       AO_MAX_CALLSIGN);
+               memcpy (telemetry.configuration.version,
+                       ao_version,
+                       AO_MAX_VERSION);
+               ao_radio_send(&telemetry, sizeof (telemetry));
+               ao_telemetry_config_cur = ao_telemetry_config_max;
+       }
+}
+
+#if HAS_GPS
+static void
+ao_send_location(void)
+{
+       if (--ao_telemetry_loc_cur <= 0)
+       {
+               telemetry.generic.type = AO_TELEMETRY_LOCATION;
+               ao_mutex_get(&ao_gps_mutex);
+               memcpy(&telemetry.location.flags,
+                      &ao_gps_data.flags,
+                      26);
+               ao_mutex_put(&ao_gps_mutex);
+               ao_radio_send(&telemetry, sizeof (telemetry));
+               ao_telemetry_loc_cur = ao_telemetry_config_max;
+       }
+}
+
+static void
+ao_send_satellite(void)
+{
+       if (--ao_telemetry_sat_cur <= 0)
+       {
+               telemetry.generic.type = AO_TELEMETRY_SATELLITE;
+               ao_mutex_get(&ao_gps_mutex);
+               telemetry.satellite.channels = ao_gps_tracking_data.channels;
+               memcpy(&telemetry.satellite.sats,
+                      &ao_gps_tracking_data.sats,
+                      AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info));
+               ao_mutex_put(&ao_gps_mutex);
+               ao_radio_send(&telemetry, sizeof (telemetry));
+               ao_telemetry_sat_cur = ao_telemetry_config_max;
+       }
+}
+#endif
+
 void
 ao_telemetry(void)
 {
        uint16_t        time;
        int16_t         delay;
-       static __xdata union ao_telemetry_all   telemetry;
-       uint8_t         sample;
 
        ao_config_get();
        while (!ao_flight_number)
@@ -56,54 +159,13 @@ ao_telemetry(void)
                time = ao_rdf_time = ao_time();
                while (ao_telemetry_interval) {
 
-                       /* Send sensor packet */
-                       sample = ao_sample_adc;
-                       
-                       telemetry.generic.tick = ao_adc_ring[sample].tick;
-                       telemetry.generic.type = AO_TELEMETRY_SENSOR;
 
-                       telemetry.sensor.state = ao_flight_state;
-#if HAS_ACCEL
-                       telemetry.sensor.accel = ao_adc_ring[sample].accel;
-#else
-                       telemetry.sensor.accel = 0;
-#endif
-                       telemetry.sensor.pres = ao_adc_ring[sample].pres;
-                       telemetry.sensor.temp = ao_adc_ring[sample].temp;
-                       telemetry.sensor.v_batt = ao_adc_ring[sample].v_batt;
-#if HAS_IGNITE
-                       telemetry.sensor.sense_d = ao_adc_ring[sample].sense_d;
-                       telemetry.sensor.sense_m = ao_adc_ring[sample].sense_m;
-#else
-                       telemetry.sensor.sense_d = 0;
-                       telemetry.sensor.sense_m = 0;
+                       ao_send_sensor();
+                       ao_send_configuration();
+#if HAS_GPS
+                       ao_send_location();
+                       ao_send_satellite();
 #endif
-
-                       telemetry.sensor.acceleration = ao_accel;
-                       telemetry.sensor.speed = ao_speed;
-                       telemetry.sensor.height = ao_height;
-
-                       telemetry.sensor.ground_pres = ao_ground_pres;
-                       telemetry.sensor.ground_accel = ao_ground_accel;
-                       telemetry.sensor.accel_plus_g = ao_config.accel_plus_g;
-                       telemetry.sensor.accel_minus_g = ao_config.accel_minus_g;
-
-                       ao_radio_send(&telemetry, sizeof (telemetry));
-
-                       telemetry.generic.type = AO_TELEMETRY_CONFIGURATION;
-                       telemetry.configuration.device = AO_idProduct_NUMBER;
-                       telemetry.configuration.flight = ao_flight_number;
-                       telemetry.configuration.config_major = AO_CONFIG_MAJOR;
-                       telemetry.configuration.config_minor = AO_CONFIG_MINOR;
-                       telemetry.configuration.main_deploy = ao_config.main_deploy;
-                       telemetry.configuration.flight_log_max = ao_config.flight_log_max;
-                       memcpy (telemetry.configuration.callsign,
-                               ao_config.callsign,
-                               AO_MAX_CALLSIGN);
-                       memcpy (telemetry.configuration.version,
-                               ao_version,
-                               AO_MAX_VERSION);
-
                        if (ao_rdf &&
                            (int16_t) (ao_time() - ao_rdf_time) >= 0)
                        {
@@ -124,6 +186,16 @@ void
 ao_telemetry_set_interval(uint16_t interval)
 {
        ao_telemetry_interval = interval;
+       ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval;
+       ao_telemetry_config_cur = 0;
+#if HAS_GPS
+       ao_telemetry_loc_cur = 0;
+       if (ao_telemetry_config_max - 1 > ao_telemetry_loc_cur)
+               ao_telemetry_loc_cur++;
+       ao_telemetry_sat_cur = ao_telemetry_loc_cur;
+       if (ao_telemetry_config_max - 1 > ao_telemetry_sat_cur)
+               ao_telemetry_sat_cur++;
+#endif
        ao_wakeup(&ao_telemetry_interval);
 }