altos: Make telemetry interval more consistent

Instead of using a delay between telemetry packets, use a telemetry
period and compute an appropriate delay each time. This requires
changing the ascent telemetry from a 50ms delay to a 100ms interval,
to provide a regular 10 packets-per-second rate. Before, we counted on
the telemetry packet taking about 50ms to send so that we would
receive about 10 per second.

This also eliminates delays during descent for RDF tones -- those will
get transmitted in the interval between telemetry packets without
interrupting the spacing of those packets.

Signed-off-by: Keith Packard <keithp@keithp.com>

2 files changed, 29 insertions, 21 deletions

diff --git a/src/ao.h b/src/ao.h
index e076831d..075ec63a 100644
--- a/src/ao.h
+++ b/src/ao.h
@@ -937,7 +937,7 @@ struct ao_telemetry_recv {
 /* Set delay between telemetry reports (0 to disable) */
 
 #define AO_TELEMETRY_INTERVAL_PAD	AO_MS_TO_TICKS(1000)
-#define AO_TELEMETRY_INTERVAL_FLIGHT	AO_MS_TO_TICKS(50)
+#define AO_TELEMETRY_INTERVAL_FLIGHT	AO_MS_TO_TICKS(100)
 #define AO_TELEMETRY_INTERVAL_RECOVER	AO_MS_TO_TICKS(1000)
 
 void
diff --git a/src/ao_telemetry.c b/src/ao_telemetry.c
index dd79f3fc..6556ce32 100644
--- a/src/ao_telemetry.c
+++ b/src/ao_telemetry.c
@@ -27,6 +27,8 @@ __xdata uint16_t ao_rdf_time;
 void
 ao_telemetry(void)
 {
+	uint16_t	time;
+	int16_t		delay;
 	static __xdata struct ao_telemetry telemetry;
 
 	ao_config_get();
@@ -37,35 +39,41 @@ ao_telemetry(void)
 	telemetry.flight = ao_log_full() ? 0 : ao_flight_number;
 	telemetry.accel_plus_g = ao_config.accel_plus_g;
 	telemetry.accel_minus_g = ao_config.accel_minus_g;
-	ao_rdf_time = ao_time();
 	for (;;) {
 		while (ao_telemetry_interval == 0)
 			ao_sleep(&ao_telemetry_interval);
-		telemetry.flight_state = ao_flight_state;
+		time = ao_rdf_time = ao_time();
+		while (ao_telemetry_interval) {
+			telemetry.flight_state = ao_flight_state;
 #if HAS_ACCEL
-		telemetry.flight_accel = ao_flight_accel;
-		telemetry.ground_accel = ao_ground_accel;
-		telemetry.flight_vel = ao_flight_vel;
+			telemetry.flight_accel = ao_flight_accel;
+			telemetry.ground_accel = ao_ground_accel;
+			telemetry.flight_vel = ao_flight_vel;
 #endif
-		telemetry.flight_pres = ao_flight_pres;
-		telemetry.ground_pres = ao_ground_pres;
+			telemetry.flight_pres = ao_flight_pres;
+			telemetry.ground_pres = ao_ground_pres;
 #if HAS_ADC
-		ao_adc_get(&telemetry.adc);
+			ao_adc_get(&telemetry.adc);
 #endif
 #if HAS_GPS
-		ao_mutex_get(&ao_gps_mutex);
-		memcpy(&telemetry.gps, &ao_gps_data, sizeof (struct ao_gps_data));
-		memcpy(&telemetry.gps_tracking, &ao_gps_tracking_data, sizeof (struct ao_gps_tracking_data));
-		ao_mutex_put(&ao_gps_mutex);
+			ao_mutex_get(&ao_gps_mutex);
+			memcpy(&telemetry.gps, &ao_gps_data, sizeof (struct ao_gps_data));
+			memcpy(&telemetry.gps_tracking, &ao_gps_tracking_data, sizeof (struct ao_gps_tracking_data));
+			ao_mutex_put(&ao_gps_mutex);
 #endif
-		ao_radio_send(&telemetry, sizeof (telemetry));
-		ao_delay(ao_telemetry_interval);
-		if (ao_rdf &&
-		    (int16_t) (ao_time() - ao_rdf_time) >= 0)
-		{
-			ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
-			ao_radio_rdf(AO_RDF_LENGTH_MS);
-			ao_delay(ao_telemetry_interval);
+			ao_radio_send(&telemetry, sizeof (telemetry));
+			if (ao_rdf &&
+			    (int16_t) (ao_time() - ao_rdf_time) >= 0)
+			{
+				ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
+				ao_radio_rdf(AO_RDF_LENGTH_MS);
+			}
+			time += ao_telemetry_interval;
+			delay = time - ao_time();
+			if (delay > 0)
+				ao_delay(delay);
+			else
+				time = ao_time();
 		}
 	}
 }