From: Keith Packard Date: Sat, 7 Jun 2014 14:41:11 +0000 (-0700) Subject: altos: Move ao_tracker.c to kernel X-Git-Tag: 1.3.2.3~17 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=1873d539a8f1a0e1e8ad539af5d49a77a129b928 altos: Move ao_tracker.c to kernel Doesn't make sense to be in product Signed-off-by: Keith Packard --- 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"); +} diff --git a/src/product/ao_tracker.c b/src/product/ao_tracker.c deleted file mode 100644 index b207c562..00000000 --- a/src/product/ao_tracker.c +++ /dev/null @@ -1,212 +0,0 @@ -/* - * 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"); -}