From 1873d539a8f1a0e1e8ad539af5d49a77a129b928 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 07:41:11 -0700 Subject: altos: Move ao_tracker.c to kernel Doesn't make sense to be in product Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 212 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 212 insertions(+) create mode 100644 src/kernel/ao_tracker.c (limited to 'src/kernel/ao_tracker.c') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c new file mode 100644 index 00000000..b207c562 --- /dev/null +++ b/src/kernel/ao_tracker.c @@ -0,0 +1,212 @@ +/* + * Copyright © 2014 Keith Packard + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include + +enum ao_flight_state ao_flight_state; + +/* 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 +ao_usb_connected(void) +{ + return ao_gpio_get(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, AO_USB_CONNECT) != 0; +} +#else +#define ao_usb_connected() 1 +#endif + +#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 = 0, new_telem_enabled; + int32_t start_latitude = 0, start_longitude = 0; + int16_t start_altitude = 0; + 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 + +#if !HAS_USB_CONNECT + ao_tracker_force_telem = 1; +#endif + + ao_log_scan(); + + ao_rdf_set(1); + + ao_telemetry_set_interval(0); + + ao_flight_state = ao_flight_startup; + for (;;) { + ao_sleep(&ao_gps_new); + + new_gps_rate = gps_rate; + new_telem_rate = telem_rate; + + new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); + + ao_mutex_get(&ao_gps_mutex); + + /* 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; + } + break; + case ao_flight_pad: + ground_distance = ao_distance(ao_gps_data.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 || + 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; + else + speed = ao_gps_data.climb_rate; + speed += ao_gps_data.ground_speed; + + if (speed < AO_TRACKER_NOT_MOVING) { + new_telem_rate = AO_SEC_TO_TICKS(10); + new_gps_rate = 10; + } else { + new_telem_rate = AO_SEC_TO_TICKS(1); + new_gps_rate = 1; + } + break; + default: + break; + } + } + ao_mutex_put(&ao_gps_mutex); + + if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { + if (new_telem_enabled) + ao_telemetry_set_interval(new_telem_rate); + else + ao_telemetry_set_interval(0); + telem_rate = new_telem_rate; + telem_enabled = new_telem_enabled; + } + + if (new_gps_rate != gps_rate) { + ao_gps_set_rate(new_gps_rate); + gps_rate = new_gps_rate; + } + } +} + +static struct ao_task ao_tracker_task; + +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 & 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[] = { + { ao_tracker_set_telem, "t \0Set telem on USB" }, + { 0, NULL }, +}; + +void +ao_tracker_init(void) +{ +#if HAS_USB_CONNECT + ao_enable_input(AO_USB_CONNECT_PORT, AO_USB_CONNECT_PIN, 0); +#endif + ao_cmd_register(&ao_tracker_cmds[0]); + ao_add_task(&ao_tracker_task, ao_tracker, "tracker"); +} -- cgit v1.2.3 From 394ab536257ab58de0190b3828dd3bb897ad4474 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 11:40:41 -0700 Subject: altos: Write tracker logging from tracker thread directly Also, logs 8 pre-launch GPS packets so we can get the ground position. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 270 ++++++++++++++++++++++++++++-------------------- src/kernel/ao_tracker.h | 23 +++++ 2 files changed, 180 insertions(+), 113 deletions(-) (limited to 'src/kernel/ao_tracker.c') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index b207c562..4bc229b4 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -17,14 +17,14 @@ #include #include +#include +#include #include +#include #include enum ao_flight_state ao_flight_state; -/* 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; @@ -40,45 +40,147 @@ ao_usb_connected(void) #define STARTUP_AVERAGE 5 +int32_t ao_tracker_start_latitude; +int32_t ao_tracker_start_longitude; +int16_t ao_tracker_start_altitude; + +struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING]; +uint8_t ao_tracker_head; +static uint8_t ao_tracker_log_pos; + +static uint16_t telem_rate; +static uint8_t gps_rate; +static uint8_t telem_enabled; + +static int16_t lat_sum, lon_sum; +static int32_t alt_sum; +static int nsamples; + static void -ao_tracker_start_flight(void) +ao_tracker_state_update(struct ao_tracker_data *tracker) { - 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); + uint16_t new_telem_rate; + uint8_t new_gps_rate; + uint8_t new_telem_enabled; + uint32_t ground_distance; + int16_t height; + uint16_t speed; + + new_gps_rate = gps_rate; + new_telem_rate = telem_rate; + + new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); + + /* Don't change anything if GPS isn't locked */ + if ((tracker->new & AO_GPS_NEW_DATA) && + (tracker->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 += tracker->gps_data.latitude; + lon_sum += tracker->gps_data.longitude; + alt_sum += tracker->gps_data.altitude; + + if (++nsamples >= STARTUP_AVERAGE) { + ao_flight_state = ao_flight_pad; + ao_wakeup(&ao_flight_state); + ao_tracker_start_latitude = lat_sum / nsamples; + ao_tracker_start_longitude = lon_sum / nsamples; + ao_tracker_start_altitude = alt_sum / nsamples; + } + break; + case ao_flight_pad: + ground_distance = ao_distance(tracker->gps_data.latitude, + tracker->gps_data.longitude, + ao_tracker_start_latitude, + ao_tracker_start_longitude); + height = tracker->gps_data.altitude - ao_tracker_start_altitude; + if (height < 0) + height = -height; + + if (ground_distance >= ao_config.tracker_start_horiz || + height >= ao_config.tracker_start_vert || + ao_tracker_force_launch) + { + ao_flight_state = ao_flight_drogue; + ao_wakeup(&ao_flight_state); + ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_head); + ao_log_start(); + ao_log_gps_flight(); + } + break; + case ao_flight_drogue: + /* Modulate data rates based on speed (in cm/s) */ + if (tracker->gps_data.climb_rate < 0) + speed = -tracker->gps_data.climb_rate; + else + speed = tracker->gps_data.climb_rate; + speed += tracker->gps_data.ground_speed; + + if (speed < AO_TRACKER_NOT_MOVING) { + new_telem_rate = AO_SEC_TO_TICKS(10); + new_gps_rate = 10; + } else { + new_telem_rate = AO_SEC_TO_TICKS(1); + new_gps_rate = 1; + } + break; + default: + break; + } + } + + if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { + if (new_telem_enabled) + ao_telemetry_set_interval(new_telem_rate); + else + ao_telemetry_set_interval(0); + telem_rate = new_telem_rate; + telem_enabled = new_telem_enabled; + } + + if (new_gps_rate != gps_rate) { + ao_gps_set_rate(new_gps_rate); + gps_rate = new_gps_rate; + } } +#if HAS_LOG +static uint8_t ao_tracker_should_log; + +static void +ao_tracker_log(void) +{ + struct ao_tracker_data *tracker; + + if (ao_log_running) { + while (ao_tracker_log_pos != ao_tracker_head) { + tracker = &ao_tracker_data[ao_tracker_log_pos]; + if (tracker->new & AO_GPS_NEW_DATA) { + ao_tracker_should_log = ao_log_gps_should_log(tracker->gps_data.latitude, + tracker->gps_data.longitude, + tracker->gps_data.altitude); + if (ao_tracker_should_log) + ao_log_gps_data(tracker->tick, tracker->state, &tracker->gps_data); + } + if (tracker->new & AO_GPS_NEW_TRACKING) { + if (ao_tracker_should_log) + ao_log_gps_tracking(tracker->tick, &tracker->gps_tracking_data); + } + ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_log_pos); + } + } +} +#endif + 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 = 0, new_telem_enabled; - int32_t start_latitude = 0, start_longitude = 0; - int16_t start_altitude = 0; - 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; + uint8_t new; + struct ao_tracker_data *tracker; #if HAS_ADC ao_timer_set_adc_interval(100); @@ -91,94 +193,36 @@ ao_tracker(void) ao_log_scan(); ao_rdf_set(1); - ao_telemetry_set_interval(0); + telem_rate = AO_SEC_TO_TICKS(1); + telem_enabled = 0; + gps_rate = 1; ao_flight_state = ao_flight_startup; for (;;) { - ao_sleep(&ao_gps_new); - - new_gps_rate = gps_rate; - new_telem_rate = telem_rate; - - new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + /* Stick GPS data into the ring */ ao_mutex_get(&ao_gps_mutex); + tracker = &ao_tracker_data[ao_tracker_head]; + tracker->tick = ao_gps_tick; + tracker->new = new; + tracker->state = ao_flight_state; + tracker->gps_data = ao_gps_data; + tracker->gps_tracking_data = ao_gps_tracking_data; + ao_tracker_head = ao_tracker_ring_next(ao_tracker_head); - /* 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; - } - break; - case ao_flight_pad: - ground_distance = ao_distance(ao_gps_data.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 || - 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; - else - speed = ao_gps_data.climb_rate; - speed += ao_gps_data.ground_speed; - - if (speed < AO_TRACKER_NOT_MOVING) { - new_telem_rate = AO_SEC_TO_TICKS(10); - new_gps_rate = 10; - } else { - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - } - break; - default: - break; - } - } + ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { - if (new_telem_enabled) - ao_telemetry_set_interval(new_telem_rate); - else - ao_telemetry_set_interval(0); - telem_rate = new_telem_rate; - telem_enabled = new_telem_enabled; - } + /* Update state based on current GPS data */ + ao_tracker_state_update(tracker); - if (new_gps_rate != gps_rate) { - ao_gps_set_rate(new_gps_rate); - gps_rate = new_gps_rate; - } +#if HAS_LOG + /* Log all gps data */ + ao_tracker_log(); +#endif } } diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h index 43556965..1adf0f33 100644 --- a/src/kernel/ao_tracker.h +++ b/src/kernel/ao_tracker.h @@ -21,6 +21,29 @@ #define AO_CONFIG_DEFAULT_TRACKER_START_HORIZ 1000 #define AO_CONFIG_DEFAULT_TRACKER_START_VERT 100 +/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */ +#define AO_TRACKER_NOT_MOVING 200 + +extern int32_t ao_tracker_start_latitude; +extern int32_t ao_tracker_start_longitude; +extern int16_t ao_tracker_start_altitude; + +#define AO_TRACKER_RING 8 + +struct ao_tracker_data { + uint16_t tick; + uint8_t new; + uint8_t state; + struct ao_telemetry_location gps_data; + struct ao_telemetry_satellite gps_tracking_data; +}; + +extern struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING]; +extern uint8_t ao_tracker_head; + +#define ao_tracker_ring_next(n) (((n) + 1) & (AO_TRACKER_RING-1)) +#define ao_tracker_ring_prev(n) (((n) - 1) & (AO_TRACKER_RING-1)) + void ao_tracker_init(void); -- cgit v1.2.3 From bd9e4f30b2a491b030246943767960ab053ac94c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 21:05:01 -0700 Subject: altos: Define lat/lon sum variables as 64-bit instead of 16 Oops. 16 bits won't hold position information... Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) (limited to 'src/kernel/ao_tracker.c') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index 4bc229b4..cdf147cd 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -52,7 +52,7 @@ static uint16_t telem_rate; static uint8_t gps_rate; static uint8_t telem_enabled; -static int16_t lat_sum, lon_sum; +static int64_t lat_sum, lon_sum; static int32_t alt_sum; static int nsamples; @@ -78,13 +78,18 @@ ao_tracker_state_update(struct ao_tracker_data *tracker) { switch (ao_flight_state) { case ao_flight_startup: + new_telem_rate = AO_SEC_TO_TICKS(1); + new_gps_rate = 1; + /* startup to pad when GPS locks */ lat_sum += tracker->gps_data.latitude; lon_sum += tracker->gps_data.longitude; alt_sum += tracker->gps_data.altitude; - if (++nsamples >= STARTUP_AVERAGE) { + ++nsamples; + + if (nsamples >= STARTUP_AVERAGE) { ao_flight_state = ao_flight_pad; ao_wakeup(&ao_flight_state); ao_tracker_start_latitude = lat_sum / nsamples; @@ -93,6 +98,9 @@ ao_tracker_state_update(struct ao_tracker_data *tracker) } break; case ao_flight_pad: + new_telem_rate = AO_SEC_TO_TICKS(1); + new_gps_rate = 1; + ground_distance = ao_distance(tracker->gps_data.latitude, tracker->gps_data.longitude, ao_tracker_start_latitude, @@ -190,6 +198,11 @@ ao_tracker(void) ao_tracker_force_telem = 1; #endif + nsamples = 0; + lat_sum = 0; + lon_sum = 0; + alt_sum = 0; + ao_log_scan(); ao_rdf_set(1); @@ -231,11 +244,16 @@ static struct ao_task ao_tracker_task; static void ao_tracker_set_telem(void) { + uint8_t telem, launch; + ao_cmd_hex(); + telem = ao_cmd_lex_i; ao_cmd_hex(); + launch = 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; + ao_tracker_force_telem = telem; + ao_tracker_force_launch = launch; } + ao_cmd_status = ao_cmd_success; printf ("flight %d force telem %d force launch %d\n", ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch); } -- cgit v1.2.3 From 9d7f4fb6af0fee843191766858e39a481aeda347 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 10 Jun 2014 09:52:15 -0700 Subject: altos: Simplify tracker logic, removing boost detect This removes the ao_flight_state value from the tracker code and makes it simply log position information when the device has moved within the last 10 log intervals. This also changes the configuration parameters to define what 'motionless' means, and what interval to configure the GPS receiver for, log data and send telemetry. Signed-off-by: Keith Packard --- src/kernel/ao_config.c | 20 +-- src/kernel/ao_config.h | 4 +- src/kernel/ao_log_gps.c | 37 +----- src/kernel/ao_log_gps.h | 9 +- src/kernel/ao_tracker.c | 277 ++++++++++++++---------------------------- src/kernel/ao_tracker.h | 27 +--- src/telegps-v0.3/ao_telegps.c | 1 - src/telegps-v1.0/ao_telegps.c | 1 - 8 files changed, 113 insertions(+), 263 deletions(-) (limited to 'src/kernel/ao_tracker.c') diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 72170555..71445335 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -184,8 +184,8 @@ _ao_config_get(void) #endif #if HAS_TRACKER if (minor < 17) { - ao_config.tracker_start_horiz = AO_CONFIG_DEFAULT_TRACKER_START_HORIZ; - ao_config.tracker_start_vert = AO_CONFIG_DEFAULT_TRACKER_START_VERT; + ao_config.tracker_motion = AO_TRACKER_MOTION_DEFAULT; + ao_config.tracker_interval = AO_TRACKER_INTERVAL_DEFAULT; } #endif #if AO_PYRO_NUM @@ -695,25 +695,25 @@ void ao_config_tracker_show(void) { printf ("Tracker setting: %d %d\n", - ao_config.tracker_start_horiz, - ao_config.tracker_start_vert); + ao_config.tracker_motion, + ao_config.tracker_interval); } void ao_config_tracker_set(void) { - uint16_t h, v; + uint16_t m, i; ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - h = ao_cmd_lex_i; + m = ao_cmd_lex_i; ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - v = ao_cmd_lex_i; + i = ao_cmd_lex_i; _ao_config_edit_start(); - ao_config.tracker_start_horiz = h; - ao_config.tracker_start_vert = v; + ao_config.tracker_motion = m; + ao_config.tracker_interval = i; _ao_config_edit_finish(); } #endif /* HAS_TRACKER */ @@ -814,7 +814,7 @@ __code struct ao_config_var ao_config_vars[] = { ao_config_beep_set, ao_config_beep_show }, #endif #if HAS_TRACKER - { "t \0Tracker start trigger distances", + { "t \0Tracker configuration", ao_config_tracker_set, ao_config_tracker_show }, #endif { "s\0Show", diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index 77f73fbe..2b5cd352 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -96,8 +96,8 @@ struct ao_config { uint8_t mid_beep; /* minor version 16 */ #endif #if HAS_TRACKER - uint16_t tracker_start_horiz; /* minor version 17 */ - uint16_t tracker_start_vert; /* minor version 17 */ + uint16_t tracker_motion; /* minor version 17 */ + uint8_t tracker_interval; /* minor version 17 */ #endif #if AO_PYRO_NUM uint16_t pyro_time; /* minor version 18 */ diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c index e9e573a5..8bf529f4 100644 --- a/src/kernel/ao_log_gps.c +++ b/src/kernel/ao_log_gps.c @@ -60,51 +60,17 @@ ao_log_gps(__xdata struct ao_log_gps *log) __reentrant return wrote; } - -static int32_t prev_lat, prev_lon; -static int16_t prev_alt; -static uint8_t has_prev, unmoving; - -#define GPS_SPARSE_UNMOVING_REPORTS 10 -#define GPS_SPARSE_UNMOVING_GROUND 10 -#define GPS_SPARSE_UNMOVING_AIR 10 - -uint8_t -ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt) -{ - if (has_prev && ao_log_running) { - uint32_t h = ao_distance(prev_lat, prev_lon, lat, lon); - uint16_t v = alt > prev_alt ? (alt - prev_alt) : (prev_alt - alt); - - if (h < GPS_SPARSE_UNMOVING_GROUND && v < GPS_SPARSE_UNMOVING_AIR) { - if (unmoving < GPS_SPARSE_UNMOVING_REPORTS) - ++unmoving; - } else - unmoving = 0; - } else - unmoving = 0; - - prev_lat = lat; - prev_lon = lon; - prev_alt = alt; - has_prev = 1; - return unmoving >= GPS_SPARSE_UNMOVING_REPORTS; -} - void ao_log_gps_flight(void) { log.type = AO_LOG_FLIGHT; log.tick = ao_time(); log.u.flight.flight = ao_flight_number; - log.u.flight.start_altitude = ao_tracker_start_altitude; - log.u.flight.start_latitude = ao_tracker_start_latitude; - log.u.flight.start_longitude = ao_tracker_start_longitude; ao_log_gps(&log); } void -ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data) +ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data) { log.tick = tick; log.type = AO_LOG_GPS_TIME; @@ -126,7 +92,6 @@ ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_ log.u.gps.hdop = gps_data->hdop; log.u.gps.vdop = gps_data->vdop; log.u.gps.mode = gps_data->mode; - log.u.gps.state = state; ao_log_gps(&log); } diff --git a/src/kernel/ao_log_gps.h b/src/kernel/ao_log_gps.h index 733db19b..5851f4d1 100644 --- a/src/kernel/ao_log_gps.h +++ b/src/kernel/ao_log_gps.h @@ -18,6 +18,9 @@ #include "ao.h" #include "ao_telemetry.h" +#ifndef _AO_LOG_GPS_H_ +#define _AO_LOG_GPS_H_ + uint8_t ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt); @@ -25,8 +28,6 @@ void ao_log_gps_flight(void); void -ao_log_gps_data(uint16_t tick, uint8_t state, struct ao_telemetry_location *gps_data); - -void -ao_log_gps_tracking(uint16_t tick, struct ao_telemetry_satellite *gps_tracking_data); +ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data); +#endif /* _AO_LOG_GPS_H_ */ diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index cdf147cd..fb9e75d0 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -23,10 +23,7 @@ #include #include -enum ao_flight_state ao_flight_state; - -static uint8_t ao_tracker_force_telem; -static uint8_t ao_tracker_force_launch; +static uint8_t ao_tracker_force_telem; #if HAS_USB_CONNECT static inline uint8_t @@ -38,157 +35,22 @@ ao_usb_connected(void) #define ao_usb_connected() 1 #endif -#define STARTUP_AVERAGE 5 - -int32_t ao_tracker_start_latitude; -int32_t ao_tracker_start_longitude; -int16_t ao_tracker_start_altitude; - -struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING]; -uint8_t ao_tracker_head; -static uint8_t ao_tracker_log_pos; - -static uint16_t telem_rate; -static uint8_t gps_rate; -static uint8_t telem_enabled; - -static int64_t lat_sum, lon_sum; -static int32_t alt_sum; -static int nsamples; - -static void -ao_tracker_state_update(struct ao_tracker_data *tracker) -{ - uint16_t new_telem_rate; - uint8_t new_gps_rate; - uint8_t new_telem_enabled; - uint32_t ground_distance; - int16_t height; - uint16_t speed; - - new_gps_rate = gps_rate; - new_telem_rate = telem_rate; - - new_telem_enabled = ao_tracker_force_telem || !ao_usb_connected(); - - /* Don't change anything if GPS isn't locked */ - if ((tracker->new & AO_GPS_NEW_DATA) && - (tracker->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: - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - - /* startup to pad when GPS locks */ - - lat_sum += tracker->gps_data.latitude; - lon_sum += tracker->gps_data.longitude; - alt_sum += tracker->gps_data.altitude; - - ++nsamples; - - if (nsamples >= STARTUP_AVERAGE) { - ao_flight_state = ao_flight_pad; - ao_wakeup(&ao_flight_state); - ao_tracker_start_latitude = lat_sum / nsamples; - ao_tracker_start_longitude = lon_sum / nsamples; - ao_tracker_start_altitude = alt_sum / nsamples; - } - break; - case ao_flight_pad: - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - - ground_distance = ao_distance(tracker->gps_data.latitude, - tracker->gps_data.longitude, - ao_tracker_start_latitude, - ao_tracker_start_longitude); - height = tracker->gps_data.altitude - ao_tracker_start_altitude; - if (height < 0) - height = -height; - - if (ground_distance >= ao_config.tracker_start_horiz || - height >= ao_config.tracker_start_vert || - ao_tracker_force_launch) - { - ao_flight_state = ao_flight_drogue; - ao_wakeup(&ao_flight_state); - ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_head); - ao_log_start(); - ao_log_gps_flight(); - } - break; - case ao_flight_drogue: - /* Modulate data rates based on speed (in cm/s) */ - if (tracker->gps_data.climb_rate < 0) - speed = -tracker->gps_data.climb_rate; - else - speed = tracker->gps_data.climb_rate; - speed += tracker->gps_data.ground_speed; - - if (speed < AO_TRACKER_NOT_MOVING) { - new_telem_rate = AO_SEC_TO_TICKS(10); - new_gps_rate = 10; - } else { - new_telem_rate = AO_SEC_TO_TICKS(1); - new_gps_rate = 1; - } - break; - default: - break; - } - } - - if (new_telem_rate != telem_rate || new_telem_enabled != telem_enabled) { - if (new_telem_enabled) - ao_telemetry_set_interval(new_telem_rate); - else - ao_telemetry_set_interval(0); - telem_rate = new_telem_rate; - telem_enabled = new_telem_enabled; - } - - if (new_gps_rate != gps_rate) { - ao_gps_set_rate(new_gps_rate); - gps_rate = new_gps_rate; - } -} - -#if HAS_LOG -static uint8_t ao_tracker_should_log; - -static void -ao_tracker_log(void) -{ - struct ao_tracker_data *tracker; - - if (ao_log_running) { - while (ao_tracker_log_pos != ao_tracker_head) { - tracker = &ao_tracker_data[ao_tracker_log_pos]; - if (tracker->new & AO_GPS_NEW_DATA) { - ao_tracker_should_log = ao_log_gps_should_log(tracker->gps_data.latitude, - tracker->gps_data.longitude, - tracker->gps_data.altitude); - if (ao_tracker_should_log) - ao_log_gps_data(tracker->tick, tracker->state, &tracker->gps_data); - } - if (tracker->new & AO_GPS_NEW_TRACKING) { - if (ao_tracker_should_log) - ao_log_gps_tracking(tracker->tick, &tracker->gps_tracking_data); - } - ao_tracker_log_pos = ao_tracker_ring_next(ao_tracker_log_pos); - } - } -} -#endif +static int32_t last_log_latitude, last_log_longitude; +static int16_t last_log_altitude; +static uint8_t unmoving; +static uint8_t log_started; +static struct ao_telemetry_location gps_data; +static uint8_t tracker_running; +static uint16_t tracker_interval; static void ao_tracker(void) { - uint8_t new; - struct ao_tracker_data *tracker; + uint8_t new; + int32_t ground_distance; + int16_t height; + uint16_t gps_tick; + uint8_t new_tracker_running; #if HAS_ADC ao_timer_set_adc_interval(100); @@ -197,45 +59,83 @@ ao_tracker(void) #if !HAS_USB_CONNECT ao_tracker_force_telem = 1; #endif - - nsamples = 0; - lat_sum = 0; - lon_sum = 0; - alt_sum = 0; - ao_log_scan(); + ao_log_start(); ao_rdf_set(1); - ao_telemetry_set_interval(0); - telem_rate = AO_SEC_TO_TICKS(1); - telem_enabled = 0; - gps_rate = 1; - ao_flight_state = ao_flight_startup; + tracker_interval = ao_config.tracker_interval; + ao_gps_set_rate(tracker_interval); + for (;;) { + + /** Wait for new GPS data + */ while (!(new = ao_gps_new)) ao_sleep(&ao_gps_new); - - /* Stick GPS data into the ring */ ao_mutex_get(&ao_gps_mutex); - tracker = &ao_tracker_data[ao_tracker_head]; - tracker->tick = ao_gps_tick; - tracker->new = new; - tracker->state = ao_flight_state; - tracker->gps_data = ao_gps_data; - tracker->gps_tracking_data = ao_gps_tracking_data; - ao_tracker_head = ao_tracker_ring_next(ao_tracker_head); - + gps_data = ao_gps_data; + gps_tick = ao_gps_tick; ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - /* Update state based on current GPS data */ - ao_tracker_state_update(tracker); + new_tracker_running = ao_tracker_force_telem || !ao_usb_connected(); -#if HAS_LOG - /* Log all gps data */ - ao_tracker_log(); -#endif + if (ao_config.tracker_interval != tracker_interval) { + tracker_interval = ao_config.tracker_interval; + ao_gps_set_rate(tracker_interval); + + /* force telemetry interval to be reset */ + tracker_running = 0; + } + + 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 (!tracker_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) { + ground_distance = ao_distance(gps_data.latitude, gps_data.longitude, + last_log_latitude, last_log_longitude); + height = last_log_altitude - gps_data.altitude; + if (height < 0) + height = -height; + if (ground_distance <= ao_config.tracker_motion && + height <= ao_config.tracker_motion) + { + if (unmoving < AO_TRACKER_MOTION_COUNT) + unmoving++; + } else + unmoving = 0; + } + } 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; + } + 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; + } + } } } @@ -244,18 +144,23 @@ static struct ao_task ao_tracker_task; static void ao_tracker_set_telem(void) { - uint8_t telem, launch; + uint8_t telem; ao_cmd_hex(); telem = ao_cmd_lex_i; - ao_cmd_hex(); - launch = ao_cmd_lex_i; - if (ao_cmd_status == ao_cmd_success) { + if (ao_cmd_status == ao_cmd_success) ao_tracker_force_telem = telem; - ao_tracker_force_launch = launch; - } ao_cmd_status = ao_cmd_success; - printf ("flight %d force telem %d force launch %d\n", - ao_flight_number, ao_tracker_force_telem, ao_tracker_force_launch); + printf ("flight: %d\n", ao_flight_number); + printf ("force_telem: %d\n", ao_tracker_force_telem); + 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 ("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); + printf ("log_end_pos: %ld\n", (long) ao_log_end_pos); } static const struct ao_cmds ao_tracker_cmds[] = { diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h index 63bdaf2f..a0fd2f49 100644 --- a/src/kernel/ao_tracker.h +++ b/src/kernel/ao_tracker.h @@ -18,31 +18,12 @@ #ifndef _AO_TRACKER_H_ #define _AO_TRACKER_H_ -#define AO_CONFIG_DEFAULT_TRACKER_START_HORIZ 1000 -#define AO_CONFIG_DEFAULT_TRACKER_START_VERT 100 +/* Any motion more than this will result in a log entry */ -/* Speeds for the various modes, 2m/s seems reasonable for 'not moving' */ -#define AO_TRACKER_NOT_MOVING 200 +#define AO_TRACKER_MOTION_DEFAULT 10 +#define AO_TRACKER_INTERVAL_DEFAULT 1 -extern int32_t ao_tracker_start_latitude; -extern int32_t ao_tracker_start_longitude; -extern int16_t ao_tracker_start_altitude; - -#define AO_TRACKER_RING 4 - -struct ao_tracker_data { - uint16_t tick; - uint8_t new; - uint8_t state; - struct ao_telemetry_location gps_data; - struct ao_telemetry_satellite gps_tracking_data; -}; - -extern struct ao_tracker_data ao_tracker_data[AO_TRACKER_RING]; -extern uint8_t ao_tracker_head; - -#define ao_tracker_ring_next(n) (((n) + 1) & (AO_TRACKER_RING-1)) -#define ao_tracker_ring_prev(n) (((n) - 1) & (AO_TRACKER_RING-1)) +#define AO_TRACKER_MOTION_COUNT 10 void ao_tracker_init(void); diff --git a/src/telegps-v0.3/ao_telegps.c b/src/telegps-v0.3/ao_telegps.c index bc7eeea8..dd699ecf 100644 --- a/src/telegps-v0.3/ao_telegps.c +++ b/src/telegps-v0.3/ao_telegps.c @@ -52,7 +52,6 @@ main(void) ao_tracker_init(); ao_telemetry_init(); - ao_telemetry_set_interval(AO_SEC_TO_TICKS(1)); #if HAS_SAMPLE_PROFILE ao_sample_profile_init(); diff --git a/src/telegps-v1.0/ao_telegps.c b/src/telegps-v1.0/ao_telegps.c index 1185a5dd..7a71699b 100644 --- a/src/telegps-v1.0/ao_telegps.c +++ b/src/telegps-v1.0/ao_telegps.c @@ -55,7 +55,6 @@ main(void) ao_tracker_init(); ao_telemetry_init(); - ao_telemetry_set_interval(AO_SEC_TO_TICKS(1)); #if HAS_SAMPLE_PROFILE ao_sample_profile_init(); -- cgit v1.2.3 From 7e911c2afff78db2e385c6346c90bfcd72a8f3fb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 12 Jun 2014 14:34:02 -0700 Subject: altos/telegps: Don't log data when plugged in to USB We don't want to accidentally log stuff when you're just trying to charge the battery. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/kernel/ao_tracker.c') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index fb9e75d0..f0229230 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -91,8 +91,10 @@ ao_tracker(void) 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) { ao_telemetry_set_interval(0); + ao_log_stop(); } tracker_running = new_tracker_running; -- cgit v1.2.3 From dcaaf51245b44a440ee8590512f71195c30c16ae Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 12 Jun 2014 21:54:13 -0700 Subject: altos/telegps: Keep ring of recent GPS positions to detect motion quickly Instead of comparing only against the last logged value, keep a ring and start logging as soon as we move away from the furthest one in the ring. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.c | 74 ++++++++++++++++++++++++++++++------------------- 1 file changed, 45 insertions(+), 29 deletions(-) (limited to 'src/kernel/ao_tracker.c') diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index f0229230..a6ad87a3 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -35,9 +35,21 @@ ao_usb_connected(void) #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; + int16_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 log_started; static struct ao_telemetry_location gps_data; static uint8_t tracker_running; @@ -60,7 +72,6 @@ ao_tracker(void) ao_tracker_force_telem = 1; #endif ao_log_scan(); - ao_log_start(); ao_rdf_set(1); @@ -106,36 +117,41 @@ ao_tracker(void) 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; + + 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 - gps_data.altitude; if (height < 0) height = -height; - if (ground_distance <= ao_config.tracker_motion && - height <= ao_config.tracker_motion) + + if (ao_tracker_force_telem) + 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)) { - 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) { + printf ("moving %d\n", moving); + flush(); + } + if (moving) { + 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 = gps_data.altitude; + gps_head = ao_gps_ring_next(gps_head); } - 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; } } } @@ -154,8 +170,8 @@ ao_tracker_set_telem(void) 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); -- cgit v1.2.3 From f49540acd48292bd9f68ded647561d0e800c619d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 12 Jun 2014 23:59:37 -0700 Subject: altos/telegps: Create new flight if current flight is erased telegps is unique in that USB may be connected while a flight is active and sensible things should happen. If a flight is being recorded and gets erased, then a new flight should be started. This is done by hooking in the flight erase code and calling out to the tracker code to figure out whether to switch to a new flight or not. Signed-off-by: Keith Packard --- src/kernel/ao_log.c | 22 ++++++++++++++++------ src/kernel/ao_tracker.c | 43 ++++++++++++++++++++++++++++++++++++------- src/kernel/ao_tracker.h | 6 ++++++ 3 files changed, 58 insertions(+), 13 deletions(-) (limited to 'src/kernel/ao_tracker.c') diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c index d9c3e00f..91617d93 100644 --- a/src/kernel/ao_log.c +++ b/src/kernel/ao_log.c @@ -18,6 +18,9 @@ #include "ao.h" #include #include +#if HAS_TRACKER +#include +#endif __xdata uint8_t ao_log_mutex; __pdata uint32_t ao_log_current_pos; @@ -251,6 +254,7 @@ ao_log_delete(void) __reentrant { uint8_t slot; uint8_t slots; + uint32_t log_current_pos, log_end_pos; ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) @@ -261,10 +265,13 @@ ao_log_delete(void) __reentrant if (ao_cmd_lex_i) { for (slot = 0; slot < slots; slot++) { if (ao_log_flight(slot) == ao_cmd_lex_i) { +#if HAS_TRACKER + ao_tracker_erase_start(ao_cmd_lex_i); +#endif ao_log_erase_mark(); - ao_log_current_pos = ao_log_pos(slot); - ao_log_end_pos = ao_log_current_pos + ao_config.flight_log_max; - while (ao_log_current_pos < ao_log_end_pos) { + log_current_pos = ao_log_pos(slot); + log_end_pos = log_current_pos + ao_config.flight_log_max; + while (log_current_pos < log_end_pos) { uint8_t i; static __xdata uint8_t b; @@ -274,15 +281,18 @@ ao_log_delete(void) __reentrant * memory over and over again */ for (i = 0; i < 16; i++) { - if (ao_storage_read(ao_log_current_pos + i, &b, 1)) + if (ao_storage_read(log_current_pos + i, &b, 1)) if (b != 0xff) break; } if (i == 16) break; - ao_storage_erase(ao_log_current_pos); - ao_log_current_pos += ao_storage_block; + ao_storage_erase(log_current_pos); + log_current_pos += ao_storage_block; } +#if HAS_TRACKER + ao_tracker_erase_end(); +#endif puts("Erased"); return; } diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index a6ad87a3..9febc7f0 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -50,6 +50,7 @@ struct gps_position gps_position[GPS_RING]; 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; @@ -100,17 +101,19 @@ 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) { @@ -138,10 +141,11 @@ ao_tracker(void) } } if (ao_tracker_force_telem) { - printf ("moving %d\n", moving); + 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; @@ -151,12 +155,37 @@ ao_tracker(void) gps_position[gps_head].longitude = gps_data.longitude; gps_position[gps_head].altitude = gps_data.altitude; gps_head = ao_gps_ring_next(gps_head); + ao_mutex_put(&tracker_mutex); } } } } } +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) { + ao_log_scan(); + log_started = 0; + ao_mutex_put(&tracker_mutex); + } +} + static struct ao_task ao_tracker_task; static void diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h index a0fd2f49..78c40cb4 100644 --- a/src/kernel/ao_tracker.h +++ b/src/kernel/ao_tracker.h @@ -28,4 +28,10 @@ void ao_tracker_init(void); +void +ao_tracker_erase_start(uint16_t flight); + +void +ao_tracker_erase_end(void); + #endif /* _AO_TRACKER_H_ */ -- cgit v1.2.3