X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fproduct%2Fao_tracker.c;h=b207c56204d866fae30eb33e53b5c263223f46ab;hp=d4960d258081a0b0e8223baab5df493c2cfe83af;hb=6a79fe8144dcab10294caa4b0eac967eeb0d8e7e;hpb=b619199345256cdc456f8a0b99c0ad9335e877c7

diff --git a/src/product/ao_tracker.c b/src/product/ao_tracker.c
index d4960d25..b207c562 100644
--- a/src/product/ao_tracker.c
+++ b/src/product/ao_tracker.c
@@ -26,6 +26,7 @@ enum ao_flight_state	ao_flight_state;
 #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
@@ -39,12 +40,37 @@ ao_usb_connected(void)
 
 #define STARTUP_AVERAGE	5
 
+static void
+ao_tracker_start_flight(void)
+{
+	struct ao_log_mega log;
+	ao_log_start();
+	log.type = AO_LOG_FLIGHT;
+	log.tick = ao_time();
+#if HAS_ACCEL
+	log.u.flight.ground_accel = ao_ground_accel;
+#endif
+#if HAS_GYRO
+	log.u.flight.ground_accel_along = ao_ground_accel_along;
+	log.u.flight.ground_accel_across = ao_ground_accel_across;
+	log.u.flight.ground_accel_through = ao_ground_accel_through;
+	log.u.flight.ground_roll = ao_ground_roll;
+	log.u.flight.ground_pitch = ao_ground_pitch;
+	log.u.flight.ground_yaw = ao_ground_yaw;
+#endif
+#if HAS_FLIGHT
+	log.u.flight.ground_pres = ao_ground_pres;
+#endif
+	log.u.flight.flight = ao_flight_number;
+	ao_log_mega(&log);
+}
+
 static void
 ao_tracker(void)
 {
 	uint16_t	telem_rate = AO_SEC_TO_TICKS(1), new_telem_rate;
 	uint8_t		gps_rate = 1, new_gps_rate;
-	uint8_t		telem_enabled = 1, new_telem_enabled;
+	uint8_t		telem_enabled = 0, new_telem_enabled;
 	int32_t		start_latitude = 0, start_longitude = 0;
 	int16_t		start_altitude = 0;
 	uint32_t	ground_distance;
@@ -58,6 +84,10 @@ ao_tracker(void)
 	ao_timer_set_adc_interval(100);
 #endif
 
+#if !HAS_USB_CONNECT
+	ao_tracker_force_telem = 1;
+#endif
+
 	ao_log_scan();
 
 	ao_rdf_set(1);
@@ -97,21 +127,24 @@ ao_tracker(void)
 				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();
+					ao_tracker_start_flight();
 				}
 				break;
 			case ao_flight_drogue:
-
 				/* Modulate data rates based on speed (in cm/s) */
 				if (ao_gps_data.climb_rate < 0)
 					speed = -ao_gps_data.climb_rate;
@@ -155,8 +188,12 @@ 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 ("flight %d force telem %d force launch %d\n",
+		ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch);
 }
 
 static const struct ao_cmds ao_tracker_cmds[] = {