From ef3ce687d73c1274ce5368432f4d449b063ce5c0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 4 Jul 2011 23:39:21 -0700 Subject: [PATCH] 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 --- src/ao.h | 162 ++++++++++++++++++------------------ src/ao_gps_print.c | 6 +- src/ao_gps_report.c | 12 +-- src/ao_gps_sirf.c | 6 +- src/ao_gps_skytraq.c | 10 +-- src/ao_gps_test.c | 12 ++- src/ao_gps_test_skytraq.c | 13 +-- src/ao_monitor.c | 81 ++++++++++++------ src/ao_telemetry.c | 170 +++++++++++++++++++++++++++----------- 9 files changed, 292 insertions(+), 180 deletions(-) diff --git a/src/ao.h b/src/ao.h index 4d31f4f5..08f45275 100644 --- 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 { diff --git a/src/ao_gps_print.c b/src/ao_gps_print.c index ca071b42..fcdedd30 100644 --- a/src/ao_gps_print.c +++ b/src/ao_gps_print.c @@ -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) diff --git a/src/ao_gps_report.c b/src/ao_gps_report.c index 7abc93f5..e57f8744 100644 --- a/src/ao_gps_report.c +++ b/src/ao_gps_report.c @@ -20,14 +20,14 @@ 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)) diff --git a/src/ao_gps_sirf.c b/src/ao_gps_sirf.c index 87b1d69c..5827c687 100644 --- 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 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; diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c index 6099ca96..84743ff5 100644 --- 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; -__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); } diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c index 44efb50c..93d7a9ab 100644 --- 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) -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) { diff --git a/src/ao_gps_test_skytraq.c b/src/ao_gps_test_skytraq.c index b94e9bd2..a78fae0f 100644 --- 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) -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) { diff --git a/src/ao_monitor.c b/src/ao_monitor.c index e5e9159f..8f1b9e12 100644 --- 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; -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"); } diff --git a/src/ao_telemetry.c b/src/ao_telemetry.c index 024ac6f8..94ea0b22 100644 --- a/src/ao_telemetry.c +++ b/src/ao_telemetry.c @@ -18,7 +18,13 @@ #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); } -- 2.30.2