From 2625a464417c8475c66101757ca2c30cd6c74e0c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 21 May 2014 14:02:35 -0700 Subject: altos: Add config values for tracker start motion limits TeleGPS switches from 'pad' to 'drogue' states after the device moves a specified distance from the initial starting point. These values can be configured, and this is the configuration for them. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.h | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 src/kernel/ao_tracker.h (limited to 'src/kernel/ao_tracker.h') diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h new file mode 100644 index 00000000..43556965 --- /dev/null +++ b/src/kernel/ao_tracker.h @@ -0,0 +1,27 @@ +/* + * 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. + */ + +#ifndef _AO_TRACKER_H_ +#define _AO_TRACKER_H_ + +#define AO_CONFIG_DEFAULT_TRACKER_START_HORIZ 1000 +#define AO_CONFIG_DEFAULT_TRACKER_START_VERT 100 + +void +ao_tracker_init(void); + +#endif /* _AO_TRACKER_H_ */ -- 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.h') 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 d165079b9275c69e727a1dac996ad1788c58ed40 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 7 Jun 2014 21:11:33 -0700 Subject: altos: Reduce tracker GPS buffer to 4 samples We just don't have enough RAM for 8 samples. Signed-off-by: Keith Packard --- src/kernel/ao_tracker.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/kernel/ao_tracker.h') diff --git a/src/kernel/ao_tracker.h b/src/kernel/ao_tracker.h index 1adf0f33..63bdaf2f 100644 --- a/src/kernel/ao_tracker.h +++ b/src/kernel/ao_tracker.h @@ -28,7 +28,7 @@ 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 +#define AO_TRACKER_RING 4 struct ao_tracker_data { uint16_t tick; -- 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.h') 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 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.h') 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