X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_telemetry.c;h=27306a344c27a0cdb27b7e2bf19789e1fef4c97f;hp=dcda0dd7fdb451e3f797fa2f2213f4465c931111;hb=0cedc27e22a9fbc9ccfe1b403c84d728bb23220d;hpb=ef85b3bc5300904ebfb878b1c7313a82b5b7aebf

diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c
index dcda0dd7..27306a34 100644
--- a/src/kernel/ao_telemetry.c
+++ b/src/kernel/ao_telemetry.c
@@ -1,5 +1,5 @@
 /*
- * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ * Copyright © 2011 Keth Packard <keithp@keithp.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -25,6 +25,10 @@
 
 static __pdata uint16_t ao_telemetry_interval;
 
+#if HAS_RADIO_RATE
+static __xdata uint16_t ao_telemetry_desired_interval;
+#endif
+
 #if HAS_RDF
 static __pdata uint8_t ao_rdf = 0;
 static __pdata uint16_t ao_rdf_time;
@@ -64,7 +68,7 @@ static void
 ao_send_sensor(void)
 {
 	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
-			
+
 	telemetry.generic.tick = packet->tick;
 	telemetry.generic.type = AO_TELEMETRY_SENSOR;
 
@@ -106,12 +110,13 @@ ao_send_sensor(void)
 
 
 #ifdef AO_SEND_MEGA
+
 /* Send mega sensor packet */
 static void
 ao_send_mega_sensor(void)
 {
 	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
-			
+
 	telemetry.generic.tick = packet->tick;
 	telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR;
 
@@ -240,7 +245,7 @@ static void
 ao_send_mini(void)
 {
 	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
-			
+
 	telemetry.generic.tick = packet->tick;
 	telemetry.generic.type = AO_TELEMETRY_MINI;
 
@@ -339,10 +344,7 @@ ao_send_location(void)
 		ao_mutex_get(&ao_gps_mutex);
 		ao_xmemcpy(&telemetry.location.flags,
 		       &ao_gps_data.flags,
-		       26);
-#if HAS_TRACKER
-		telemetry.location.state = ao_flight_state | AO_GPS_STATE_VALID;
-#endif
+		       27);
 		telemetry.location.tick = ao_gps_tick;
 		ao_mutex_put(&ao_gps_mutex);
 		ao_radio_send(&telemetry, sizeof (telemetry));
@@ -493,12 +495,33 @@ ao_telemetry(void)
 	}
 }
 
+#if HAS_RADIO_RATE
+void
+ao_telemetry_reset_interval(void)
+{
+	ao_telemetry_set_interval(ao_telemetry_desired_interval);
+}
+#endif
+
 void
 ao_telemetry_set_interval(uint16_t interval)
 {
 	int8_t	cur = 0;
+
+#if HAS_RADIO_RATE
+	/* Limit max telemetry rate based on available radio bandwidth.
+	 */
+	static __xdata const uint16_t min_interval[] = {
+		/* [AO_RADIO_RATE_38400] = */ AO_MS_TO_TICKS(100),
+		/* [AO_RADIO_RATE_9600] = */ AO_MS_TO_TICKS(500),
+		/* [AO_RADIO_RATE_2400] = */ AO_MS_TO_TICKS(1000)
+	};
+
+	ao_telemetry_desired_interval = interval;
+	if (interval && interval < min_interval[ao_config.radio_rate])
+		interval = min_interval[ao_config.radio_rate];
+#endif
 	ao_telemetry_interval = interval;
-	
 #if AO_SEND_MEGA
 	if (interval > 1)
 		ao_telemetry_mega_data_max = 1;
@@ -524,7 +547,7 @@ ao_telemetry_set_interval(uint16_t interval)
 	ao_telemetry_companion_cur = cur;
 #endif
 
-	ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval;
+	ao_telemetry_config_max = AO_SEC_TO_TICKS(5) / interval;
 #if HAS_COMPANION
 	if (ao_telemetry_config_max > cur)
 		cur++;