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 <keithp@keithp.com>

1 files changed, 121 insertions, 49 deletions

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