From cf20e213f39fb24f15e0ac94307c2d138fcadecb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 24 Aug 2017 16:45:54 -0700 Subject: altos: Perform time comparisons using 16-bit arithmetic to handle wrap Subtracting two 16-bit unsigned values to perform time comparisons yields mystic results unless we carefully cast that to int16_t. Signed-off-by: Keith Packard --- src/kernel/ao_flight.c | 2 +- src/kernel/ao_kalman.c | 18 +++++++++--------- src/kernel/ao_sample.c | 2 +- 3 files changed, 11 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 50f2b68f..b0d911ed 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -269,7 +269,7 @@ ao_flight(void) * number of seconds. */ if (ao_config.apogee_lockout) { - if ((ao_sample_tick - ao_boost_tick) < + if ((int16_t) (ao_sample_tick - ao_boost_tick) < AO_SEC_TO_TICKS(ao_config.apogee_lockout)) break; } diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 69a1b3de..d401a3c8 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -55,14 +55,14 @@ static void ao_kalman_predict(void) { #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { + if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50) { 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) { + if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) { 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; @@ -141,13 +141,13 @@ ao_kalman_correct_baro(void) { ao_kalman_err_height(); #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { + if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50) { 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) { + if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) { 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; @@ -180,7 +180,7 @@ ao_kalman_correct_both(void) ao_kalman_err_accel(); #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { + if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50) { if (ao_flight_debug) { printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", ao_k_speed / (65536.0 * 16.0), @@ -201,7 +201,7 @@ ao_kalman_correct_both(void) (ao_k_t) AO_BOTH_K21_1 * ao_error_a; return; } - if (ao_sample_tick - ao_sample_prev_tick > 5) { + if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) { if (ao_flight_debug) { printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", ao_k_speed / (65536.0 * 16.0), @@ -250,7 +250,7 @@ ao_kalman_correct_accel(void) { ao_kalman_err_accel(); - if (ao_sample_tick - ao_sample_prev_tick > 5) { + if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) { 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; @@ -285,9 +285,9 @@ ao_kalman(void) ao_max_height = ao_height; ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height; #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) + if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50) ao_avg_height = (ao_avg_height_scaled + 1) >> 1; - else if (ao_sample_tick - ao_sample_prev_tick > 5) + else if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) ao_avg_height = (ao_avg_height_scaled + 7) >> 4; else #endif diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 90ea07ad..f0ab0169 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -180,7 +180,7 @@ static void ao_sample_rotate(void) { #ifdef AO_FLIGHT_TEST - float dt = (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; + float dt = (int16_t) (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; #else static const float dt = 1/TIME_DIV; #endif -- cgit v1.2.3 From 12ef994a24eb996458092dc35c671d6b824b1576 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Aug 2017 16:57:13 -0700 Subject: altos: Eliminate separate height error filter for accelerometer devices We don't use the error value in flight for those models anyways; it's only useful on baro-only hardware. Signed-off-by: Keith Packard --- src/kernel/ao_kalman.c | 5 ----- 1 file changed, 5 deletions(-) (limited to 'src') diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index d401a3c8..87f1bf66 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -96,13 +96,8 @@ ao_kalman_err_height(void) e = -e; if (e > 127) e = 127; -#if HAS_ACCEL - ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2; - ao_error_h_sq_avg += (e * e) >> 2; -#else ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4; ao_error_h_sq_avg += (e * e) >> 4; -#endif if (ao_flight_state >= ao_flight_drogue) return; -- cgit v1.2.3 From ea6fe21d78744d7e6225a56c369d54f7cd956767 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 26 Aug 2017 19:16:47 -0700 Subject: altos: Don't eliminate baro above mach speed, just trust it less Instead of completely eliminating the baro sensor above mach speed, just derate it a bit so that the accel will dominate for speed computation and keep the device from false-triggering across mach transitions. When we completely ignored the baro sensor above mach, and the flight spent considerable time in that speed range, then the estimated height could be far from the real value. When the estimated speed dropped back down and the baro values were brought back into the computation, then the resulting rapid shift in estimated speed could trigger accidental apogee detection. By mixing in a bit of baro data even above mach, we keep the estimated height closer to the baro value and prevent this error, at least in flights measured so far. The flight known to have this problem is: 2015-09-26-serial-2093-flight-0012.eeprom Signed-off-by: Keith Packard --- src/kernel/ao_flight.c | 4 +++- src/kernel/ao_kalman.c | 8 ++++---- src/kernel/ao_sample.h | 7 ++++++- 3 files changed, 13 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index b0d911ed..f06125cd 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -282,9 +282,11 @@ ao_flight(void) * the measured altitude reasonably closely; otherwise * we're probably transsonic. */ +#define AO_ERROR_BOUND 100 + if (ao_speed < 0 #if !HAS_ACCEL - && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100) + && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < AO_ERROR_BOUND) #endif ) { diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 87f1bf66..82315c48 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -103,13 +103,13 @@ ao_kalman_err_height(void) return; height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT; #if HAS_ACCEL - /* speed is stored * 16, but we need to ramp between 200 and 328, so + /* speed is stored * 16, but we need to ramp between 248 and 328, so * we want to multiply by 2. The result is a shift by 3. */ speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1); - if (speed_distrust <= 0) - speed_distrust = 0; - else if (speed_distrust > height_distrust) + if (speed_distrust > AO_MAX_SPEED_DISTRUST) + speed_distrust = AO_MAX_SPEED_DISTRUST; + if (speed_distrust > height_distrust) height_distrust = speed_distrust; #endif if (height_distrust > 0) { diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index da40187b..f89d6a4c 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -95,7 +95,12 @@ typedef int16_t ao_v_t; /* * Above this speed, baro measurements are unreliable */ -#define AO_MAX_BARO_SPEED 200 +#define AO_MAX_BARO_SPEED 248 + +/* The maximum amount (in a range of 0-256) to de-rate the + * baro sensor data based on speed. + */ +#define AO_MAX_SPEED_DISTRUST 160 #define ACCEL_NOSE_UP (ao_accel_2g >> 2) -- cgit v1.2.3 From 2dce02efd54ad4051f1ddb94048696e8677dd225 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Aug 2017 17:04:27 -0700 Subject: altos/test: telemetrum baro data is inverted before being written to eeprom No need for the test code to invert it during replay Signed-off-by: Keith Packard --- src/test/ao_flight_test.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index d3d39f2a..65b939c0 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -72,7 +72,7 @@ struct ao_adc { #define AO_ADC_NUM_SENSE 2 #define HAS_MS5607 1 #define HAS_MMA655X 1 -#define AO_MMA655X_INVERT 1 +#define AO_MMA655X_INVERT 0 #define HAS_BEEP 1 #define AO_CONFIG_MAX_SIZE 1024 -- cgit v1.2.3 From bef7c37d1c986cd477367c0c015be61368a788d2 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Aug 2017 17:30:15 -0700 Subject: altos/test: Integrate raw accel to provide speed for comparison This can provide a useful visualization of the 'true' vs 'kalman' speed value, as the kalman is necessarily delayed due to the model assuming constant acceleration. Signed-off-by: Keith Packard --- src/test/ao_flight_test.c | 37 +++++++++++++++++++++---------------- src/test/plottest | 13 +++++++------ 2 files changed, 28 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 65b939c0..7683a4d9 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -264,6 +264,7 @@ double main_time; int tick_offset; static ao_k_t ao_k_height; +static double simple_speed; int16_t ao_time(void) @@ -501,6 +502,7 @@ ao_insert(void) tick_offset = -ao_data_static.tick; if ((prev_tick - ao_data_static.tick) > 0x400) tick_offset += 65536; + simple_speed += accel * (ao_data_static.tick - prev_tick) / 100.0; prev_tick = ao_data_static.tick; time = (double) (ao_data_static.tick + tick_offset) / 100; @@ -616,17 +618,27 @@ ao_insert(void) #endif #if 1 - printf("%7.2f height %8.2f accel %8.3f " -#if TELEMEGA && 1 - "angle %5d " + printf("%7.2f height %8.2f accel %8.3f accel_speed %8.3f " + "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d" +#if TELEMEGA + " 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 - "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", + "\n", time, height, accel, -#if TELEMEGA && 1 - ao_sample_orient, + simple_speed > -100.0 ? simple_speed : -100.0, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_k_speed / 65536.0 / 16.0, + ao_k_accel / 65536.0 / 16.0, + ao_avg_height, + drogue_height, + main_height, + ao_error_h_sq_avg +#if TELEMEGA + , ao_sample_orient, ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), @@ -637,18 +649,11 @@ ao_insert(void) ao_data_static.hmc5883.x, ao_data_static.hmc5883.y, ao_data_static.hmc5883.z, - ao_mag_angle, + ao_mag_angle #endif - ao_state_names[ao_flight_state], - ao_k_height / 65536.0, - ao_k_speed / 65536.0 / 16.0, - ao_k_accel / 65536.0 / 16.0, - ao_avg_height, - drogue_height, - main_height, - ao_error_h_sq_avg); + ); #endif - + // if (ao_flight_state == ao_flight_landed) // ao_test_exit(); } diff --git a/src/test/plottest b/src/test/plottest index 7d253ff1..95337f10 100755 --- a/src/test/plottest +++ b/src/test/plottest @@ -7,10 +7,11 @@ set ytics border out nomirror set y2tics border out nomirror plot "$1" using 1:3 with lines axes x1y1 title "raw height",\ "$1" using 1:5 with lines axes x1y2 title "raw accel",\ -"$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: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" +"$1" using 1:7 with lines axes x1y2 title "accel speed",\ +"$1" using 1:11 with lines axes x1y1 title "height",\ +"$1" using 1:13 with lines axes x1y2 title "speed",\ +"$1" using 1:15 with lines axes x1y2 title "accel",\ +"$1" using 1:19 with lines axes x1y1 title "drogue",\ +"$1" using 1:21 with lines axes x1y1 title "main",\ +"$1" using 1:23 with lines axes x1y1 title "error" EOF -- cgit v1.2.3 From f5642db4e03856b1f2ffeae6570fcf35fb7d93fb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 27 Aug 2017 17:32:27 -0700 Subject: altos/test: Fake baro data for flight 12 serial 2093 This flight had a baro spike due to an accidental drogue charge firing but is otherwise quite useful when testing for various mach delay effects, so fake out the data during that spike. Signed-off-by: Keith Packard --- src/test/ao_flight_test.c | 42 ++++++++++++++++++++++++++++++++---------- 1 file changed, 32 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 7683a4d9..55bfe410 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -321,7 +321,7 @@ ao_dump_state(void); void ao_sleep(void *wchan); -const char const * const ao_state_names[] = { +const char * const ao_state_names[] = { "startup", "idle", "pad", "boost", "fast", "coast", "drogue", "main", "landed", "invalid" }; @@ -362,6 +362,9 @@ extern int16_t ao_accel_2g; typedef int16_t accel_t; +uint16_t ao_serial_number; +uint16_t ao_flight_number; + extern uint16_t ao_sample_tick; extern alt_t ao_sample_height; @@ -480,7 +483,6 @@ ao_insert(void) double time; ao_data_ring[ao_data_head] = ao_data_static; - ao_data_head = ao_data_ring_next(ao_data_head); if (ao_flight_state != ao_flight_startup) { #if HAS_ACCEL double accel = ((ao_flight_ground_accel - ao_data_accel_cook(&ao_data_static)) * GRAVITY * 2.0) / @@ -488,14 +490,6 @@ ao_insert(void) #else double accel = 0.0; #endif -#if TELEMEGA || TELEMETRUM_V2 || EASYMINI - double height; - - ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); - height = ao_pa_to_altitude(ao_data_static.ms5607_cooked.pres) - ao_ground_height; -#else - double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; -#endif (void) accel; if (!tick_offset) @@ -506,6 +500,27 @@ ao_insert(void) prev_tick = ao_data_static.tick; time = (double) (ao_data_static.tick + tick_offset) / 100; +#if TELEMEGA || TELEMETRUM_V2 || EASYMINI + ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked); + double height = ao_pa_to_altitude(ao_data_static.ms5607_cooked.pres) - ao_ground_height; + + /* Hack to skip baro spike at accidental drogue charge + * firing in 2015-09-26-serial-2093-flight-0012.eeprom + * so we can test the kalman filter with this data. Just + * keep reporting the same baro value across the pressure spike + */ + { + static struct ao_ms5607_sample save; + if (ao_serial_number == 2093 && ao_flight_number == 12 && 32.5 < time && time < 33.7) { + ao_data_ring[ao_data_head].ms5607_raw = save; + } else { + save = ao_data_static.ms5607_raw; + } + } +#else + double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; +#endif + if (ao_test_max_height < height) { ao_test_max_height = height; ao_test_max_height_time = time; @@ -658,6 +673,7 @@ ao_insert(void) // ao_test_exit(); } } + ao_data_head = ao_data_ring_next(ao_data_head); } @@ -768,6 +784,7 @@ ao_sleep(void *wchan) // printf ("\n"); switch (type) { case 'F': + ao_flight_number = uint16(bytes, 0); ao_flight_ground_accel = int16(bytes, 2); ao_flight_started = 1; ao_ground_pres = int32(bytes, 4); @@ -877,6 +894,7 @@ ao_sleep(void *wchan) switch (type) { case 'F': ao_flight_started = 1; + ao_flight_number = uint16(bytes, 0); ao_ground_pres = uint32(bytes, 4); ao_ground_height = ao_pa_to_altitude(ao_ground_pres); #if 0 @@ -942,6 +960,7 @@ ao_sleep(void *wchan) // printf ("\n"); switch (type) { case 'F': + ao_flight_number = uint16(bytes, 0); ao_flight_ground_accel = int16(bytes, 2); ao_flight_started = 1; ao_ground_pres = int32(bytes, 4); @@ -990,6 +1009,8 @@ ao_sleep(void *wchan) #endif else if (nword == 2 && strcmp(words[0], "log-format") == 0) { log_format = strtoul(words[1], NULL, 10); + } else if (nword == 2 && strcmp(words[0], "serial-number") == 0) { + ao_serial_number = strtoul(words[1], NULL, 10); } else if (nword >= 6 && strcmp(words[0], "Accel") == 0) { ao_config.accel_plus_g = atoi(words[3]); ao_config.accel_minus_g = atoi(words[5]); @@ -1112,6 +1133,7 @@ ao_sleep(void *wchan) switch (type) { case 'F': ao_flight_ground_accel = a; + ao_flight_number = b; if (ao_config.accel_plus_g == 0) { ao_config.accel_plus_g = a; ao_config.accel_minus_g = a + 530; -- cgit v1.2.3 From 4775b1d9b50a8732d66a0ad3b73ff74901a8cb7f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 Aug 2017 00:15:43 -0700 Subject: altos: Don't compute filtered average of height error when HAS_ACCEL We only use this for baro-only devices to avoid firing drogue charges at mach transitions; we trust the combination of accel+baro to do the right thing when available. Signed-off-by: Keith Packard --- src/kernel/ao_kalman.c | 6 ++++++ src/kernel/ao_sample.h | 2 ++ 2 files changed, 8 insertions(+) (limited to 'src') diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 82315c48..ac41085d 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -45,7 +45,9 @@ static __pdata ao_k_t ao_avg_height_scaled; __xdata ao_v_t ao_avg_height; __pdata ao_v_t ao_error_h; +#if !HAS_ACCEL __pdata ao_v_t ao_error_h_sq_avg; +#endif #if HAS_ACCEL __pdata ao_v_t ao_error_a; @@ -83,7 +85,9 @@ ao_kalman_predict(void) static void ao_kalman_err_height(void) { +#if !HAS_ACCEL ao_v_t e; +#endif ao_v_t height_distrust; #if HAS_ACCEL ao_v_t speed_distrust; @@ -91,6 +95,7 @@ ao_kalman_err_height(void) ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16); +#if !HAS_ACCEL e = ao_error_h; if (e < 0) e = -e; @@ -98,6 +103,7 @@ ao_kalman_err_height(void) e = 127; ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4; ao_error_h_sq_avg += (e * e) >> 4; +#endif if (ao_flight_state >= ao_flight_drogue) return; diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index f89d6a4c..fbef031d 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -180,7 +180,9 @@ 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 ao_v_t ao_error_h; +#if !HAS_ACCEL extern __pdata ao_v_t ao_error_h_sq_avg; +#endif #if HAS_ACCEL extern __pdata ao_v_t ao_error_a; -- cgit v1.2.3