- /* Don't change anything if GPS isn't locked */
- if ((ao_gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
- (AO_GPS_VALID|AO_GPS_COURSE_VALID))
- {
- switch (ao_flight_state) {
- case ao_flight_startup:
- /* startup to pad when GPS locks */
-
- 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;
+ if (new_tracker_running && !tracker_running)
+ ao_telemetry_set_interval(AO_SEC_TO_TICKS(tracker_interval));
+ else if (!new_tracker_running && tracker_running)
+ ao_telemetry_set_interval(0);
+
+ tracker_running = new_tracker_running;
+
+ if (new_tracker_running && !ao_log_running)
+ ao_log_start();
+ else if (!new_tracker_running && ao_log_running)
+ ao_log_stop();
+
+ if (!ao_log_running)
+ continue;
+
+ if (new & AO_GPS_NEW_DATA) {
+ if ((gps_data.flags & (AO_GPS_VALID|AO_GPS_COURSE_VALID)) ==
+ (AO_GPS_VALID|AO_GPS_COURSE_VALID))
+ {
+ 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 - altitude;
+ if (height < 0)
+ height = -height;
+
+ 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))
+ {
+ moving = 1;
+ break;
+ }