X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_tracker.c;h=0f2491031e8e2d0de8774e2ba1ab96f4d70e9b47;hp=f022923027edfcc17bfe42e44f75ed182cc2b1ee;hb=3ed101d634968666cd3ee2d8c49737970caf406b;hpb=7e911c2afff78db2e385c6346c90bfcd72a8f3fb diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index f0229230..0f249103 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,15 +30,28 @@ 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 -static int32_t last_log_latitude, last_log_longitude; -static int16_t last_log_altitude; -static uint8_t unmoving; +struct gps_position { + int32_t latitude; + int32_t longitude; + gps_alt_t altitude; +}; + +#define GPS_RING 16 + +struct gps_position gps_position[GPS_RING]; + +#define ao_gps_ring_next(n) (((n) + 1) & (GPS_RING - 1)) +#define ao_gps_ring_prev(n) (((n) - 1) & (GPS_RING - 1)) + +static uint8_t gps_head; + +static uint8_t tracker_mutex; static uint8_t log_started; static struct ao_telemetry_location gps_data; static uint8_t tracker_running; @@ -59,8 +73,7 @@ ao_tracker(void) #if !HAS_USB_CONNECT ao_tracker_force_telem = 1; #endif - ao_log_scan(); - ao_log_start(); + log_started = ao_log_scan(); ao_rdf_set(1); @@ -89,76 +102,126 @@ ao_tracker(void) tracker_running = 0; } - if (new_tracker_running && !tracker_running) { + if (new_tracker_running && !tracker_running) ao_telemetry_set_interval(AO_SEC_TO_TICKS(tracker_interval)); - ao_log_start(); - } else if (!new_tracker_running && tracker_running) { + else if (!new_tracker_running && tracker_running) ao_telemetry_set_interval(0); - ao_log_stop(); - } tracker_running = new_tracker_running; - if (!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)) { - if (log_started) { + 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, - last_log_latitude, last_log_longitude); - height = last_log_altitude - gps_data.altitude; + gps_position[ring].latitude, + gps_position[ring].longitude); + height = gps_position[ring].altitude - altitude; if (height < 0) height = -height; - if (ground_distance <= ao_config.tracker_motion && - height <= ao_config.tracker_motion) + + if (ao_tracker_force_telem > 1) + printf("head %d ring %d ground_distance %ld height %d\n", gps_head, ring, (long) ground_distance, height); + if (ground_distance > ao_config.tracker_motion || + height > (ao_config.tracker_motion << 1)) { - if (unmoving < AO_TRACKER_MOTION_COUNT) - unmoving++; - } else - unmoving = 0; + moving = 1; + break; + } } - } else { - if (!log_started) - continue; - if (unmoving < AO_TRACKER_MOTION_COUNT) - unmoving++; - } - - if (unmoving < AO_TRACKER_MOTION_COUNT) { - if (!log_started) { - ao_log_gps_flight(); - log_started = 1; + if (ao_tracker_force_telem > 1) { + printf ("moving %d started %d\n", moving, log_started); + flush(); + } + if (moving) { + ao_mutex_get(&tracker_mutex); + if (!log_started) { + ao_log_gps_flight(); + log_started = 1; + } + 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 = altitude; + gps_head = ao_gps_ring_next(gps_head); + ao_mutex_put(&tracker_mutex); } - ao_log_gps_data(gps_tick, &gps_data); - last_log_latitude = gps_data.latitude; - last_log_longitude = gps_data.longitude; - last_log_altitude = gps_data.altitude; } } } } +#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 +ao_tracker_erase_start(uint16_t flight) +{ + erasing_current = flight == ao_flight_number; + if (erasing_current) { + ao_mutex_get(&tracker_mutex); + ao_log_stop(); + if (++ao_flight_number == 0) + ao_flight_number = 1; + } +} + +void +ao_tracker_erase_end(void) +{ + if (erasing_current) { + log_started = ao_log_scan(); + ao_mutex_put(&tracker_mutex); + } +} + 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); + printf ("tracker_running: %d\n", tracker_running); printf ("log_started: %d\n", log_started); - printf ("unmoving: %d\n", unmoving); 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); @@ -166,7 +229,7 @@ ao_tracker_set_telem(void) } static const struct ao_cmds ao_tracker_cmds[] = { - { ao_tracker_set_telem, "t \0Set telem on USB" }, + { ao_tracker_set_telem, "t \0Set telem on USB (0 off, 1 on, 2 dbg)" }, { 0, NULL }, }; @@ -178,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 }