From 602d6a2424a5c16e22febf6b4e6b3816022261c7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 20 Jun 2014 00:41:17 -0700 Subject: altos/aprs: Encode last serial number in SSID. Transmit serial in comment This makes it a lot easier to tell which device is sending information, and to receive data from multiple devices on the same receiver. Signed-off-by: Keith Packard --- src/test/ao_aprs_test.c | 24 ------------------------ 1 file changed, 24 deletions(-) (limited to 'src/test') diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index 86cf527a..573b5cb2 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -97,36 +97,12 @@ audio_gap(int secs) #endif } -#include - -int -ao_aprs_encode_altitude_expensive(int meters) -{ - double feet = meters / 0.3048; - - double encode = log(feet) / log(1.002); - return floor(encode + 0.5); -} - // This is where we go after reset. int main(int argc, char **argv) { int e, x; int a; - for (a = 1; a < 100000; a++) { - e = ao_aprs_encode_altitude(a); - x = ao_aprs_encode_altitude_expensive(a); - - if (e != x) { - double back_feet, back_meters; - back_feet = pow(1.002, e); - back_meters = back_feet * 0.3048; - fprintf (stderr, "APRS altitude encoding failure: altitude %d actual %d expected %d actual meters %f\n", - a, e, x, back_meters); - } - } - audio_gap(1); ao_gps_data.latitude = (45.0 + 28.25 / 60.0) * 10000000; -- cgit v1.2.3 From 309d91d25099bebda21e165165efa9ce86cb0a47 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 24 Jun 2014 15:57:13 -0700 Subject: altos/ao_flight_test: Get the Tmega version compiling again A few changes broke this recently. Signed-off-by: Keith Packard --- src/test/ao_flight_test.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/test') diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 0647fc6c..1ab22e5b 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -50,6 +50,7 @@ int ao_gps_new; #define HAS_MMA655X 1 #define HAS_HMC5883 1 #define HAS_BEEP 1 +#define AO_CONFIG_MAX_SIZE 1024 struct ao_adc { int16_t sense[AO_ADC_NUM_SENSE]; @@ -795,6 +796,7 @@ ao_sleep(void *wchan) pyro->flags |= ao_pyro_values[j].flag; if (ao_pyro_values[j].offset != NO_VALUE && i + 1 < nword) { int16_t val = strtoul(words[++i], NULL, 10); + printf("pyro %d condition %s value %d\n", p, words[i-1], val); *((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val; } } -- cgit v1.2.3 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/cc1111/ao_pins.h | 1 + src/drivers/ao_aprs.c | 2 +- src/drivers/ao_gps_skytraq.c | 3 ++- src/drivers/ao_gps_ublox.c | 2 +- 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 +++++++------ src/test/ao_aprs_test.c | 7 +++---- src/test/ao_flight_test.c | 7 +++++-- src/test/ao_gps_test.c | 5 ++++- src/test/ao_gps_test_skytraq.c | 4 ++++ src/test/ao_gps_test_ublox.c | 9 ++++++++- 19 files changed, 99 insertions(+), 45 deletions(-) (limited to 'src/test') diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 1bc3d716..4db49215 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 HAS_WIDE_GPS 0 #if defined(TELEMETRUM_V_1_0) /* Discontinued and was never built with CC1111 chips needing this */ diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index a9047149..19beb78f 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -713,7 +713,7 @@ static int tncPositionPacket(void) if (ao_gps_data.flags & AO_GPS_VALID) { latitude = ao_gps_data.latitude; longitude = ao_gps_data.longitude; - altitude = ao_gps_data.altitude; + altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data); if (altitude < 0) altitude = 0; } diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index 944a37f9..81178051 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -287,7 +287,8 @@ ao_nmea_gga(void) ao_gps_next.hdop = i; ao_gps_skip_field(); - ao_gps_next.altitude = ao_gps_decimal(0xff); + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_next, ao_gps_decimal(0xff)); + ao_gps_skip_field(); /* skip any fractional portion */ ao_nmea_finish(); diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 077698a9..48765998 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -728,7 +728,7 @@ ao_gps(void) __reentrant if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC)) ao_gps_data.flags |= AO_GPS_DATE_VALID; - ao_gps_data.altitude = nav_posllh.alt_msl / 1000; + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000); ao_gps_data.latitude = nav_posllh.lat; ao_gps_data.longitude = nav_posllh.lon; 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); diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index 573b5cb2..ae505dea 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -21,6 +21,8 @@ #include #include +#define HAS_GPS 1 + #include #define AO_GPS_NUM_SAT_MASK (0xf << 0) @@ -100,14 +102,11 @@ audio_gap(int secs) // This is where we go after reset. int main(int argc, char **argv) { - int e, x; - int a; - audio_gap(1); ao_gps_data.latitude = (45.0 + 28.25 / 60.0) * 10000000; ao_gps_data.longitude = (-(122 + 44.2649 / 60.0)) * 10000000; - ao_gps_data.altitude = 84; + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, 84); ao_gps_data.flags = (AO_GPS_VALID|AO_GPS_RUNNING); /* Transmit one packet */ diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 1ab22e5b..314998c1 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -175,7 +175,7 @@ ao_gps_angle(void) ao_gps_static.latitude / 1e7, ao_gps_static.longitude / 1e7, &dist, &bearing); - height = ao_gps_static.altitude - ao_gps_prev.altitude; + height = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_static) - AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_prev); angle = atan2(dist, height); return angle * 180/M_PI; @@ -756,7 +756,10 @@ ao_sleep(void *wchan) ao_gps_static.tick = tick; ao_gps_static.latitude = int32(bytes, 0); ao_gps_static.longitude = int32(bytes, 4); - ao_gps_static.altitude = int32(bytes, 8); + { + int32_t altitude = int32(bytes, 8); + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_static, altitude); + } ao_gps_static.flags = bytes[13]; if (!ao_gps_count) ao_gps_first = ao_gps_static; diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c index e799ab0f..543bbcc3 100644 --- a/src/test/ao_gps_test.c +++ b/src/test/ao_gps_test.c @@ -53,6 +53,9 @@ struct ao_gps_orig { uint16_t v_error; /* m */ }; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((l)->altitude) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) ((l)->altitude = (a)) + #define SIRF_SAT_STATE_ACQUIRED (1 << 0) #define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) #define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) @@ -433,7 +436,7 @@ ao_dump_state(void *wchan) if (wchan != &ao_gps_new) return; - + if (ao_gps_new & AO_GPS_NEW_DATA) { ao_gps_print(&ao_gps_data); putchar('\n'); diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c index 1b590d5e..5eb7118d 100644 --- a/src/test/ao_gps_test_skytraq.c +++ b/src/test/ao_gps_test_skytraq.c @@ -16,6 +16,7 @@ */ #define AO_GPS_TEST +#define HAS_GPS 1 #include "ao_host.h" #include #include @@ -53,6 +54,9 @@ struct ao_gps_orig { uint16_t v_error; /* m */ }; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((l)->altitude) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) ((l)->altitude = (a)) + #define SIRF_SAT_STATE_ACQUIRED (1 << 0) #define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) #define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c index 4eb4b837..5ea205d6 100644 --- a/src/test/ao_gps_test_ublox.c +++ b/src/test/ao_gps_test_ublox.c @@ -16,6 +16,7 @@ */ #define AO_GPS_TEST +#define HAS_GPS 1 #include "ao_host.h" #include #include @@ -44,7 +45,7 @@ struct ao_telemetry_location { uint8_t flags; int32_t latitude; /* degrees * 10⁷ */ int32_t longitude; /* degrees * 10⁷ */ - int16_t altitude; /* m */ + int16_t altitude_low; /* m */ uint16_t ground_speed; /* cm/s */ uint8_t course; /* degrees / 2 */ uint8_t pdop; /* * 5 */ @@ -53,8 +54,14 @@ struct ao_telemetry_location { int16_t climb_rate; /* cm/s */ uint16_t h_error; /* m */ uint16_t v_error; /* m */ + int16_t altitude_high; /* m */ }; +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)->altitude_high = (a) >> 16), \ + ((l)->altitude_low = (a))) + #define UBLOX_SAT_STATE_ACQUIRED (1 << 0) #define UBLOX_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) #define UBLOX_SAT_BIT_SYNC_COMPLETE (1 << 2) -- 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/test') 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 0fd867f504dd7df62e95da98ded511bb7b9e4c66 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 13 Jul 2014 20:44:39 -0700 Subject: altos/test: Build TeleMetrumV2 flight test variant Parses TeleMetrumV2 eeprom files and runs the TeleMetrumV2 flight code. Signed-off-by: Keith Packard --- src/test/Makefile | 4 ++ src/test/ao_flight_test.c | 108 ++++++++++++++++++++++++++++++++++++---------- src/test/plottest | 6 +-- 3 files changed, 93 insertions(+), 25 deletions(-) (limited to 'src/test') diff --git a/src/test/Makefile b/src/test/Makefile index 017f7f71..02e1d22b 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,6 +1,7 @@ vpath % ..:../kernel:../drivers:../util:../micropeak:../aes:../product PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ + ao_flight_test_metrum \ ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \ ao_ms5607_convert_test ao_quaternion_test @@ -33,6 +34,9 @@ ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kal ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c ao_pyro.c ao_pyro.h $(INCS) cc -DTELEMEGA=1 $(CFLAGS) -o $@ $< -lm +ao_flight_test_metrum: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c ao_pyro.c ao_pyro.h $(INCS) + cc -DTELEMETRUM_V2=1 $(CFLAGS) -o $@ $< -lm + ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index bb5c3a7d..8b737ca1 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -45,6 +45,10 @@ int ao_gps_new; +#if !defined(TELEMEGA) && !defined(TELEMETRUM_V2) +#define TELEMETRUM_V1 1 +#endif + #if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 @@ -60,7 +64,25 @@ struct ao_adc { int16_t v_pbatt; int16_t temp; }; -#else +#endif + +#if TELEMETRUM_V2 +#define AO_ADC_NUM_SENSE 2 +#define HAS_MS5607 1 +#define HAS_MMA655X 1 +#define HAS_BEEP 1 +#define AO_CONFIG_MAX_SIZE 1024 + +struct ao_adc { + int16_t sense_a; + int16_t sense_m; + int16_t v_batt; + int16_t temp; +}; +#endif + + +#if TELEMETRUM_V1 /* * One set of samples read from the A/D converter */ @@ -189,17 +211,6 @@ ao_gps_angle(void) #define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) #define from_fix(x) ((x) >> 16) -/* - * Above this height, the baro sensor doesn't work - */ -#define AO_BARO_SATURATE 13000 -#define AO_MIN_BARO_VALUE ao_altitude_to_pres(AO_BARO_SATURATE) - -/* - * Above this speed, baro measurements are unreliable - */ -#define AO_MAX_BARO_SPEED 200 - #define ACCEL_NOSE_UP (ao_accel_2g >> 2) extern enum ao_flight_state ao_flight_state; @@ -309,7 +320,7 @@ struct ao_cmds { #define ao_xmemcmp(d,s,c) memcmp(d,s,c) #define AO_NEED_ALTITUDE_TO_PRES 1 -#if TELEMEGA +#if TELEMEGA || TELEMETRUM_V2 #include "ao_convert_pa.c" #include struct ao_ms5607_prom ao_ms5607_prom; @@ -459,7 +470,7 @@ ao_insert(void) #else double accel = 0.0; #endif -#if TELEMEGA +#if TELEMEGA || TELEMETRUM_V2 double height; ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); @@ -554,6 +565,7 @@ ao_insert(void) mag_azel.el, mag_azel.az); #endif +#if 0 printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n", time, ao_state_names[ao_flight_state], @@ -563,6 +575,7 @@ ao_insert(void) (int) floor (ao_gps_angle() + 0.5), (ao_gps_static.flags & 0xf) * 10); +#endif #if 0 printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n", ao_state_names[ao_flight_state], @@ -585,9 +598,9 @@ ao_insert(void) #endif #endif -#if 0 +#if 1 printf("%7.2f height %8.2f accel %8.3f " -#if TELEMEGA +#if TELEMEGA && 0 "angle %5d " "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d " #endif @@ -595,7 +608,7 @@ ao_insert(void) time, height, accel, -#if TELEMEGA +#if TELEMEGA && 0 ao_sample_orient, ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), @@ -678,7 +691,8 @@ ao_sleep(void *wchan) { #if TELEMEGA ao_data_static.mpu6000 = ao_ground_mpu6000; -#else +#endif +#if TELEMETRUM_V1 ao_data_static.adc.accel = ao_flight_ground_accel; #endif ao_insert(); @@ -807,7 +821,59 @@ ao_sleep(void *wchan) } } } -#else +#endif +#if TELEMETRUM_V2 + if (log_format == AO_LOG_FORMAT_TELEMETRUM && nword == 14 && strlen(words[0]) == 1) { + int i; + struct ao_ms5607_value value; + + type = words[0][0]; + tick = strtoul(words[1], NULL, 16); +// printf ("%c %04x", type, tick); + for (i = 2; i < nword; i++) { + bytes[i - 2] = strtoul(words[i], NULL, 16); +// printf(" %02x", bytes[i-2]); + } +// printf ("\n"); + switch (type) { + case 'F': + ao_flight_ground_accel = int16(bytes, 2); + ao_flight_started = 1; + ao_ground_pres = int32(bytes, 4); + ao_ground_height = ao_pa_to_altitude(ao_ground_pres); + break; + case 'A': + ao_data_static.tick = tick; + ao_data_static.ms5607_raw.pres = int32(bytes, 0); + ao_data_static.ms5607_raw.temp = int32(bytes, 4); + ao_ms5607_convert(&ao_data_static.ms5607_raw, &value); + ao_data_static.mma655x = int16(bytes, 8); + ao_records_read++; + ao_insert(); + return; + } + continue; + } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) { + if (strcmp(words[1], "reserved:") == 0) + ao_ms5607_prom.reserved = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "sens:") == 0) + ao_ms5607_prom.sens = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "off:") == 0) + ao_ms5607_prom.off = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tcs:") == 0) + ao_ms5607_prom.tcs = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tco:") == 0) + ao_ms5607_prom.tco = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tref:") == 0) + ao_ms5607_prom.tref = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "tempsens:") == 0) + ao_ms5607_prom.tempsens = strtoul(words[2], NULL, 10); + else if (strcmp(words[1], "crc:") == 0) + ao_ms5607_prom.crc = strtoul(words[2], NULL, 10); + continue; + } +#endif +#if TELEMETRUM_V1 if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) { type = words[0][0]; tick = strtoul(words[1], NULL, 16); @@ -932,7 +998,7 @@ ao_sleep(void *wchan) if (type != 'F' && !ao_flight_started) continue; -#if TELEMEGA +#if TELEMEGA || TELEMETRUM_V2 (void) a; (void) b; #else @@ -953,8 +1019,6 @@ ao_sleep(void *wchan) ao_data_static.tick = tick; ao_data_static.adc.accel = a; ao_data_static.adc.pres_real = b; - if (b < AO_MIN_BARO_VALUE) - b = AO_MIN_BARO_VALUE; ao_data_static.adc.pres = b; ao_records_read++; ao_insert(); diff --git a/src/test/plottest b/src/test/plottest index 76af5ee7..7d253ff1 100755 --- a/src/test/plottest +++ b/src/test/plottest @@ -10,7 +10,7 @@ plot "$1" using 1:3 with lines axes x1y1 title "raw height",\ "$1" using 1:9 with lines axes x1y1 title "height",\ "$1" using 1:11 with lines axes x1y2 title "speed",\ "$1" using 1:13 with lines axes x1y2 title "accel",\ -"$1" using 1:15 with lines axes x1y1 title "drogue",\ -"$1" using 1:17 with lines axes x1y1 title "main",\ -"$1" using 1:19 with lines axes x1y1 title "error" +"$1" using 1:17 with lines axes x1y1 title "drogue",\ +"$1" using 1:19 with lines axes x1y1 title "main",\ +"$1" using 1:21 with lines axes x1y1 title "error" EOF -- cgit v1.2.3