From 4d51570ed8776461d084726149923c5be43d622e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 25 Oct 2014 19:56:25 -0700 Subject: [PATCH 1/1] altos: Fix up telemetry delay computations With RDF, APRS and telemetry all being sent at varying rates, computing when to send the next radio data is not as simple as sending telemetry and then figuring out whether to send RDF and/or APRS. Fix this by computing times for the next telemetry/rdf/aprs packet, and only sending each when that time has passed. Compute the delay until the next radio activity as the minimum time to any transmission. This also adds code to the config bits to reset the radio times whenever something changes that might affect which radio data to send next. Signed-off-by: Keith Packard --- src/kernel/ao_config.c | 9 ++- src/kernel/ao_telemetry.c | 143 ++++++++++++++++++++++++-------------- src/kernel/ao_telemetry.h | 6 ++ 3 files changed, 104 insertions(+), 54 deletions(-) diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 6b8a1813..8dab7c42 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -557,10 +557,10 @@ ao_config_radio_rate_set(void) __reentrant } _ao_config_edit_start(); ao_config.radio_rate = ao_cmd_lex_i; + _ao_config_edit_finish(); #if HAS_TELEMETRY ao_telemetry_reset_interval(); #endif - _ao_config_edit_finish(); #if HAS_RADIO_RECV ao_radio_recv_abort(); #endif @@ -684,6 +684,9 @@ ao_config_radio_enable_set(void) __reentrant _ao_config_edit_start(); ao_config.radio_enable = ao_cmd_lex_i; _ao_config_edit_finish(); +#if HAS_TELEMETRY && HAS_RADIO_RATE + ao_telemetry_reset_interval(); +#endif } #endif /* HAS_RADIO */ @@ -735,6 +738,7 @@ ao_config_aprs_set(void) _ao_config_edit_start(); ao_config.aprs_interval = ao_cmd_lex_i; _ao_config_edit_finish(); + ao_telemetry_reset_interval(); } #endif /* HAS_APRS */ @@ -825,6 +829,9 @@ ao_config_tracker_set(void) ao_config.tracker_motion = m; ao_config.tracker_interval = i; _ao_config_edit_finish(); +#if HAS_TELEMETRY + ao_telemetry_reset_interval(); +#endif } #endif /* HAS_TRACKER */ diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index 5b56d025..868b3260 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -19,19 +19,32 @@ #include "ao_log.h" #include "ao_product.h" -#ifndef HAS_RDF -#define HAS_RDF 1 -#endif - static __pdata uint16_t ao_telemetry_interval; #if HAS_RADIO_RATE static __xdata uint16_t ao_telemetry_desired_interval; #endif +/* TeleMetrum v1.0 just doesn't have enough space to + * manage the more complicated telemetry scheduling, so + * it loses the ability to disable telem/rdf separately + */ + +#if defined(TELEMETRUM_V_1_0) +#define SIMPLIFY +#endif + +#ifdef SIMPLIFY +#define ao_telemetry_time time +#define RDF_SPACE __pdata +#else +#define RDF_SPACE __xdata +static __pdata uint16_t ao_telemetry_time; +#endif + #if HAS_RDF -static __pdata uint8_t ao_rdf = 0; -static __pdata uint16_t ao_rdf_time; +static RDF_SPACE uint8_t ao_rdf = 0; +static RDF_SPACE uint16_t ao_rdf_time; #endif #if HAS_APRS @@ -308,6 +321,7 @@ ao_send_configuration(void) #if HAS_GPS +static __pdata int8_t ao_telemetry_gps_max; static __pdata int8_t ao_telemetry_loc_cur; static __pdata int8_t ao_telemetry_sat_cur; @@ -324,7 +338,7 @@ ao_send_location(void) telemetry.location.tick = ao_gps_tick; ao_mutex_put(&ao_gps_mutex); ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_loc_cur = ao_telemetry_config_max; + ao_telemetry_loc_cur = ao_telemetry_gps_max; } } @@ -341,7 +355,7 @@ ao_send_satellite(void) 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; + ao_telemetry_sat_cur = ao_telemetry_gps_max; } } #endif @@ -387,6 +401,7 @@ ao_telemetry(void) while (ao_telemetry_interval == 0) ao_sleep(&telemetry); time = ao_time(); + ao_telemetry_time = time; #if HAS_RDF ao_rdf_time = time; #endif @@ -394,73 +409,85 @@ ao_telemetry(void) ao_aprs_time = time; #endif while (ao_telemetry_interval) { -#if HAS_APRS + time = ao_time() + AO_SEC_TO_TICKS(100); +#ifndef SIMPLIFY if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) #endif { -#if HAS_FLIGHT +#ifndef SIMPLIFY + if ( (int16_t) (ao_time() - ao_telemetry_time) >= 0) +#endif + { + ao_telemetry_time = ao_time() + ao_telemetry_interval; # ifdef AO_SEND_MEGA - ao_send_mega_sensor(); - ao_send_mega_data(); + ao_send_mega_sensor(); + ao_send_mega_data(); # endif # ifdef AO_SEND_METRUM - ao_send_metrum_sensor(); - ao_send_metrum_data(); + ao_send_metrum_sensor(); + ao_send_metrum_data(); # endif # ifdef AO_SEND_MINI - ao_send_mini(); + ao_send_mini(); # endif # ifdef AO_TELEMETRY_SENSOR - ao_send_sensor(); + ao_send_sensor(); # endif -#endif /* HAS_FLIGHT */ - #if HAS_COMPANION - if (ao_companion_running) - ao_send_companion(); + if (ao_companion_running) + ao_send_companion(); #endif - ao_send_configuration(); #if HAS_GPS - ao_send_location(); - ao_send_satellite(); + ao_send_location(); + ao_send_satellite(); +#endif + ao_send_configuration(); + } +#ifndef SIMPLIFY + time = ao_telemetry_time; #endif } #if HAS_RDF - if (ao_rdf && -#if HAS_APRS - !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && -#endif /* HAS_APRS */ - (int16_t) (ao_time() - ao_rdf_time) >= 0) + if (ao_rdf +#ifndef SIMPLIFY + && !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) +#endif + ) { + if ((int16_t) (ao_time() - ao_rdf_time) >= 0) { #if HAS_IGNITE_REPORT - uint8_t c; -#endif /* HAS_IGNITE_REPORT */ - ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; + uint8_t c; +#endif + ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; #if HAS_IGNITE_REPORT - if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) - ao_radio_continuity(c); - else -#endif /* HAS_IGNITE_REPORT*/ - ao_radio_rdf(); + if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) + ao_radio_continuity(c); + else +#endif + ao_radio_rdf(); + } +#ifndef SIMPLIFY + if ((int16_t) (time - ao_rdf_time) > 0) + time = ao_rdf_time; +#endif } #endif /* HAS_RDF */ #if HAS_APRS - if (ao_config.aprs_interval != 0 && - (int16_t) (ao_time() - ao_aprs_time) >= 0) - { - ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); - ao_aprs_send(); + if (ao_config.aprs_interval != 0) { + if ((int16_t) (ao_time() - ao_aprs_time) >= 0) { + ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); + ao_aprs_send(); + } + if ((int16_t) (time - ao_aprs_time) > 0) + time = ao_aprs_time; } #endif /* HAS_APRS */ - time += ao_telemetry_interval; delay = time - ao_time(); if (delay > 0) { ao_alarm(delay); ao_sleep(&telemetry); ao_clear_alarm(); } - else - time = ao_time(); } } } @@ -517,21 +544,31 @@ ao_telemetry_set_interval(uint16_t interval) ao_telemetry_companion_cur = cur; #endif - ao_telemetry_config_max = AO_SEC_TO_TICKS(5) / interval; -#if HAS_COMPANION - if (ao_telemetry_config_max > cur) - cur++; - ao_telemetry_config_cur = cur; -#endif - #if HAS_GPS - if (ao_telemetry_config_max > cur) + ao_telemetry_gps_max = AO_SEC_TO_TICKS(1) / interval; + if (ao_telemetry_gps_max > cur) cur++; ao_telemetry_loc_cur = cur; - if (ao_telemetry_config_max > cur) + if (ao_telemetry_gps_max > cur) cur++; ao_telemetry_sat_cur = cur; #endif + + ao_telemetry_config_max = AO_SEC_TO_TICKS(5) / interval; + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_config_cur = cur; + +#ifndef SIMPLIFY + ao_telemetry_time = +#if HAS_RDF + ao_rdf_time = +#endif +#if HAS_APRS + ao_aprs_time = +#endif + ao_time(); +#endif ao_wakeup(&telemetry); } diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h index 340c388e..711e0d36 100644 --- a/src/kernel/ao_telemetry.h +++ b/src/kernel/ao_telemetry.h @@ -120,6 +120,12 @@ struct ao_telemetry_location { #define HAS_WIDE_GPS 1 #endif +#ifdef HAS_TELEMETRY +#ifndef HAS_RDF +#define HAS_RDF 1 +#endif +#endif + #if HAS_WIDE_GPS typedef int32_t gps_alt_t; #define AO_TELEMETRY_LOCATION_ALTITUDE(l) (((gps_alt_t) (l)->altitude_high << 16) | ((l)->altitude_low)) -- 2.30.2