X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_tracker.c;h=1454c17c31815aadd08c9d060f10a81cacf78711;hp=9febc7f090cb6ac33197b40cc054b9ee216e8a39;hb=8cc418ed3b0df0e465f0af27c2255cb97294e6ce;hpb=f49540acd48292bd9f68ded647561d0e800c619d

diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c
index 9febc7f0..1454c17c 100644
--- a/src/kernel/ao_tracker.c
+++ b/src/kernel/ao_tracker.c
@@ -3,7 +3,8 @@
  *
  * 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
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -29,16 +30,16 @@ static uint8_t		ao_tracker_force_telem;
 static inline uint8_t
 ao_usb_connected(void)
 {
-	return ao_gpio_get(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, AO_USB_CONNECT) != 0;
+	return ao_gpio_get(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN) != 0;
 }
 #else
 #define ao_usb_connected()	1
 #endif
 
 struct gps_position {
-	int32_t	latitude;
-	int32_t	longitude;
-	int16_t	altitude;
+	int32_t		latitude;
+	int32_t		longitude;
+	gps_alt_t	altitude;
 };
 
 #define GPS_RING	16
@@ -72,7 +73,7 @@ ao_tracker(void)
 #if !HAS_USB_CONNECT
 	ao_tracker_force_telem = 1;
 #endif
-	ao_log_scan();
+	log_started = ao_log_scan();
 
 	ao_rdf_set(1);
 
@@ -122,16 +123,17 @@ ao_tracker(void)
 			{
 				uint8_t	ring;
 				uint8_t	moving = 0;
+				gps_alt_t altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data);
 
 				for (ring = ao_gps_ring_next(gps_head); ring != gps_head; ring = ao_gps_ring_next(ring)) {
 					ground_distance = ao_distance(gps_data.latitude, gps_data.longitude,
 								      gps_position[ring].latitude,
 								      gps_position[ring].longitude);
-					height = gps_position[ring].altitude - gps_data.altitude;
+					height = gps_position[ring].altitude - altitude;
 					if (height < 0)
 						height = -height;
 
-					if (ao_tracker_force_telem)
+					if (ao_tracker_force_telem > 1)
 						printf("head %d ring %d ground_distance %d height %d\n", gps_head, ring, ground_distance, height);
 					if (ground_distance > ao_config.tracker_motion ||
 					    height > (ao_config.tracker_motion << 1))
@@ -140,7 +142,7 @@ ao_tracker(void)
 						break;
 					}
 				}
-				if (ao_tracker_force_telem) {
+				if (ao_tracker_force_telem > 1) {
 					printf ("moving %d started %d\n", moving, log_started);
 					flush();
 				}
@@ -153,7 +155,7 @@ ao_tracker(void)
 					ao_log_gps_data(gps_tick, &gps_data);
 					gps_position[gps_head].latitude = gps_data.latitude;
 					gps_position[gps_head].longitude = gps_data.longitude;
-					gps_position[gps_head].altitude = gps_data.altitude;
+					gps_position[gps_head].altitude = altitude;
 					gps_head = ao_gps_ring_next(gps_head);
 					ao_mutex_put(&tracker_mutex);
 				}
@@ -162,6 +164,25 @@ ao_tracker(void)
 	}
 }
 
+#ifdef AO_LED_GPS_LOCK
+
+static struct ao_task ao_gps_lock_task;
+
+static void
+ao_gps_lock(void)
+{
+	for (;;) {
+		if ((gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
+		    (AO_GPS_VALID|AO_GPS_COURSE_VALID))
+		{
+			ao_led_for(AO_LED_GPS_LOCK, AO_MS_TO_TICKS(20));
+		}
+		ao_delay(AO_SEC_TO_TICKS(3));
+	}
+}
+#endif
+
+
 static uint8_t erasing_current;
 
 void
@@ -180,8 +201,7 @@ void
 ao_tracker_erase_end(void)
 {
 	if (erasing_current) {
-		ao_log_scan();
-		log_started = 0;
+		log_started = ao_log_scan();
 		ao_mutex_put(&tracker_mutex);
 	}
 }
@@ -191,11 +211,9 @@ static struct ao_task ao_tracker_task;
 static void
 ao_tracker_set_telem(void)
 {
-	uint8_t	telem;
-	ao_cmd_hex();
-	telem = ao_cmd_lex_i;
+	uint16_t r = ao_cmd_hex();
 	if (ao_cmd_status == ao_cmd_success)
-		ao_tracker_force_telem = telem;
+		ao_tracker_force_telem = r;
 	ao_cmd_status = ao_cmd_success;
 	printf ("flight: %d\n", ao_flight_number);
 	printf ("force_telem: %d\n", ao_tracker_force_telem);
@@ -203,7 +221,7 @@ ao_tracker_set_telem(void)
 	printf ("log_started: %d\n", log_started);
 	printf ("latitude: %ld\n", (long) gps_data.latitude);
 	printf ("longitude: %ld\n", (long) gps_data.longitude);
-	printf ("altitude: %d\n", gps_data.altitude);
+	printf ("altitude: %ld\n", AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data));
 	printf ("log_running: %d\n", ao_log_running);
 	printf ("log_start_pos: %ld\n", (long) ao_log_start_pos);
 	printf ("log_cur_pos: %ld\n", (long) ao_log_current_pos);
@@ -211,7 +229,7 @@ ao_tracker_set_telem(void)
 }
 
 static const struct ao_cmds ao_tracker_cmds[] = {
-	{ ao_tracker_set_telem,	"t <d>\0Set telem on USB" },
+	{ ao_tracker_set_telem,	"t <d>\0Set telem on USB (0 off, 1 on, 2 dbg)" },
 	{ 0, NULL },
 };
 
@@ -223,4 +241,7 @@ ao_tracker_init(void)
 #endif
 	ao_cmd_register(&ao_tracker_cmds[0]);
 	ao_add_task(&ao_tracker_task, ao_tracker, "tracker");
+#ifdef AO_LED_GPS_LOCK
+	ao_add_task(&ao_gps_lock_task, ao_gps_lock, "gps lock");
+#endif
 }