altos: Complete new telemetry switchover

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>

diff --git a/src/ao.h b/src/ao.h
index 4d31f4f52dd30e65746708af477db5035a51d558..08f45275fd4e51e34d3b92a2f738ea786d587782 100644 (file)
--- a/src/ao.h
+++ b/src/ao.h
@@ -965,80 +965,8 @@ void
 ao_spi_init(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
 #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         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 */
 };
 
        /* 32 */
 };
 

-#define AO_TELEMETRY_SATELLITE         0x12
+#define AO_TELEMETRY_SATELLITE         0x06

 
 struct ao_telemetry_satellite_info {
        uint8_t         svid;
 
 struct ao_telemetry_satellite_info {
        uint8_t         svid;


@@ -1157,10 +1086,79 @@ union ao_telemetry_all {
        struct ao_telemetry_satellite           satellite;
 };
 
        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;
 
 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;
        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];
        char                    callsign[AO_MAX_CALLSIGN];

-       struct ao_gps_tracking_data     gps_tracking;
+       struct ao_gps_tracking_orig     gps_tracking;

 };
 
 struct ao_telemetry_tiny {
 };
 
 struct ao_telemetry_tiny {

diff --git a/src/ao_gps_print.c b/src/ao_gps_print.c
index ca071b42be360f7b21157854eac6c35af8fd76d5..fcdedd30a762ce9ee3d22a7c1eb46f577af99e8c 100644 (file)
--- a/src/ao_gps_print.c
+++ b/src/ao_gps_print.c
@@ -21,7 +21,7 @@
 #include "ao_telem.h"
 
 void
 #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;
 
 {
        char    state;
 


@@ -77,10 +77,10 @@ ao_gps_print(__xdata struct ao_gps_data *gps_data) __reentrant
 }
 
 void
 }
 
 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;
 {
        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)
 
        n = gps_tracking_data->channels;
        if (n == 0)

diff --git a/src/ao_gps_report.c b/src/ao_gps_report.c
index 7abc93f5d8e868cda3c5542a75b06a9e73846f4e..e57f87443c2bb8debb6f3ca0d346910283767ee7 100644 (file)
--- a/src/ao_gps_report.c
+++ b/src/ao_gps_report.c
@@ -20,14 +20,14 @@
 void
 ao_gps_report(void)
 {
 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);
        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))
                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)
 {
 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;
        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))
                ao_mutex_put(&ao_gps_mutex);
 
                if (!(n = gps_tracking_data.channels))

diff --git a/src/ao_gps_sirf.c b/src/ao_gps_sirf.c
index 87b1d69ce2f5ae91c9db2fa217c83239c1ab4883..5827c687c7541f80eb6659b782d1fa200841cd52 100644 (file)
--- a/src/ao_gps_sirf.c
+++ b/src/ao_gps_sirf.c
@@ -21,8 +21,8 @@
 
 __xdata uint8_t ao_gps_mutex;
 __xdata uint16_t ao_gps_tick;
 
 __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";
 
 
 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;
                        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
                        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;
                                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;
                        ao_mutex_put(&ao_gps_mutex);
                        ao_wakeup(&ao_gps_data);
                        break;

diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c
index 6099ca96896bdf742e72982400f885d99d92148d..84743ff53f92da48742c1e5571ed7e483638bca2 100644 (file)
--- a/src/ao_gps_skytraq.c
+++ b/src/ao_gps_skytraq.c
@@ -29,13 +29,13 @@ static __xdata uint8_t ao_gps_cksum;
 static __xdata uint8_t ao_gps_error;
 
 __xdata uint16_t ao_gps_tick;
 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 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 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
 
 #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;
        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);
        }
                ao_mutex_put(&ao_gps_mutex);
                ao_wakeup(&ao_gps_data);
        }

diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c
index 44efb50cf97be50c53a99945de74c674a591dd50..93d7a9abc6ee5e3371cf72f33fda50d19dd9ad50 100644 (file)
--- a/src/ao_gps_test.c
+++ b/src/ao_gps_test.c
@@ -30,7 +30,7 @@
 #define AO_GPS_DATE_VALID      (1 << 6)
 #define AO_GPS_COURSE_VALID    (1 << 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;
        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)
 
 #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
 
        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;
        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)
 {
 void
 ao_mutex_get(uint8_t *mutex)
 {

diff --git a/src/ao_gps_test_skytraq.c b/src/ao_gps_test_skytraq.c
index b94e9bd28a9806857f0b1b0a7b51d00bfeeb1e93..a78fae0fefcda4f49ecf6a3b04107976fc57baf0 100644 (file)
--- a/src/ao_gps_test_skytraq.c
+++ b/src/ao_gps_test_skytraq.c
@@ -30,7 +30,7 @@
 #define AO_GPS_DATE_VALID      (1 << 6)
 #define AO_GPS_COURSE_VALID    (1 << 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;
        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)
 
 #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         svid;

-       uint8_t         state;

        uint8_t         c_n_1;
 };
 
 #define AO_MAX_GPS_TRACKING    12
 
        uint8_t         c_n_1;
 };
 
 #define AO_MAX_GPS_TRACKING    12
 

-struct ao_gps_tracking_data {
+struct ao_gps_tracking_orig {

        uint8_t                 channels;
        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)
 {
 void
 ao_mutex_get(uint8_t *mutex)
 {

diff --git a/src/ao_monitor.c b/src/ao_monitor.c
index e5e9159f0bcc4cce71c75d4e210e8d330e52dd93..8f1b9e12cdcc209dae9543c0a41885a68ef3bf4b 100644 (file)
--- a/src/ao_monitor.c
+++ b/src/ao_monitor.c
@@ -21,32 +21,71 @@
 __xdata uint8_t ao_monitoring;
 __pdata uint8_t ao_monitor_led;
 
 __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;
                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;
        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 (;;) {
        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:
                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 */
                        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:
                        }
                        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 */
                        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:
                        }
                        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++) {
                        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();
                        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)
 
 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_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");

 }
 }

diff --git a/src/ao_telemetry.c b/src/ao_telemetry.c
index 024ac6f8a4021a68f56de65c38f28366cd9751c3..94ea0b2235bcffbc8acf79835d60d5112a2c3e63 100644 (file)
--- a/src/ao_telemetry.c
+++ b/src/ao_telemetry.c
@@ -18,7 +18,13 @@
 #include "ao.h"
 #include "ao_product.h"
 
 #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;
 
 __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
 
 #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;
 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)
 
        ao_config_get();
        while (!ao_flight_number)


@@ -56,54 +159,13 @@ ao_telemetry(void)
                time = ao_rdf_time = ao_time();
                while (ao_telemetry_interval) {
 
                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
 #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)
                        {
                        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_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);
 }
 
        ao_wakeup(&ao_telemetry_interval);
 }