From 34d5be68ca23e8beb05db9a480faef63ecc911d0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 10 Jul 2014 17:07:48 -0700 Subject: altos: Extend GPS altitudes to at least 24 bits everywhere Telemetry gets a special 'mode' flag indicating that 24-bit data is present; log files get new data and log readers are expected to detect that via the firmware version number. Signed-off-by: Keith Packard --- src/kernel/ao_gps_report.c | 8 ++++++-- src/kernel/ao_gps_report_mega.c | 7 ++++--- src/kernel/ao_gps_report_metrum.c | 3 ++- src/kernel/ao_gps_show.c | 8 +++++++- src/kernel/ao_log.h | 23 +++++++++++++++-------- src/kernel/ao_log_gps.c | 3 ++- src/kernel/ao_log_gps.h | 3 --- src/kernel/ao_telemetry.c | 2 +- src/kernel/ao_telemetry.h | 34 ++++++++++++++++++++++++++-------- src/kernel/ao_tracker.c | 13 +++++++------ 10 files changed, 70 insertions(+), 34 deletions(-) (limited to 'src/kernel') 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..31939385 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; 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 #endif +#include + 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_log.h b/src/kernel/ao_log.h index 33cda3eb..080cfb02 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -176,8 +176,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 +246,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 +261,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 +274,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 +312,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 */ @@ -381,7 +388,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 +403,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..a5a6358b 100644 --- a/src/kernel/ao_log_gps.c +++ b/src/kernel/ao_log_gps.c @@ -75,7 +75,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; 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_telemetry.c b/src/kernel/ao_telemetry.c index d321c8ff..56bd715e 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -344,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)); 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..d9434048 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 @@ -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); } @@ -203,7 +204,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); -- cgit v1.2.3 From 31ae24b5da3e198e7555ea3768d3cbdec3a28a5f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 10 Jul 2014 17:15:09 -0700 Subject: altos: Record pdop value in TeleMetrumV2 log There's only one byte free, so we'll record the pdop value Signed-off-by: Keith Packard --- src/kernel/ao_gps_report_metrum.c | 1 + src/kernel/ao_log.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) (limited to 'src/kernel') diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c index 31939385..8ce074fe 100644 --- a/src/kernel/ao_gps_report_metrum.c +++ b/src/kernel/ao_gps_report_metrum.c @@ -56,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_log.h b/src/kernel/ao_log.h index 080cfb02..da20e067 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -324,7 +324,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 { -- cgit v1.2.3 From 013e9ccfbe76dc46e8c69ea314950bed83d9a39f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 10 Jul 2014 17:18:38 -0700 Subject: altos: Use 32-bits for flight state data (alt/speed/accel) Stores 32-bits for all of the flight parameters. Uses 64-bit intermediates for kalman computation. Signed-off-by: Keith Packard --- src/Makefile | 2 +- src/cc1111/ao_pins.h | 1 + src/kalman/kalman.5c | 2 +- src/kalman/kalman_micro.5c | 2 +- src/kernel/ao.h | 14 ++-- src/kernel/ao_cmd.c | 3 + src/kernel/ao_convert.c | 10 +-- src/kernel/ao_convert_pa.c | 16 ++--- src/kernel/ao_convert_pa_test.c | 1 + src/kernel/ao_data.h | 6 -- src/kernel/ao_flight.c | 12 ++-- src/kernel/ao_kalman.c | 147 ++++++++++++++++++++-------------------- src/kernel/ao_microkalman.c | 12 ++-- src/kernel/ao_sample.c | 2 +- src/kernel/ao_sample.h | 56 +++++++++++---- src/product/Makefile.telemetrum | 1 + src/telefire-v0.1/Makefile | 1 - src/telefire-v0.2/Makefile | 1 - src/telemega-v1.0/Makefile | 1 + src/telemini-v2.0/ao_pins.h | 1 + src/teleterra-v0.2/ao_pins.h | 2 + src/test/ao_flight_test.c | 5 +- src/test/ao_micropeak_test.c | 1 + 23 files changed, 170 insertions(+), 129 deletions(-) (limited to 'src/kernel') diff --git a/src/Makefile b/src/Makefile index a7a26b26..3494ba62 100644 --- a/src/Makefile +++ b/src/Makefile @@ -109,7 +109,7 @@ altitude-pa.h: make-altitude-pa altitude-pa-small.h: make-altitude-pa nickle $< --sample 3 > $@ -ao_kalman.h: make-kalman kalman.5c kalman_filter.5c load_csv.5c matrix.5c +ao_kalman.h: make-kalman kalman.5c kalman_micro.5c kalman_filter.5c load_csv.5c matrix.5c bash $< kalman > $@ ao_whiten.h: make-whiten diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 4db49215..e12f813f 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -20,6 +20,7 @@ #define HAS_RADIO 1 #define DISABLE_LOG_SPACE 1 +#define AO_VALUE_32 0 #define HAS_WIDE_GPS 0 #if defined(TELEMETRUM_V_1_0) diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c index 46fa8e1f..55fde04c 100755 --- a/src/kalman/kalman.5c +++ b/src/kalman/kalman.5c @@ -457,7 +457,7 @@ void main() { name = sprintf("%s_K%d_%d", prefix, i, time_inc); else name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc); - printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]); + printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]); } printf ("\n"); exit(0); diff --git a/src/kalman/kalman_micro.5c b/src/kalman/kalman_micro.5c index 266a1182..1b080384 100644 --- a/src/kalman/kalman_micro.5c +++ b/src/kalman/kalman_micro.5c @@ -307,7 +307,7 @@ void main() { name = sprintf("%s_K%d_%d", prefix, i, time_inc); else name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc); - printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]); + printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]); } printf ("\n"); exit(0); diff --git a/src/kernel/ao.h b/src/kernel/ao.h index a225bc4a..ad5bbf8e 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -278,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 @@ -296,11 +298,13 @@ ao_temp_to_dC(int16_t temp) __reentrant; #include +#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 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 @@ -289,6 +289,9 @@ version(void) #endif #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 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 + +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 20162c1f..410815b6 100644 --- a/src/kernel/ao_convert_pa.c +++ b/src/kernel/ao_convert_pa.c @@ -39,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; @@ -52,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; @@ -76,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 #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_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_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 /* 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_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/product/Makefile.telemetrum b/src/product/Makefile.telemetrum index dbbf57d8..e9b144c0 100644 --- a/src/product/Makefile.telemetrum +++ b/src/product/Makefile.telemetrum @@ -24,6 +24,7 @@ INC = \ altitude.h \ ao_kalman.h \ ao_product.h \ + ao_telemetry.h \ $(TM_INC) CORE_SRC = \ diff --git a/src/telefire-v0.1/Makefile b/src/telefire-v0.1/Makefile index 25267268..99d29826 100644 --- a/src/telefire-v0.1/Makefile +++ b/src/telefire-v0.1/Makefile @@ -25,7 +25,6 @@ INC = \ CORE_SRC = \ ao_cmd.c \ ao_config.c \ - ao_convert.c \ ao_mutex.c \ ao_panic.c \ ao_stdio.c \ diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile index ad5065c1..944543c5 100644 --- a/src/telefire-v0.2/Makefile +++ b/src/telefire-v0.2/Makefile @@ -25,7 +25,6 @@ INC = \ CORE_SRC = \ ao_cmd.c \ ao_config.c \ - ao_convert.c \ ao_mutex.c \ ao_panic.c \ ao_stdio.c \ diff --git a/src/telemega-v1.0/Makefile b/src/telemega-v1.0/Makefile index 46c768e4..4a1b3908 100644 --- a/src/telemega-v1.0/Makefile +++ b/src/telemega-v1.0/Makefile @@ -31,6 +31,7 @@ INC = \ ao_mpu.h \ stm32l.h \ math.h \ + ao_ms5607_convert.c \ Makefile # diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index 948310e5..ed911798 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -22,6 +22,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 +#define AO_VALUE_32 0 #define HAS_USB_PULLUP 1 #define AO_USB_PULLUP_PORT P1 diff --git a/src/teleterra-v0.2/ao_pins.h b/src/teleterra-v0.2/ao_pins.h index 60d627ad..472af534 100644 --- a/src/teleterra-v0.2/ao_pins.h +++ b/src/teleterra-v0.2/ao_pins.h @@ -72,6 +72,8 @@ #define BATTERY_PIN 5 #define HAS_TELEMETRY 0 + + #define AO_VALUE_32 0 #endif #if DBG_ON_P1 diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 314998c1..bb5c3a7d 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -34,9 +34,11 @@ #define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) #define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) +#if 0 #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)) +#endif #define AO_GPS_NEW_DATA 1 #define AO_GPS_NEW_TRACKING 2 @@ -93,6 +95,7 @@ struct ao_adc { #include #include #include +#include #if TELEMEGA int ao_gps_count; @@ -234,7 +237,7 @@ double main_time; int tick_offset; -static int32_t ao_k_height; +static ao_k_t ao_k_height; int16_t ao_time(void) diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c index 5961bd93..f4af707e 100644 --- a/src/test/ao_micropeak_test.c +++ b/src/test/ao_micropeak_test.c @@ -33,6 +33,7 @@ uint8_t ao_flight_debug; #define AO_FLIGHT_TEST typedef int32_t alt_t; +typedef int32_t pres_t; #define AO_MS_TO_TICKS(ms) ((ms) / 10) -- cgit v1.2.3 From 6c3d09bf40f2af6e8722f33a70b41e5d94ceaf9f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 13 Jul 2014 20:42:41 -0700 Subject: altos: Interrupt radio receive when changing data rate This aborts any pending radio receive when changing the data rate so that the radio can be reprogrammed to receive at the correct rate. Signed-off-by: Keith Packard --- src/kernel/ao_config.c | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/kernel') diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 32a0967c..d73a3733 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -518,6 +518,9 @@ ao_config_radio_rate_set(void) __reentrant ao_telemetry_reset_interval(); #endif _ao_config_edit_finish(); +#if HAS_RADIO_RECV + ao_radio_recv_abort(); +#endif } #endif -- cgit v1.2.3 From 50aec54bdc35962145eff9b465f9cd7b3d9fea0b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 13 Jul 2014 20:43:27 -0700 Subject: altos: Make ao_gps_print deal with telem containing 32-bit altitude values ao_gps_print is used with new telem packets from a few places; use AO_TELEMETRY_LOCATION_ALTITUDE when necessary. Signed-off-by: Keith Packard --- src/kernel/ao_gps_print.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src/kernel') 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 " -- cgit v1.2.3