diff options
Diffstat (limited to 'src/kernel')
35 files changed, 791 insertions, 228 deletions
diff --git a/src/kernel/ao.h b/src/kernel/ao.h index 5ff9b518..ad5bbf8e 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -132,6 +132,9 @@ ao_clock_init(void); */ #ifndef ao_mutex_get +uint8_t +ao_mutex_try(__xdata uint8_t *ao_mutex, uint8_t task_id) __reentrant; + void ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant; @@ -275,15 +278,17 @@ ao_report_init(void); * Given raw data, convert to SI units */ +#if HAS_BARO /* pressure from the sensor to altitude in meters */ -int16_t -ao_pres_to_altitude(int16_t pres) __reentrant; +alt_t +ao_pres_to_altitude(pres_t pres) __reentrant; -int16_t -ao_altitude_to_pres(int16_t alt) __reentrant; +pres_t +ao_altitude_to_pres(alt_t alt) __reentrant; int16_t ao_temp_to_dC(int16_t temp) __reentrant; +#endif /* * ao_convert_pa.c @@ -293,11 +298,13 @@ ao_temp_to_dC(int16_t temp) __reentrant; #include <ao_data.h> +#if HAS_BARO alt_t -ao_pa_to_altitude(int32_t pa); +ao_pa_to_altitude(pres_t pa); int32_t ao_altitude_to_pa(alt_t alt); +#endif #if HAS_DBG #include <ao_dbg.h> @@ -522,6 +529,9 @@ struct ao_telemetry_raw_recv { #endif void +ao_telemetry_reset_interval(void); + +void ao_telemetry_set_interval(uint16_t interval); void @@ -558,6 +568,15 @@ extern __xdata int8_t ao_radio_rssi; #define HAS_RADIO_XMIT HAS_RADIO #endif +#define AO_RADIO_RATE_38400 0 +#define AO_RADIO_RATE_9600 1 +#define AO_RADIO_RATE_2400 2 +#define AO_RADIO_RATE_MAX AO_RADIO_RATE_2400 + +#if defined(HAS_RADIO) && !defined(HAS_RADIO_RATE) +#define HAS_RADIO_RATE HAS_RADIO +#endif + void ao_radio_general_isr(void) ao_arch_interrupt(16); @@ -757,11 +776,13 @@ extern __xdata uint8_t ao_force_freq; * ao_rssi.c */ +#ifdef AO_LED_TYPE void -ao_rssi_set(int rssi_value); +ao_rssi_set(int16_t rssi_value); void -ao_rssi_init(uint8_t rssi_led); +ao_rssi_init(AO_LED_TYPE rssi_led); +#endif /* * ao_product.c diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c index 0052bdce..d2f583ef 100644 --- a/src/kernel/ao_cmd.c +++ b/src/kernel/ao_cmd.c @@ -290,6 +290,9 @@ version(void) #if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND) "program-space %u\n" #endif +#if AO_VALUE_32 + "altitude-32 1\n" +#endif , ao_manufacturer , ao_product , ao_serial_number diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 71445335..6b8a1813 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -50,17 +50,25 @@ __xdata uint8_t ao_config_mutex; #error Please define USE_INTERNAL_FLASH #endif #endif + #ifndef AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX -#if USE_INTERNAL_FLASH -#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ao_storage_config -#else -#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 192 * (uint32_t) 1024) -#endif +# if FLIGHT_LOG_APPEND +# define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ao_storage_log_max +# else +# if USE_INTERNAL_FLASH +# define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ao_storage_config +# else +# define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 192 * (uint32_t) 1024) +# endif +# endif #endif + #ifndef AO_CONFIG_DEFAULT_RADIO_POWER #define AO_CONFIG_DEFAULT_RADIO_POWER 0x60 #endif #define AO_CONFIG_DEFAULT_RADIO_AMP 0 +#define AO_CONFIG_DEFAULT_APRS_SSID (ao_serial_number % 10) +#define AO_CONFIG_DEFAULT_RADIO_RATE AO_RADIO_RATE_38400 #if HAS_EEPROM static void @@ -85,10 +93,18 @@ ao_config_put(void) #endif #if HAS_RADIO + +#if HAS_RADIO_FORWARD +__xdata uint32_t ao_send_radio_setting; +#endif + void ao_config_set_radio(void) { ao_config.radio_setting = ao_freq_to_set(ao_config.frequency, ao_config.radio_cal); +#if HAS_RADIO_FORWARD + ao_send_radio_setting = ao_freq_to_set(ao_config.send_frequency, ao_config.radio_cal); +#endif } #endif /* HAS_RADIO */ @@ -192,6 +208,18 @@ _ao_config_get(void) if (minor < 18) ao_config.pyro_time = AO_CONFIG_DEFAULT_PYRO_TIME; #endif +#if HAS_APRS + if (minor < 19) + ao_config.aprs_ssid = AO_CONFIG_DEFAULT_APRS_SSID; +#endif +#if HAS_RADIO_RATE + if (minor < 20) + ao_config.radio_rate = AO_CONFIG_DEFAULT_RADIO_RATE; +#endif +#if HAS_RADIO_FORWARD + if (minor < 21) + ao_config.send_frequency = 434550; +#endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; } @@ -283,6 +311,32 @@ ao_config_frequency_set(void) __reentrant ao_radio_recv_abort(); #endif } + +#endif + +#if HAS_RADIO_FORWARD +void +ao_config_send_frequency_show(void) __reentrant +{ + printf("Send frequency: %ld\n", + ao_config.send_frequency); +} + +void +ao_config_send_frequency_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.send_frequency = ao_cmd_lex_u32; + ao_config_set_radio(); + _ao_config_edit_finish(); +#if HAS_RADIO_RECV + ao_radio_recv_abort(); +#endif +} + #endif #if HAS_FLIGHT @@ -477,18 +531,73 @@ ao_config_radio_cal_set(void) __reentrant ao_config_set_radio(); _ao_config_edit_finish(); } + +#endif + +#if HAS_RADIO_RATE +#ifndef HAS_TELEMETRY +#error Please define HAS_TELEMETRY +#endif + +void +ao_config_radio_rate_show(void) __reentrant +{ + printf("Telemetry rate: %d\n", ao_config.radio_rate); +} + +void +ao_config_radio_rate_set(void) __reentrant +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + if (AO_RADIO_RATE_MAX < ao_cmd_lex_i) { + ao_cmd_status = ao_cmd_lex_error; + return; + } + _ao_config_edit_start(); + ao_config.radio_rate = ao_cmd_lex_i; +#if HAS_TELEMETRY + ao_telemetry_reset_interval(); +#endif + _ao_config_edit_finish(); +#if HAS_RADIO_RECV + ao_radio_recv_abort(); +#endif +} #endif #if HAS_LOG + void ao_config_log_show(void) __reentrant { printf("Max flight log: %d kB\n", (int16_t) (ao_config.flight_log_max >> 10)); +#if FLIGHT_LOG_APPEND + printf("Log fixed: 1\n"); +#endif } +#if FLIGHT_LOG_APPEND +void +ao_config_log_fix_append(void) +{ + _ao_config_edit_start(); + ao_config.flight_log_max = ao_storage_log_max; + _ao_config_edit_finish(); + ao_mutex_get(&ao_config_mutex); + _ao_config_put(); + ao_config_dirty = 0; + ao_mutex_put(&ao_config_mutex); +} +#endif + void ao_config_log_set(void) __reentrant { +#if FLIGHT_LOG_APPEND + printf("Flight log fixed size %d kB\n", ao_storage_log_max >> 10); +#else uint16_t block = (uint16_t) (ao_storage_block >> 10); uint16_t log_max = (uint16_t) (ao_storage_log_max >> 10); @@ -506,6 +615,7 @@ ao_config_log_set(void) __reentrant ao_config.flight_log_max = (uint32_t) ao_cmd_lex_i << 10; _ao_config_edit_finish(); } +#endif } #endif /* HAS_LOG */ @@ -737,6 +847,30 @@ ao_config_pyro_time_set(void) } #endif +#if HAS_APRS +void +ao_config_aprs_ssid_show(void) +{ + printf ("APRS SSID: %d\n", + ao_config.aprs_ssid); +} + +void +ao_config_aprs_ssid_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + if (15 < ao_cmd_lex_i) { + ao_cmd_status = ao_cmd_lex_error; + return; + } + _ao_config_edit_start(); + ao_config.aprs_ssid = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif /* HAS_APRS */ + struct ao_config_var { __code char *str; void (*set)(void) __reentrant; @@ -766,12 +900,20 @@ __code struct ao_config_var ao_config_vars[] = { #if HAS_RADIO { "F <freq>\0Frequency (kHz)", ao_config_frequency_set, ao_config_frequency_show }, +#if HAS_RADIO_FORWARD + { "R <freq>\0Repeater output frequency (kHz)", + ao_config_send_frequency_set, ao_config_send_frequency_show }, +#endif { "c <call>\0Callsign (8 char max)", ao_config_callsign_set, ao_config_callsign_show }, { "e <0 disable, 1 enable>\0Enable telemetry and RDF", ao_config_radio_enable_set, ao_config_radio_enable_show }, { "f <cal>\0Radio calib (cal = rf/(xtal/2^16))", ao_config_radio_cal_set, ao_config_radio_cal_show }, +#if HAS_RADIO_RATE + { "T <rate>\0Telemetry rate (0=38.4, 1=9.6, 2=2.4)", + ao_config_radio_rate_set, ao_config_radio_rate_show }, +#endif #if HAS_RADIO_POWER { "p <setting>\0Radio power setting (0-255)", ao_config_radio_power_set, ao_config_radio_power_show }, @@ -817,6 +959,10 @@ __code struct ao_config_var ao_config_vars[] = { { "t <motion> <interval>\0Tracker configuration", ao_config_tracker_set, ao_config_tracker_show }, #endif +#if HAS_APRS + { "S <ssid>\0Set APRS SSID (0-15)", + ao_config_aprs_ssid_set, ao_config_aprs_ssid_show }, +#endif { "s\0Show", ao_config_show, 0 }, #if HAS_EEPROM diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index 2b5cd352..164584a5 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -28,6 +28,10 @@ #define USE_EEPROM_CONFIG 0 #endif +#ifndef FLIGHT_LOG_APPEND +#define FLIGHT_LOG_APPEND 0 +#endif + #if USE_STORAGE_CONFIG #include <ao_storage.h> @@ -53,7 +57,7 @@ #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 18 +#define AO_CONFIG_MINOR 21 #define AO_AES_LEN 16 @@ -102,8 +106,21 @@ struct ao_config { #if AO_PYRO_NUM uint16_t pyro_time; /* minor version 18 */ #endif +#if HAS_APRS + uint8_t aprs_ssid; /* minor version 19 */ +#endif +#if HAS_RADIO_RATE + uint8_t radio_rate; /* minor version 20 */ +#endif +#if HAS_RADIO_FORWARD + uint32_t send_frequency; /* minor version 21 */ +#endif }; +#if HAS_RADIO_FORWARD +extern __xdata uint32_t ao_send_radio_setting; +#endif + #define AO_IGNITE_MODE_DUAL 0 #define AO_IGNITE_MODE_APOGEE 1 #define AO_IGNITE_MODE_MAIN 2 @@ -141,6 +158,9 @@ void ao_config_set_radio(void); void +ao_config_log_fix_append(void); + +void ao_config_init(void); #endif /* _AO_CONFIG_H_ */ diff --git a/src/kernel/ao_convert.c b/src/kernel/ao_convert.c index aa9b5f48..db1f2301 100644 --- a/src/kernel/ao_convert.c +++ b/src/kernel/ao_convert.c @@ -19,14 +19,16 @@ #include "ao.h" #endif -static const int16_t altitude_table[] = { +#include <ao_sample.h> + +static const ao_v_t altitude_table[] = { #include "altitude.h" }; #define ALT_FRAC_SCALE (1 << ALT_FRAC_BITS) #define ALT_FRAC_MASK (ALT_FRAC_SCALE - 1) -int16_t +ao_v_t ao_pres_to_altitude(int16_t pres) __reentrant { uint8_t o; @@ -43,9 +45,9 @@ ao_pres_to_altitude(int16_t pres) __reentrant #if AO_NEED_ALTITUDE_TO_PRES int16_t -ao_altitude_to_pres(int16_t alt) __reentrant +ao_altitude_to_pres(ao_v_t alt) __reentrant { - int16_t span, sub_span; + ao_v_t span, sub_span; uint8_t l, h, m; int32_t pres; diff --git a/src/kernel/ao_convert_pa.c b/src/kernel/ao_convert_pa.c index fe6e0ef6..410815b6 100644 --- a/src/kernel/ao_convert_pa.c +++ b/src/kernel/ao_convert_pa.c @@ -24,7 +24,11 @@ #endif static const alt_t altitude_table[] AO_CONST_ATTRIB = { +#if AO_SMALL_ALTITUDE_TABLE +#include "altitude-pa-small.h" +#else #include "altitude-pa.h" +#endif }; #ifndef FETCH_ALT @@ -35,11 +39,11 @@ static const alt_t altitude_table[] AO_CONST_ATTRIB = { #define ALT_MASK (ALT_SCALE - 1) alt_t -ao_pa_to_altitude(int32_t pa) +ao_pa_to_altitude(pres_t pa) { int16_t o; int16_t part; - int32_t low, high; + alt_t low, high; if (pa < 0) pa = 0; @@ -48,16 +52,16 @@ ao_pa_to_altitude(int32_t pa) o = pa >> ALT_SHIFT; part = pa & ALT_MASK; - low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part); - high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); + low = (alt_t) FETCH_ALT(o) * (ALT_SCALE - part); + high = (alt_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); return (low + high) >> ALT_SHIFT; } #ifdef AO_CONVERT_TEST -int32_t -ao_altitude_to_pa(int32_t alt) +pres_t +ao_altitude_to_pa(alt_t alt) { - int32_t span, sub_span; + alt_t span, sub_span; uint16_t l, h, m; int32_t pa; @@ -72,7 +76,7 @@ ao_altitude_to_pa(int32_t alt) } span = altitude_table[l] - altitude_table[h]; sub_span = altitude_table[l] - alt; - pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span; + pa = ((((alt_t) l * (span - sub_span) + (alt_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span; if (pa > 120000) pa = 120000; if (pa < 0) diff --git a/src/kernel/ao_convert_pa_test.c b/src/kernel/ao_convert_pa_test.c index 7d5b1922..95422862 100644 --- a/src/kernel/ao_convert_pa_test.c +++ b/src/kernel/ao_convert_pa_test.c @@ -18,6 +18,7 @@ #include <stdint.h> #define AO_CONVERT_TEST typedef int32_t alt_t; +typedef int32_t pres_t; #include "ao_host.h" #include "ao_convert_pa.c" diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h index c4b062fd..8f75ad87 100644 --- a/src/kernel/ao_data.h +++ b/src/kernel/ao_data.h @@ -117,9 +117,7 @@ extern volatile __data uint8_t ao_data_count; typedef int32_t pres_t; -#ifndef AO_ALT_TYPE #define AO_ALT_TYPE int32_t -#endif typedef AO_ALT_TYPE alt_t; @@ -146,10 +144,6 @@ typedef int16_t alt_t; #endif -#if !HAS_BARO -typedef int16_t alt_t; -#endif - /* * Need a few macros to pull data from the sensors: * diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 2b433ee9..251dbc02 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -60,10 +60,10 @@ __xdata uint8_t ao_sensor_errors; * resting */ static __data uint16_t ao_interval_end; -static __data int16_t ao_interval_min_height; -static __data int16_t ao_interval_max_height; +static __data ao_v_t ao_interval_min_height; +static __data ao_v_t ao_interval_max_height; #if HAS_ACCEL -static __data int16_t ao_coast_avg_accel; +static __data ao_v_t ao_coast_avg_accel; #endif __pdata uint8_t ao_flight_force_idle; @@ -398,14 +398,14 @@ ao_flight(void) } #if HAS_FLIGHT_DEBUG -static inline int int_part(int16_t i) { return i >> 4; } -static inline int frac_part(int16_t i) { return ((i & 0xf) * 100 + 8) / 16; } +static inline int int_part(ao_v_t i) { return i >> 4; } +static inline int frac_part(ao_v_t i) { return ((i & 0xf) * 100 + 8) / 16; } static void ao_flight_dump(void) { #if HAS_ACCEL - int16_t accel; + ao_v_t accel; accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; #endif diff --git a/src/kernel/ao_forward.c b/src/kernel/ao_forward.c new file mode 100644 index 00000000..2a937183 --- /dev/null +++ b/src/kernel/ao_forward.c @@ -0,0 +1,48 @@ +/* + * Copyright © 2014 Keith Packard <keithp@keithp.com> + * + * 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 "ao.h" +#include "ao_telem.h" + +static void +ao_monitor_forward(void) +{ + uint32_t recv_radio_setting; + static __xdata struct ao_telemetry_all_recv packet; + + for (;;) { + while (ao_monitoring) + ao_sleep(DATA_TO_XDATA(&ao_monitoring)); + + if (!ao_radio_recv(&packet, sizeof(packet), 0)) + continue; + if (!(packet.status & PKT_APPEND_STATUS_1_CRC_OK)) + continue; + recv_radio_setting = ao_config.radio_setting; + ao_config.radio_setting = ao_send_radio_setting; + ao_radio_send(&packet.telemetry, sizeof (packet.telemetry)); + ao_config.radio_setting = recv_radio_setting; + } +} + +static __xdata struct ao_task ao_monitor_forward_task; + +void +ao_monitor_forward_init(void) __reentrant +{ + ao_add_task(&ao_monitor_forward_task, ao_monitor_forward, "monitor_forward"); +} diff --git a/src/kernel/ao_forward.h b/src/kernel/ao_forward.h new file mode 100644 index 00000000..1897dc08 --- /dev/null +++ b/src/kernel/ao_forward.h @@ -0,0 +1,24 @@ +/* + * Copyright © 2014 Keith Packard <keithp@keithp.com> + * + * 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_FORWARD_H_ +#define _AO_FORWARD_H_ + +void +ao_monitor_forward_init(void) __reentrant; + +#endif /* _AO_FORWARD_H_ */ diff --git a/src/kernel/ao_gps_print.c b/src/kernel/ao_gps_print.c index 47c945d7..d26021da 100644 --- a/src/kernel/ao_gps_print.c +++ b/src/kernel/ao_gps_print.c @@ -20,6 +20,10 @@ #endif #include "ao_telem.h" +#ifndef AO_TELEMETRY_LOCATION_ALTITUDE +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((l)->altitude) +#endif + void ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant { @@ -42,7 +46,7 @@ ao_gps_print(__xdata struct ao_gps_orig *gps_data) __reentrant AO_TELEM_GPS_ALTITUDE " %d ", (long) gps_data->latitude, (long) gps_data->longitude, - gps_data->altitude); + AO_TELEMETRY_LOCATION_ALTITUDE(gps_data)); if (gps_data->flags & AO_GPS_DATE_VALID) printf(AO_TELEM_GPS_YEAR " %d " diff --git a/src/kernel/ao_gps_report.c b/src/kernel/ao_gps_report.c index 07201ac2..7ef98a97 100644 --- a/src/kernel/ao_gps_report.c +++ b/src/kernel/ao_gps_report.c @@ -52,8 +52,12 @@ ao_gps_report(void) gps_log.u.gps_longitude = gps_data.longitude; ao_log_data(&gps_log); gps_log.type = AO_LOG_GPS_ALT; - gps_log.u.gps_altitude.altitude = gps_data.altitude; - gps_log.u.gps_altitude.unused = 0xffff; + gps_log.u.gps_altitude.altitude_low = gps_data.altitude_low; +#if HAS_WIDE_GPS + gps_log.u.gps_altitude.altitude_high = gps_data.altitude_high; +#else + gps_log.u.gps_altitude.altitude_high = 0xffff; +#endif ao_log_data(&gps_log); if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) { gps_log.type = AO_LOG_GPS_DATE; diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c index cb0c0fd9..f3711fb1 100644 --- a/src/kernel/ao_gps_report_mega.c +++ b/src/kernel/ao_gps_report_mega.c @@ -78,7 +78,8 @@ ao_gps_report_mega(void) #if GPS_SPARSE_LOG /* Don't log data if GPS has a fix and hasn't moved for a while */ if ((gps_data.flags & AO_GPS_VALID) && - !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude, gps_data.altitude)) + !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude, + AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data)) continue; #endif if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { @@ -87,8 +88,8 @@ ao_gps_report_mega(void) gps_log.type = AO_LOG_GPS_TIME; gps_log.u.gps.latitude = gps_data.latitude; gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; - + gps_log.u.gps.altitude_low = gps_data.altitude_low; + gps_log.u.gps.altitude_high = gps_data.altitude_high; gps_log.u.gps.hour = gps_data.hour; gps_log.u.gps.minute = gps_data.minute; gps_log.u.gps.second = gps_data.second; diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c index 696a833b..8ce074fe 100644 --- a/src/kernel/ao_gps_report_metrum.c +++ b/src/kernel/ao_gps_report_metrum.c @@ -44,7 +44,8 @@ ao_gps_report_metrum(void) gps_log.type = AO_LOG_GPS_POS; gps_log.u.gps.latitude = gps_data.latitude; gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; + gps_log.u.gps.altitude_low = gps_data.altitude_low; + gps_log.u.gps.altitude_high = gps_data.altitude_high; ao_log_metrum(&gps_log); gps_log.type = AO_LOG_GPS_TIME; @@ -55,6 +56,7 @@ ao_gps_report_metrum(void) gps_log.u.gps_time.year = gps_data.year; gps_log.u.gps_time.month = gps_data.month; gps_log.u.gps_time.day = gps_data.day; + gps_log.u.gps_time.pdop = gps_data.pdop; ao_log_metrum(&gps_log); } diff --git a/src/kernel/ao_gps_show.c b/src/kernel/ao_gps_show.c index 3a05e35a..e45cd795 100644 --- a/src/kernel/ao_gps_show.c +++ b/src/kernel/ao_gps_show.c @@ -19,6 +19,8 @@ #include <ao.h> #endif +#include <ao_data.h> + void ao_gps_show(void) __reentrant { @@ -27,7 +29,11 @@ ao_gps_show(void) __reentrant printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day); printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude); - printf ("Alt: %d\n", ao_gps_data.altitude); +#if HAS_WIDE_GPS + printf ("Alt: %ld\n", (long) AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data)); +#else + printf ("Alt: %d\n", AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data)); +#endif printf ("Flags: 0x%x\n", ao_gps_data.flags); printf ("Sats: %d", ao_gps_tracking_data.channels); for (i = 0; i < ao_gps_tracking_data.channels; i++) diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 9aea1f14..7b0f8145 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -23,32 +23,31 @@ #include "ao_sample.h" #include "ao_kalman.h" +static __pdata ao_k_t ao_k_height; +static __pdata ao_k_t ao_k_speed; +static __pdata ao_k_t ao_k_accel; -static __pdata int32_t ao_k_height; -static __pdata int32_t ao_k_speed; -static __pdata int32_t ao_k_accel; +#define AO_K_STEP_100 to_fix_v(0.01) +#define AO_K_STEP_2_2_100 to_fix_v(0.00005) -#define AO_K_STEP_100 to_fix16(0.01) -#define AO_K_STEP_2_2_100 to_fix16(0.00005) +#define AO_K_STEP_10 to_fix_v(0.1) +#define AO_K_STEP_2_2_10 to_fix_v(0.005) -#define AO_K_STEP_10 to_fix16(0.1) -#define AO_K_STEP_2_2_10 to_fix16(0.005) +#define AO_K_STEP_1 to_fix_v(1) +#define AO_K_STEP_2_2_1 to_fix_v(0.5) -#define AO_K_STEP_1 to_fix16(1) -#define AO_K_STEP_2_2_1 to_fix16(0.5) +__pdata ao_v_t ao_height; +__pdata ao_v_t ao_speed; +__pdata ao_v_t ao_accel; +__xdata ao_v_t ao_max_height; +static __pdata ao_k_t ao_avg_height_scaled; +__xdata ao_v_t ao_avg_height; -__pdata int16_t ao_height; -__pdata int16_t ao_speed; -__pdata int16_t ao_accel; -__xdata int16_t ao_max_height; -static __pdata int32_t ao_avg_height_scaled; -__xdata int16_t ao_avg_height; - -__pdata int16_t ao_error_h; -__pdata int16_t ao_error_h_sq_avg; +__pdata ao_v_t ao_error_h; +__pdata ao_v_t ao_error_h_sq_avg; #if HAS_ACCEL -__pdata int16_t ao_error_a; +__pdata ao_v_t ao_error_a; #endif static void @@ -56,40 +55,40 @@ ao_kalman_predict(void) { #ifdef AO_FLIGHT_TEST if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 + - (int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1; + ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_1 + + (ao_k_t) ao_accel * AO_K_STEP_2_2_1) >> 4; + ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_1; return; } if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 + - (int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10; + ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_10 + + (ao_k_t) ao_accel * AO_K_STEP_2_2_10) >> 4; + ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_10; return; } if (ao_flight_debug) { printf ("predict speed %g + (%g * %g) = %g\n", ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0, - (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); + (ao_k_speed + (ao_k_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); } #endif - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 + - (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100; + ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_100 + + (ao_k_t) ao_accel * AO_K_STEP_2_2_100) >> 4; + ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100; } static void ao_kalman_err_height(void) { - int16_t e; - int16_t height_distrust; + ao_v_t e; + ao_v_t height_distrust; #if HAS_ACCEL - int16_t speed_distrust; + ao_v_t speed_distrust; #endif - ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16); + ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16); e = ao_error_h; if (e < 0) @@ -123,7 +122,7 @@ ao_kalman_err_height(void) #endif if (height_distrust > 0x100) height_distrust = 0x100; - ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8); + ao_error_h = (ao_v_t) (((ao_k_t) ao_error_h * (0x100 - height_distrust)) >> 8); #ifdef AO_FLIGHT_TEST if (ao_flight_debug) { printf("over height %g over speed %g distrust: %g height: error %d -> %d\n", @@ -142,21 +141,21 @@ ao_kalman_correct_baro(void) ao_kalman_err_height(); #ifdef AO_FLIGHT_TEST if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_1 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_1 * ao_error_h; + ao_k_height += (ao_k_t) AO_BARO_K0_1 * ao_error_h; + ao_k_speed += (ao_k_t) AO_BARO_K1_1 * ao_error_h; + ao_k_accel += (ao_k_t) AO_BARO_K2_1 * ao_error_h; return; } if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_10 * ao_error_h; + ao_k_height += (ao_k_t) AO_BARO_K0_10 * ao_error_h; + ao_k_speed += (ao_k_t) AO_BARO_K1_10 * ao_error_h; + ao_k_accel += (ao_k_t) AO_BARO_K2_10 * ao_error_h; return; } #endif - ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_100 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_100 * ao_error_h; + ao_k_height += (ao_k_t) AO_BARO_K0_100 * ao_error_h; + ao_k_speed += (ao_k_t) AO_BARO_K1_100 * ao_error_h; + ao_k_accel += (ao_k_t) AO_BARO_K2_100 * ao_error_h; } #if HAS_ACCEL @@ -164,7 +163,7 @@ ao_kalman_correct_baro(void) static void ao_kalman_err_accel(void) { - int32_t accel; + ao_k_t accel; accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; @@ -187,18 +186,18 @@ ao_kalman_correct_both(void) (double) ao_error_h, AO_BOTH_K10_1 / 65536.0, (double) ao_error_a, AO_BOTH_K11_1 / 65536.0, (ao_k_speed + - (int32_t) AO_BOTH_K10_1 * ao_error_h + - (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0)); + (ao_k_t) AO_BOTH_K10_1 * ao_error_h + + (ao_k_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0)); } ao_k_height += - (int32_t) AO_BOTH_K00_1 * ao_error_h + - (int32_t) AO_BOTH_K01_1 * ao_error_a; + (ao_k_t) AO_BOTH_K00_1 * ao_error_h + + (ao_k_t) AO_BOTH_K01_1 * ao_error_a; ao_k_speed += - (int32_t) AO_BOTH_K10_1 * ao_error_h + - (int32_t) AO_BOTH_K11_1 * ao_error_a; + (ao_k_t) AO_BOTH_K10_1 * ao_error_h + + (ao_k_t) AO_BOTH_K11_1 * ao_error_a; ao_k_accel += - (int32_t) AO_BOTH_K20_1 * ao_error_h + - (int32_t) AO_BOTH_K21_1 * ao_error_a; + (ao_k_t) AO_BOTH_K20_1 * ao_error_h + + (ao_k_t) AO_BOTH_K21_1 * ao_error_a; return; } if (ao_sample_tick - ao_sample_prev_tick > 5) { @@ -208,18 +207,18 @@ ao_kalman_correct_both(void) (double) ao_error_h, AO_BOTH_K10_10 / 65536.0, (double) ao_error_a, AO_BOTH_K11_10 / 65536.0, (ao_k_speed + - (int32_t) AO_BOTH_K10_10 * ao_error_h + - (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); + (ao_k_t) AO_BOTH_K10_10 * ao_error_h + + (ao_k_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); } ao_k_height += - (int32_t) AO_BOTH_K00_10 * ao_error_h + - (int32_t) AO_BOTH_K01_10 * ao_error_a; + (ao_k_t) AO_BOTH_K00_10 * ao_error_h + + (ao_k_t) AO_BOTH_K01_10 * ao_error_a; ao_k_speed += - (int32_t) AO_BOTH_K10_10 * ao_error_h + - (int32_t) AO_BOTH_K11_10 * ao_error_a; + (ao_k_t) AO_BOTH_K10_10 * ao_error_h + + (ao_k_t) AO_BOTH_K11_10 * ao_error_a; ao_k_accel += - (int32_t) AO_BOTH_K20_10 * ao_error_h + - (int32_t) AO_BOTH_K21_10 * ao_error_a; + (ao_k_t) AO_BOTH_K20_10 * ao_error_h + + (ao_k_t) AO_BOTH_K21_10 * ao_error_a; return; } if (ao_flight_debug) { @@ -228,19 +227,19 @@ ao_kalman_correct_both(void) (double) ao_error_h, AO_BOTH_K10_100 / 65536.0, (double) ao_error_a, AO_BOTH_K11_100 / 65536.0, (ao_k_speed + - (int32_t) AO_BOTH_K10_100 * ao_error_h + - (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); + (ao_k_t) AO_BOTH_K10_100 * ao_error_h + + (ao_k_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); } #endif ao_k_height += - (int32_t) AO_BOTH_K00_100 * ao_error_h + - (int32_t) AO_BOTH_K01_100 * ao_error_a; + (ao_k_t) AO_BOTH_K00_100 * ao_error_h + + (ao_k_t) AO_BOTH_K01_100 * ao_error_a; ao_k_speed += - (int32_t) AO_BOTH_K10_100 * ao_error_h + - (int32_t) AO_BOTH_K11_100 * ao_error_a; + (ao_k_t) AO_BOTH_K10_100 * ao_error_h + + (ao_k_t) AO_BOTH_K11_100 * ao_error_a; ao_k_accel += - (int32_t) AO_BOTH_K20_100 * ao_error_h + - (int32_t) AO_BOTH_K21_100 * ao_error_a; + (ao_k_t) AO_BOTH_K20_100 * ao_error_h + + (ao_k_t) AO_BOTH_K21_100 * ao_error_a; } #else @@ -251,14 +250,14 @@ ao_kalman_correct_accel(void) ao_kalman_err_accel(); if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a; - ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a; - ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a; + ao_k_height +=(ao_k_t) AO_ACCEL_K0_10 * ao_error_a; + ao_k_speed += (ao_k_t) AO_ACCEL_K1_10 * ao_error_a; + ao_k_accel += (ao_k_t) AO_ACCEL_K2_10 * ao_error_a; return; } - ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a; - ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a; - ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a; + ao_k_height += (ao_k_t) AO_ACCEL_K0_100 * ao_error_a; + ao_k_speed += (ao_k_t) AO_ACCEL_K1_100 * ao_error_a; + ao_k_accel += (ao_k_t) AO_ACCEL_K2_100 * ao_error_a; } #endif /* else FORCE_ACCEL */ diff --git a/src/kernel/ao_log.c b/src/kernel/ao_log.c index 91617d93..40a96ef7 100644 --- a/src/kernel/ao_log.c +++ b/src/kernel/ao_log.c @@ -142,19 +142,39 @@ ao_log_max_flight(void) return max_flight; } -void -ao_log_scan(void) __reentrant +static void +ao_log_erase(uint8_t slot) __reentrant { - uint8_t log_slot; - uint8_t log_slots; - uint8_t log_want; + uint32_t log_current_pos, log_end_pos; - ao_config_get(); + ao_log_erase_mark(); + 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; + + /* + * Check to see if we've reached the end of + * the used memory to avoid re-erasing the same + * memory over and over again + */ + for (i = 0; i < 16; i++) { + if (ao_storage_read(log_current_pos + i, &b, 1)) + if (b != 0xff) + break; + } + if (i == 16) + break; + ao_storage_erase(log_current_pos); + log_current_pos += ao_storage_block; + } +} - ao_flight_number = ao_log_max_flight(); - if (ao_flight_number) - if (++ao_flight_number == 0) - ao_flight_number = 1; +static void +ao_log_find_max_erase_flight(void) __reentrant +{ + uint8_t log_slot; /* Now look through the log of flight numbers from erase operations and * see if the last one is bigger than what we found above @@ -170,6 +190,80 @@ ao_log_scan(void) __reentrant } if (ao_flight_number == 0) ao_flight_number = 1; +} + +uint8_t +ao_log_scan(void) __reentrant +{ + uint8_t log_slot; + uint8_t log_slots; +#if FLIGHT_LOG_APPEND + uint8_t ret; +#else + uint8_t log_want; +#endif + + ao_config_get(); + + /* Get any existing flight number */ + ao_flight_number = ao_log_max_flight(); + +#if FLIGHT_LOG_APPEND + + /* Deal with older OS versions which stored multiple + * flights in rom by erasing everything after the first + * slot + */ + if (ao_config.flight_log_max != ao_storage_log_max) { + log_slots = ao_log_slots(); + for (log_slot = 1; log_slot < log_slots; log_slot++) { + if (ao_log_flight(log_slot) != 0) + ao_log_erase(log_slot); + } + ao_config_log_fix_append(); + } + ao_log_current_pos = ao_log_pos(0); + ao_log_end_pos = ao_log_current_pos + ao_storage_log_max; + + if (ao_flight_number) { + uint32_t full = ao_log_current_pos; + uint32_t empty = ao_log_end_pos - ao_log_size; + + /* If there's already a flight started, then find the + * end of it + */ + for (;;) { + ao_log_current_pos = (full + empty) >> 1; + ao_log_current_pos -= ao_log_current_pos % ao_log_size; + + if (ao_log_current_pos == full) { + if (ao_log_check(ao_log_current_pos)) + ao_log_current_pos += ao_log_size; + break; + } + if (ao_log_current_pos == empty) + break; + + if (ao_log_check(ao_log_current_pos)) { + full = ao_log_current_pos; + } else { + empty = ao_log_current_pos; + } + } + ret = 1; + } else { + ao_log_find_max_erase_flight(); + ret = 0; + } + ao_wakeup(&ao_flight_number); + return ret; +#else + + if (ao_flight_number) + if (++ao_flight_number == 0) + ao_flight_number = 1; + + ao_log_find_max_erase_flight(); /* With a flight number in hand, find a place to write a new log, * use the target flight number to index the available log slots so @@ -190,8 +284,9 @@ ao_log_scan(void) __reentrant if (++log_slot >= log_slots) log_slot = 0; } while (log_slot != log_want); - ao_wakeup(&ao_flight_number); + return 0; +#endif } void @@ -254,7 +349,6 @@ 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) @@ -268,28 +362,7 @@ ao_log_delete(void) __reentrant #if HAS_TRACKER ao_tracker_erase_start(ao_cmd_lex_i); #endif - ao_log_erase_mark(); - 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; - - /* - * Check to see if we've reached the end of - * the used memory to avoid re-erasing the same - * memory over and over again - */ - for (i = 0; i < 16; i++) { - if (ao_storage_read(log_current_pos + i, &b, 1)) - if (b != 0xff) - break; - } - if (i == 16) - break; - ao_storage_erase(log_current_pos); - log_current_pos += ao_storage_block; - } + ao_log_erase(slot); #if HAS_TRACKER ao_tracker_erase_end(); #endif diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h index 33cda3eb..c13a2580 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -51,11 +51,16 @@ extern __pdata enum ao_flight_state ao_log_state; #define AO_LOG_FORMAT_NONE 127 /* No log at all */ extern __code uint8_t ao_log_format; +extern __code uint8_t ao_log_size; /* Return the flight number from the given log slot, 0 if none */ uint16_t ao_log_flight(uint8_t slot); +/* Check if there is valid log data at the specified location */ +uint8_t +ao_log_check(uint32_t pos); + /* Flush the log */ void ao_log_flush(void); @@ -67,7 +72,7 @@ ao_log(void); /* functions provided in ao_log.c */ /* Figure out the current flight number */ -void +uint8_t ao_log_scan(void) __reentrant; /* Return the position of the start of the given log slot */ @@ -176,8 +181,8 @@ struct ao_log_record { int32_t gps_latitude; int32_t gps_longitude; struct { - int16_t altitude; - uint16_t unused; + uint16_t altitude_low; + int16_t altitude_high; } gps_altitude; struct { uint16_t svid; @@ -246,7 +251,7 @@ struct ao_log_mega { struct { int32_t latitude; /* 4 */ int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ + uint16_t altitude_low; /* 12 */ uint8_t hour; /* 14 */ uint8_t minute; /* 15 */ uint8_t second; /* 16 */ @@ -261,7 +266,8 @@ struct ao_log_mega { uint8_t hdop; /* 27 */ uint8_t vdop; /* 28 */ uint8_t mode; /* 29 */ - } gps; /* 30 */ + int16_t altitude_high; /* 30 */ + } gps; /* 32 */ /* AO_LOG_GPS_SAT */ struct { uint16_t channels; /* 4 */ @@ -273,6 +279,11 @@ struct ao_log_mega { } u; }; +#define AO_LOG_MEGA_GPS_ALTITUDE(l) ((int32_t) ((l)->u.gps.altitude_high << 16) | ((l)->u.gps.altitude_low)) +#define AO_LOG_MEGA_SET_GPS_ALTITUDE(l,a) (((l)->u.gps.mode |= AO_GPS_MODE_ALTITUDE_24), \ + ((l)->u.gps.altitude_high = (a) >> 16), \ + (l)->u.gps.altitude_low = (a)) + struct ao_log_metrum { char type; /* 0 */ uint8_t csum; /* 1 */ @@ -306,8 +317,9 @@ struct ao_log_metrum { struct { int32_t latitude; /* 4 */ int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ - } gps; /* 14 */ + uint16_t altitude_low; /* 12 */ + int16_t altitude_high; /* 14 */ + } gps; /* 16 */ /* AO_LOG_GPS_TIME */ struct { uint8_t hour; /* 4 */ @@ -317,7 +329,7 @@ struct ao_log_metrum { uint8_t year; /* 8 */ uint8_t month; /* 9 */ uint8_t day; /* 10 */ - uint8_t pad; /* 11 */ + uint8_t pdop; /* 11 */ } gps_time; /* 12 */ /* AO_LOG_GPS_SAT (up to three packets) */ struct { @@ -381,7 +393,7 @@ struct ao_log_gps { struct { int32_t latitude; /* 4 */ int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ + uint16_t altitude_low; /* 12 */ uint8_t hour; /* 14 */ uint8_t minute; /* 15 */ uint8_t second; /* 16 */ @@ -396,7 +408,7 @@ struct ao_log_gps { uint8_t hdop; /* 27 */ uint8_t vdop; /* 28 */ uint8_t mode; /* 29 */ - uint8_t state; /* 30 */ + int16_t altitude_high; /* 30 */ } gps; /* 31 */ /* AO_LOG_GPS_SAT */ struct { diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c index 3b728c25..7643091c 100644 --- a/src/kernel/ao_log_gps.c +++ b/src/kernel/ao_log_gps.c @@ -26,6 +26,7 @@ static __xdata struct ao_log_gps log; __code uint8_t ao_log_format = AO_LOG_FORMAT_TELEGPS; +__code uint8_t ao_log_size = sizeof (struct ao_log_gps); static uint8_t ao_log_csum(__xdata uint8_t *b) __reentrant @@ -75,7 +76,8 @@ ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data) log.type = AO_LOG_GPS_TIME; log.u.gps.latitude = gps_data->latitude; log.u.gps.longitude = gps_data->longitude; - log.u.gps.altitude = gps_data->altitude; + log.u.gps.altitude_low = gps_data->altitude_low; + log.u.gps.altitude_high = gps_data->altitude_high; log.u.gps.hour = gps_data->hour; log.u.gps.minute = gps_data->minute; @@ -135,3 +137,16 @@ ao_log_flight(uint8_t slot) return log.u.flight.flight; return 0; } + +uint8_t +ao_log_check(uint32_t pos) +{ + if (!ao_storage_read(pos, + &log, + sizeof (struct ao_log_gps))) + return 0; + + if (ao_log_dump_check_data()) + return 1; + return 0; +} diff --git a/src/kernel/ao_log_gps.h b/src/kernel/ao_log_gps.h index 5851f4d1..a9e8c831 100644 --- a/src/kernel/ao_log_gps.h +++ b/src/kernel/ao_log_gps.h @@ -21,9 +21,6 @@ #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); - void ao_log_gps_flight(void); diff --git a/src/kernel/ao_log_micro.c b/src/kernel/ao_log_micro.c index d665efb5..aef78def 100644 --- a/src/kernel/ao_log_micro.c +++ b/src/kernel/ao_log_micro.c @@ -22,10 +22,17 @@ static uint16_t ao_log_offset = STARTING_LOG_OFFSET; +#define AO_LOG_ID_SHIFT 12 +#define AO_LOG_ID_MASK ((1 << AO_LOG_ID_SHIFT) - 1) + void ao_log_micro_save(void) { uint16_t n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t); + +#if AO_LOG_ID + n_samples |= AO_LOG_ID << AO_LOG_ID_SHIFT; +#endif ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); @@ -99,8 +106,12 @@ ao_log_micro_dump(void) uint16_t crc = 0xffff; ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); + if (n_samples == 0xffff) n_samples = 0; +#if AO_LOG_ID + n_samples &= AO_LOG_ID_MASK; +#endif nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples; ao_async_start(); ao_async_byte('M'); diff --git a/src/kernel/ao_microflight.c b/src/kernel/ao_microflight.c index f680e400..f7521f21 100644 --- a/src/kernel/ao_microflight.c +++ b/src/kernel/ao_microflight.c @@ -43,10 +43,12 @@ void ao_microflight(void) { int16_t sample_count; + int16_t log_count; uint16_t time; uint32_t pa_interval_min, pa_interval_max; int32_t pa_diff; - uint8_t h, i; + uint8_t h; + uint8_t i; uint8_t accel_lock = 0; uint32_t pa_sum = 0; @@ -60,19 +62,25 @@ ao_microflight(void) h = 0; for (;;) { time += SAMPLE_SLEEP; +#if BOOST_DETECT if ((sample_count & 0x1f) == 0) ao_led_on(AO_LED_REPORT); +#endif ao_delay_until(time); ao_microsample(); +#if BOOST_DETECT if ((sample_count & 0x1f) == 0) ao_led_off(AO_LED_REPORT); +#endif pa_hist[h] = pa; h = SKIP_PA_HIST(h,1); pa_diff = pa_ground - ao_pa; +#if BOOST_DETECT /* Check for a significant pressure change */ if (pa_diff > BOOST_DETECT) break; +#endif if (sample_count < GROUND_AVG * 2) { if (sample_count < GROUND_AVG) @@ -82,6 +90,9 @@ ao_microflight(void) pa_ground = pa_sum >> GROUND_AVG_SHIFT; pa_sum = 0; sample_count = 0; +#if !BOOST_DETECT + break; +#endif } } @@ -93,7 +104,10 @@ ao_microflight(void) } /* Log the remaining samples so we get a complete history since leaving the ground */ - for (; i != h; i = SKIP_PA_HIST(i,2)) { +#if LOG_INTERVAL < NUM_PA_HIST + for (; i != h; i = SKIP_PA_HIST(i,2)) +#endif + { pa = pa_hist[i]; ao_log_micro_data(); } @@ -101,6 +115,7 @@ ao_microflight(void) /* Now sit around until the pressure is stable again and record the max */ sample_count = 0; + log_count = 0; pa_min = ao_pa; pa_interval_min = ao_pa; pa_interval_max = ao_pa; @@ -112,8 +127,11 @@ ao_microflight(void) ao_microsample(); if ((sample_count & 3) == 0) ao_led_off(AO_LED_REPORT); - if (sample_count & 1) + if (log_count == LOG_INTERVAL - 1) { ao_log_micro_data(); + log_count = 0; + } else + log_count++; /* If accelerating upwards, don't look for min pressure */ if (ao_pa_accel < ACCEL_LOCK_PA) diff --git a/src/kernel/ao_microkalman.c b/src/kernel/ao_microkalman.c index 0684ea2b..75a29cc4 100644 --- a/src/kernel/ao_microkalman.c +++ b/src/kernel/ao_microkalman.c @@ -22,19 +22,19 @@ #define FIX_BITS 16 -#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) -#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define to_fix_v(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix_k(x) ((int32_t) ((x) * 65536.0 + 0.5)) #define from_fix8(x) ((x) >> 8) #define from_fix(x) ((x) >> 16) -#define fix8_to_fix16(x) ((x) << 8) +#define fix8_to_fix_v(x) ((x) << 8) #define fix16_to_fix8(x) ((x) >> 8) #include <ao_kalman.h> /* Basic time step (96ms) */ -#define AO_MK_STEP to_fix16(0.096) +#define AO_MK_STEP to_fix_v(0.096) /* step ** 2 / 2 */ -#define AO_MK_STEP_2_2 to_fix16(0.004608) +#define AO_MK_STEP_2_2 to_fix_v(0.004608) uint32_t ao_k_pa; /* 24.8 fixed point */ int32_t ao_k_pa_speed; /* 16.16 fixed point */ @@ -49,7 +49,7 @@ ao_microkalman_init(void) { ao_pa = pa; ao_k_pa = pa << 8; -} +} void ao_microkalman_predict(void) diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c index 18f170b4..2d75c41c 100644 --- a/src/kernel/ao_monitor.c +++ b/src/kernel/ao_monitor.c @@ -115,6 +115,8 @@ ao_monitor_put(void) { #if LEGACY_MONITOR __xdata char callsign[AO_MAX_CALLSIGN+1]; +#endif +#if LEGACY_MONITOR || HAS_RSSI int16_t rssi; #endif uint8_t ao_monitor_tail; @@ -240,7 +242,7 @@ ao_monitor_put(void) hex(sum); putchar ('\n'); #if HAS_RSSI - if (recv_raw.packet[ao_monitoring + 1] & PKT_APPEND_STATUS_1_CRC_OK) { + if (recv_raw.packet[ao_monitoring + 1] & AO_RADIO_STATUS_CRC_OK) { rssi = AO_RSSI_FROM_RADIO(recv_raw.packet[ao_monitoring]); ao_rssi_set(rssi); } diff --git a/src/kernel/ao_mutex.c b/src/kernel/ao_mutex.c index 952ff462..a36fe939 100644 --- a/src/kernel/ao_mutex.c +++ b/src/kernel/ao_mutex.c @@ -17,6 +17,29 @@ #include "ao.h" +#ifndef HAS_MUTEX_TRY +#define HAS_MUTEX_TRY 1 +#endif + +#if HAS_MUTEX_TRY + +uint8_t +ao_mutex_try(__xdata uint8_t *mutex, uint8_t task_id) __reentrant +{ + uint8_t ret; + if (*mutex == task_id) + ao_panic(AO_PANIC_MUTEX); + ao_arch_critical( + if (*mutex) + ret = 0; + else { + *mutex = task_id; + ret = 1; + }); + return ret; +} +#endif + void ao_mutex_get(__xdata uint8_t *mutex) __reentrant { diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 85d88d98..3044d565 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -69,6 +69,16 @@ ao_pyro_print_status(void) uint16_t ao_pyro_fired; +#ifndef PYRO_DBG +#define PYRO_DBG 0 +#endif + +#if PYRO_DBG +#define DBG(...) do { printf("\t%d: ", (int) (pyro - ao_config.pyro)); printf(__VA_ARGS__); } while (0) +#else +#define DBG(...) +#endif + /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -88,63 +98,73 @@ ao_pyro_ready(struct ao_pyro *pyro) case ao_pyro_accel_less: if (ao_accel <= pyro->accel_less) continue; + DBG("accel %d > %d\n", ao_accel, pyro->accel_less); break; case ao_pyro_accel_greater: if (ao_accel >= pyro->accel_greater) continue; + DBG("accel %d < %d\n", ao_accel, pyro->accel_greater); break; - - case ao_pyro_speed_less: if (ao_speed <= pyro->speed_less) continue; + DBG("speed %d > %d\n", ao_speed, pyro->speed_less); break; case ao_pyro_speed_greater: if (ao_speed >= pyro->speed_greater) continue; + DBG("speed %d < %d\n", ao_speed, pyro->speed_greater); break; - case ao_pyro_height_less: if (ao_height <= pyro->height_less) continue; + DBG("height %d > %d\n", ao_height, pyro->height_less); break; case ao_pyro_height_greater: if (ao_height >= pyro->height_greater) continue; + DBG("height %d < %d\n", ao_height, pyro->height_greater); break; #if HAS_GYRO case ao_pyro_orient_less: if (ao_sample_orient <= pyro->orient_less) continue; + DBG("orient %d > %d\n", ao_sample_orient, pyro->orient_less); break; case ao_pyro_orient_greater: if (ao_sample_orient >= pyro->orient_greater) continue; + DBG("orient %d < %d\n", ao_sample_orient, pyro->orient_greater); break; #endif case ao_pyro_time_less: if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) continue; + DBG("time %d > %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_less); break; case ao_pyro_time_greater: if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) continue; + DBG("time %d < %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_greater); break; case ao_pyro_ascending: if (ao_speed > 0) continue; + DBG("not ascending speed %d\n", ao_speed); break; case ao_pyro_descending: if (ao_speed < 0) continue; + DBG("not descending speed %d\n", ao_speed); break; case ao_pyro_after_motor: if (ao_motor_number == pyro->motor) continue; + DBG("motor %d != %d\n", ao_motor_number, pyro->motor); break; case ao_pyro_delay: @@ -154,10 +174,12 @@ ao_pyro_ready(struct ao_pyro *pyro) case ao_pyro_state_less: if (ao_flight_state < pyro->state_less) continue; + DBG("state %d >= %d\n", ao_flight_state, pyro->state_less); break; case ao_pyro_state_greater_or_equal: if (ao_flight_state >= pyro->state_greater_or_equal) continue; + DBG("state %d >= %d\n", ao_flight_state, pyro->state_less); break; default: @@ -230,7 +252,7 @@ ao_pyro_check(void) struct ao_pyro *pyro; uint8_t p, any_waiting; uint16_t fire = 0; - + any_waiting = 0; for (p = 0; p < AO_PYRO_NUM; p++) { pyro = &ao_config.pyro[p]; @@ -266,6 +288,16 @@ ao_pyro_check(void) * the delay to expire */ if (pyro->delay_done) { + + /* Check to make sure the required conditions + * remain valid. If not, inhibit the channel + * by setting the fired bit + */ + if (!ao_pyro_ready(pyro)) { + pyro->fired = 1; + continue; + } + if ((int16_t) (ao_time() - pyro->delay_done) < 0) continue; } @@ -443,7 +475,7 @@ ao_pyro_set(void) printf ("invalid pyro channel %d\n", p); return; } - pyro_tmp.flags = 0; + memset(&pyro_tmp, '\0', sizeof (pyro_tmp)); for (;;) { ao_cmd_white(); if (ao_cmd_lex_c == '\n') @@ -467,13 +499,26 @@ ao_pyro_set(void) } pyro_tmp.flags |= ao_pyro_values[v].flag; if (ao_pyro_values[v].offset != NO_VALUE) { + uint8_t negative = 0; + ao_cmd_white(); + if (ao_cmd_lex_c == '-') { + negative = 1; + ao_cmd_lex(); + } ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) + if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) { + if (negative) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } *((uint8_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; - else + } else { + if (negative) + ao_cmd_lex_i = -ao_cmd_lex_i; *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + } } } _ao_config_edit_start(); diff --git a/src/kernel/ao_pyro.h b/src/kernel/ao_pyro.h index b37aaeb1..0fde725a 100644 --- a/src/kernel/ao_pyro.h +++ b/src/kernel/ao_pyro.h @@ -83,6 +83,9 @@ ao_pyro_init(void); void ao_pyro_manual(uint8_t p); +enum ao_igniter_status +ao_pyro_status(uint8_t p); + void ao_pyro_print_status(void); diff --git a/src/kernel/ao_report.c b/src/kernel/ao_report.c index f2263154..5314fc8f 100644 --- a/src/kernel/ao_report.c +++ b/src/kernel/ao_report.c @@ -211,12 +211,23 @@ ao_report_continuity(void) __reentrant pause(AO_MS_TO_TICKS(100)); } } else { - c = 10; + c = 5; while (c--) { high(AO_MS_TO_TICKS(20)); low(AO_MS_TO_TICKS(20)); } } +#if AO_PYRO_NUM + pause(AO_MS_TO_TICKS(250)); + for(c = 0; c < AO_PYRO_NUM; c++) { + enum ao_igniter_status status = ao_pyro_status(c); + if (status == ao_igniter_ready) + mid(AO_MS_TO_TICKS(25)); + else + low(AO_MS_TO_TICKS(25)); + pause(AO_MS_TO_TICKS(200)); + } +#endif #if HAS_LOG if (ao_log_full()) { pause(AO_MS_TO_TICKS(100)); diff --git a/src/kernel/ao_rssi.c b/src/kernel/ao_rssi.c index 244a84fe..793b190b 100644 --- a/src/kernel/ao_rssi.c +++ b/src/kernel/ao_rssi.c @@ -17,9 +17,9 @@ #include "ao.h" -static __xdata uint16_t ao_rssi_time; -static __pdata uint16_t ao_rssi_delay; -static __pdata uint8_t ao_rssi_led; +static __xdata uint16_t ao_rssi_time; +static __pdata uint16_t ao_rssi_delay; +static __pdata AO_LED_TYPE ao_rssi_led; void ao_rssi(void) @@ -33,7 +33,7 @@ ao_rssi(void) } void -ao_rssi_set(int rssi_value) +ao_rssi_set(int16_t rssi_value) { if (rssi_value > 0) rssi_value = 0; @@ -45,7 +45,7 @@ ao_rssi_set(int rssi_value) __xdata struct ao_task ao_rssi_task; void -ao_rssi_init(uint8_t rssi_led) +ao_rssi_init(AO_LED_TYPE rssi_led) { ao_rssi_led = rssi_led; ao_rssi_delay = 0; diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 34658951..29bf2bf6 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -245,7 +245,7 @@ ao_sample_preflight(void) } else { #if HAS_ACCEL ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; - ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g; + ao_accel_scale = to_fix_32(GRAVITY * 2 * 16) / ao_accel_2g; #endif ao_sample_preflight_set(); ao_preflight = FALSE; diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index 16d4c507..2ec998bd 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -24,6 +24,24 @@ * ao_sample.c */ +#ifndef AO_VALUE_32 +#define AO_VALUE_32 1 +#endif + +#if AO_VALUE_32 +/* + * For 32-bit computed values, use 64-bit intermediates. + */ +typedef int64_t ao_k_t; +typedef int32_t ao_v_t; +#else +/* + * For 16-bit computed values, use 32-bit intermediates. + */ +typedef int32_t ao_k_t; +typedef int16_t ao_v_t; +#endif + /* * Barometer calibration * @@ -87,9 +105,9 @@ * 2047m/s² (over 200g) */ -#define AO_M_TO_HEIGHT(m) ((int16_t) (m)) -#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) -#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) +#define AO_M_TO_HEIGHT(m) ((ao_v_t) (m)) +#define AO_MS_TO_SPEED(ms) ((ao_v_t) ((ms) * 16)) +#define AO_MSS_TO_ACCEL(mss) ((ao_v_t) ((mss) * 16)) extern __pdata uint16_t ao_sample_tick; /* time of last data */ extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */ @@ -134,21 +152,33 @@ uint8_t ao_sample(void); * ao_kalman.c */ -#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) -#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define to_fix_16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix_32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define to_fix_64(x) ((int64_t) ((x) * 65536.0 + 0.5)) + +#ifdef AO_VALUE_32 +#if AO_VALUE_32 +#define to_fix_v(x) to_fix_32(x) +#define to_fix_k(x) to_fix_64(x) +#else +#define to_fix_v(x) to_fix_16(x) +#define to_fix_k(x) to_fix_32(x) +#endif + #define from_fix(x) ((x) >> 16) -extern __pdata int16_t ao_height; /* meters */ -extern __pdata int16_t ao_speed; /* m/s * 16 */ -extern __pdata int16_t ao_accel; /* m/s² * 16 */ -extern __xdata int16_t ao_max_height; /* max of ao_height */ -extern __xdata int16_t ao_avg_height; /* running average of height */ +extern __pdata ao_v_t ao_height; /* meters */ +extern __pdata ao_v_t ao_speed; /* m/s * 16 */ +extern __pdata ao_v_t ao_accel; /* m/s² * 16 */ +extern __xdata ao_v_t ao_max_height; /* max of ao_height */ +extern __xdata ao_v_t ao_avg_height; /* running average of height */ -extern __pdata int16_t ao_error_h; -extern __pdata int16_t ao_error_h_sq_avg; +extern __pdata ao_v_t ao_error_h; +extern __pdata ao_v_t ao_error_h_sq_avg; #if HAS_ACCEL -extern __pdata int16_t ao_error_a; +extern __pdata ao_v_t ao_error_a; +#endif #endif void ao_kalman(void); diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index 9f778b09..56bd715e 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -1,5 +1,5 @@ /* - * Copyright © 2011 Keith Packard <keithp@keithp.com> + * Copyright © 2011 Keth Packard <keithp@keithp.com> * * 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 @@ -25,6 +25,10 @@ static __pdata uint16_t ao_telemetry_interval; +#if HAS_RADIO_RATE +static __xdata uint16_t ao_telemetry_desired_interval; +#endif + #if HAS_RDF static __pdata uint8_t ao_rdf = 0; static __pdata uint16_t ao_rdf_time; @@ -64,7 +68,7 @@ static void ao_send_sensor(void) { __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - + telemetry.generic.tick = packet->tick; telemetry.generic.type = AO_TELEMETRY_SENSOR; @@ -106,12 +110,13 @@ ao_send_sensor(void) #ifdef AO_SEND_MEGA + /* Send mega sensor packet */ static void ao_send_mega_sensor(void) { __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - + telemetry.generic.tick = packet->tick; telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; @@ -240,7 +245,7 @@ static void ao_send_mini(void) { __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; - + telemetry.generic.tick = packet->tick; telemetry.generic.type = AO_TELEMETRY_MINI; @@ -339,7 +344,7 @@ ao_send_location(void) ao_mutex_get(&ao_gps_mutex); ao_xmemcpy(&telemetry.location.flags, &ao_gps_data.flags, - 26); + 27); telemetry.location.tick = ao_gps_tick; ao_mutex_put(&ao_gps_mutex); ao_radio_send(&telemetry, sizeof (telemetry)); @@ -490,12 +495,33 @@ ao_telemetry(void) } } +#if HAS_RADIO_RATE +void +ao_telemetry_reset_interval(void) +{ + ao_telemetry_set_interval(ao_telemetry_desired_interval); +} +#endif + void ao_telemetry_set_interval(uint16_t interval) { int8_t cur = 0; + +#if HAS_RADIO_RATE + /* Limit max telemetry rate based on available radio bandwidth. + */ + static __xdata const uint16_t min_interval[] = { + /* [AO_RADIO_RATE_38400] = */ AO_MS_TO_TICKS(100), + /* [AO_RADIO_RATE_9600] = */ AO_MS_TO_TICKS(500), + /* [AO_RADIO_RATE_2400] = */ AO_MS_TO_TICKS(1000) + }; + + ao_telemetry_desired_interval = interval; + if (interval < min_interval[ao_config.radio_rate]) + interval = min_interval[ao_config.radio_rate]; +#endif ao_telemetry_interval = interval; - #if AO_SEND_MEGA if (interval > 1) ao_telemetry_mega_data_max = 1; @@ -521,7 +547,7 @@ ao_telemetry_set_interval(uint16_t interval) ao_telemetry_companion_cur = cur; #endif - ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval; + ao_telemetry_config_max = AO_SEC_TO_TICKS(5) / interval; #if HAS_COMPANION if (ao_telemetry_config_max > cur) cur++; diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h index be7d0340..83d432cf 100644 --- a/src/kernel/ao_telemetry.h +++ b/src/kernel/ao_telemetry.h @@ -86,12 +86,9 @@ struct ao_telemetry_configuration { #define AO_TELEMETRY_LOCATION 0x05 -#define AO_GPS_MODE_NOT_VALID 'N' -#define AO_GPS_MODE_AUTONOMOUS 'A' -#define AO_GPS_MODE_DIFFERENTIAL 'D' -#define AO_GPS_MODE_ESTIMATED 'E' -#define AO_GPS_MODE_MANUAL 'M' -#define AO_GPS_MODE_SIMULATED 'S' +/* Mode bits */ + +#define AO_GPS_MODE_ALTITUDE_24 (1 << 0) /* reports 24-bits of altitude */ struct ao_telemetry_location { uint16_t serial; /* 0 */ @@ -99,7 +96,7 @@ struct ao_telemetry_location { uint8_t type; /* 4 */ uint8_t flags; /* 5 Number of sats and other flags */ - int16_t altitude; /* 6 GPS reported altitude (m) */ + uint16_t altitude_low; /* 6 GPS reported altitude (m) */ int32_t latitude; /* 8 latitude (degrees * 10⁷) */ int32_t longitude; /* 12 longitude (degrees * 10⁷) */ uint8_t year; /* 16 (- 2000) */ @@ -115,10 +112,31 @@ struct ao_telemetry_location { uint16_t ground_speed; /* 26 cm/s */ int16_t climb_rate; /* 28 cm/s */ uint8_t course; /* 30 degrees / 2 */ - uint8_t unused; /* 31 unused */ + int8_t altitude_high; /* 31 high byte of altitude */ /* 32 */ }; +#if HAS_GPS + +#ifndef HAS_WIDE_GPS +#define HAS_WIDE_GPS 1 +#endif + +#if HAS_WIDE_GPS +typedef int32_t gps_alt_t; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) (((gps_alt_t) (l)->altitude_high << 16) | ((l)->altitude_low)) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->mode |= AO_GPS_MODE_ALTITUDE_24), \ + ((l)->altitude_high = (a) >> 16), \ + ((l)->altitude_low = (a))) +#else +typedef int16_t gps_alt_t; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((gps_alt_t) (l)->altitude_low) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->mode = 0, \ + (l)->altitude_low = (a))) +#endif /* HAS_WIDE_GPS */ + +#endif /* HAS_GPS */ + #define AO_TELEMETRY_SATELLITE 0x06 struct ao_telemetry_satellite_info { diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index 9febc7f0..9b007af8 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -36,9 +36,9 @@ ao_usb_connected(void) #endif struct gps_position { - int32_t latitude; - int32_t longitude; - int16_t altitude; + int32_t latitude; + int32_t longitude; + gps_alt_t altitude; }; #define GPS_RING 16 @@ -72,7 +72,7 @@ ao_tracker(void) #if !HAS_USB_CONNECT ao_tracker_force_telem = 1; #endif - ao_log_scan(); + log_started = ao_log_scan(); ao_rdf_set(1); @@ -122,12 +122,13 @@ ao_tracker(void) { uint8_t ring; uint8_t moving = 0; + gps_alt_t altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data); for (ring = ao_gps_ring_next(gps_head); ring != gps_head; ring = ao_gps_ring_next(ring)) { ground_distance = ao_distance(gps_data.latitude, gps_data.longitude, gps_position[ring].latitude, gps_position[ring].longitude); - height = gps_position[ring].altitude - gps_data.altitude; + height = gps_position[ring].altitude - altitude; if (height < 0) height = -height; @@ -153,7 +154,7 @@ ao_tracker(void) 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_position[gps_head].altitude = altitude; gps_head = ao_gps_ring_next(gps_head); ao_mutex_put(&tracker_mutex); } @@ -180,8 +181,7 @@ void ao_tracker_erase_end(void) { if (erasing_current) { - ao_log_scan(); - log_started = 0; + log_started = ao_log_scan(); ao_mutex_put(&tracker_mutex); } } @@ -203,7 +203,7 @@ ao_tracker_set_telem(void) printf ("log_started: %d\n", log_started); 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); |