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>

diff --git a/src/ao.h b/src/ao.h
index e076831d98ef8fff3f057590d820b8f9c15464b8..075ec63af8bce75ef83774d3d869f775a4c94452 100644 (file)
--- 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 dd79f3fc716825ce6a600dfd7bcb0bc77ec589a5..6556ce3215668e345c7e1512b389efec85160f9a 100644 (file)
--- 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();
                }
        }
 }