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>

1 files changed, 28 insertions, 20 deletions

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();
 		}
 	}
 }