X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fproduct%2Fao_tracker.c;h=c1c3366cf08b6c25e4efac94d4f353bdff0773f5;hp=a710d49d5ac611078f1f4bc996d43e3de30d1a66;hb=6160ddadeae324b4a68db800c98c339156b63076;hpb=3e28d8a242955d65d8cd50dbba4cad4609e2e1ae

diff --git a/src/product/ao_tracker.c b/src/product/ao_tracker.c
index a710d49d..c1c3366c 100644
--- a/src/product/ao_tracker.c
+++ b/src/product/ao_tracker.c
@@ -22,10 +22,11 @@
 
 enum ao_flight_state	ao_flight_state;
 
-/* Speeds for the various modes */
-#define AO_TRACKER_NOT_MOVING	100
+/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */
+#define AO_TRACKER_NOT_MOVING	200
 
 static uint8_t	ao_tracker_force_telem;
+static uint8_t	ao_tracker_force_launch;
 
 #if HAS_USB_CONNECT
 static inline uint8_t
@@ -37,6 +38,8 @@ ao_usb_connected(void)
 #define ao_usb_connected()	1
 #endif
 
+#define STARTUP_AVERAGE	5
+
 static void
 ao_tracker(void)
 {
@@ -48,8 +51,19 @@ ao_tracker(void)
 	uint32_t	ground_distance;
 	int16_t		height;
 	uint16_t	speed;
+	int64_t		lat_sum = 0, lon_sum = 0;
+	int32_t		alt_sum = 0;
+	int		nsamples = 0;
 
+#if HAS_ADC
 	ao_timer_set_adc_interval(100);
+#endif
+
+	ao_log_scan();
+
+	ao_rdf_set(1);
+
+	ao_telemetry_set_interval(0);
 
 	ao_flight_state = ao_flight_startup;
 	for (;;) {
@@ -69,23 +83,34 @@ ao_tracker(void)
 			switch (ao_flight_state) {
 			case ao_flight_startup:
 				/* startup to pad when GPS locks */
-				ao_flight_state = ao_flight_pad;
-				start_latitude = ao_gps_data.longitude;
-				start_longitude = ao_gps_data.latitude;
-				start_altitude = ao_gps_data.altitude;
+
+				lat_sum += ao_gps_data.latitude;
+				lon_sum += ao_gps_data.longitude;
+				alt_sum += ao_gps_data.altitude;
+
+				if (++nsamples >= STARTUP_AVERAGE) {
+					ao_flight_state = ao_flight_pad;
+					ao_wakeup(&ao_flight_state);
+					start_latitude = lat_sum / nsamples;
+					start_longitude = lon_sum / nsamples;
+					start_altitude = alt_sum / nsamples;
+				}
 				break;
 			case ao_flight_pad:
 				ground_distance = ao_distance(ao_gps_data.latitude,
-							      start_latitude,
 							      ao_gps_data.longitude,
+							      start_latitude,
 							      start_longitude);
 				height = ao_gps_data.altitude - start_altitude;
 				if (height < 0)
 					height = -height;
+
 				if (ground_distance >= ao_config.tracker_start_horiz ||
-				    height >= ao_config.tracker_start_vert)
+				    height >= ao_config.tracker_start_vert ||
+				    ao_tracker_force_launch)
 				{
 					ao_flight_state = ao_flight_drogue;
+					ao_wakeup(&ao_flight_state);
 					ao_log_start();
 				}
 				break;
@@ -134,8 +159,11 @@ static void
 ao_tracker_set_telem(void)
 {
 	ao_cmd_hex();
-	if (ao_cmd_status == ao_cmd_success)
-		ao_tracker_force_telem = ao_cmd_lex_i;
+	if (ao_cmd_status == ao_cmd_success) {
+		ao_tracker_force_telem = (ao_cmd_lex_i & 1) != 0;
+		ao_tracker_force_launch = (ao_cmd_lex_i & 2) != 0;
+		printf ("force telem %d force launch %d\n", ao_tracker_force_telem, ao_tracker_force_launch);
+	}
 }
 
 static const struct ao_cmds ao_tracker_cmds[] = {