diff options
| -rw-r--r-- | src/ao.h | 1 | ||||
| -rw-r--r-- | src/ao_flight.c | 15 | ||||
| -rw-r--r-- | src/ao_flight_test.c | 58 | ||||
| -rw-r--r-- | src/ao_kalman.c | 14 |
4 files changed, 72 insertions, 16 deletions
@@ -829,6 +829,7 @@ 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 __pdata int16_t ao_max_height; /* max of ao_height */ +extern __pdata int16_t ao_avg_height; /* running average of height */ extern __pdata int16_t ao_error_h; extern __pdata int16_t ao_error_h_sq_avg; diff --git a/src/ao_flight.c b/src/ao_flight.c index f1b60d69..a8760ff0 100644 --- a/src/ao_flight.c +++ b/src/ao_flight.c @@ -43,7 +43,6 @@ __pdata uint16_t ao_launch_tick; /* time of launch detect */ __pdata uint16_t ao_interval_end; __pdata int16_t ao_interval_min_height; __pdata int16_t ao_interval_max_height; - __pdata uint8_t ao_flight_force_idle; /* We also have a clock, which can be used to sanity check things in @@ -242,7 +241,7 @@ ao_flight(void) /* initialize interval values */ ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; - ao_interval_min_height = ao_interval_max_height = ao_height; + ao_interval_min_height = ao_interval_max_height = ao_avg_height; /* and enter drogue state */ ao_flight_state = ao_flight_drogue; @@ -279,14 +278,14 @@ ao_flight(void) * barometer: altitude stable and within 1000m of the launch altitude */ - if (ao_height < ao_interval_min_height) - ao_interval_min_height = ao_height; - if (ao_height > ao_interval_max_height) - ao_interval_max_height = ao_height; + if (ao_avg_height < ao_interval_min_height) + ao_interval_min_height = ao_avg_height; + if (ao_avg_height > ao_interval_max_height) + ao_interval_max_height = ao_avg_height; if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) { if (ao_height < AO_M_TO_HEIGHT(1000) && - ao_interval_max_height - ao_interval_min_height < AO_M_TO_HEIGHT(5)) + ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(2)) { ao_flight_state = ao_flight_landed; @@ -295,7 +294,7 @@ ao_flight(void) ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } - ao_interval_min_height = ao_interval_max_height = ao_height; + ao_interval_min_height = ao_interval_max_height = ao_avg_height; ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; } break; diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c index 72ad1450..e55d5ade 100644 --- a/src/ao_flight_test.c +++ b/src/ao_flight_test.c @@ -155,6 +155,7 @@ FILE *emulator_in; char *emulator_app; char *emulator_name; double emulator_error_max = 4; +double emulator_height_error_max = 20; /* noise in the baro sensor */ void ao_dump_state(void); @@ -224,12 +225,20 @@ static int ao_test_max_height; static double ao_test_max_height_time; static int ao_test_main_height; static double ao_test_main_height_time; +static double ao_test_landed_time; +static double ao_test_landed_height; +static double ao_test_landed_time; +static int landed_set; +static double landed_time; +static double landed_height; void ao_test_exit(void) { double drogue_error; double main_error; + double landed_error; + double landed_time_error; if (!ao_test_main_height_time) { ao_test_main_height_time = ao_test_max_height_time; @@ -237,16 +246,23 @@ ao_test_exit(void) } drogue_error = fabs(ao_test_max_height_time - drogue_time); main_error = fabs(ao_test_main_height_time - main_time); - if (drogue_error > emulator_error_max || main_error > emulator_error_max) { + landed_error = fabs(ao_test_landed_height - landed_height); + landed_time_error = ao_test_landed_time - landed_time; + if (drogue_error > emulator_error_max || main_error > emulator_error_max || + landed_time_error > emulator_error_max || landed_error > emulator_height_error_max) { printf ("%s %s\n", emulator_app, emulator_name); printf ("\tApogee error %g\n", drogue_error); printf ("\tMain error %g\n", main_error); - printf ("\tActual: apogee: %d at %7.2f main: %d at %7.2f\n", + printf ("\tLanded height error %g\n", landed_error); + printf ("\tLanded time error %g\n", landed_time_error); + printf ("\tActual: apogee: %d at %7.2f main: %d at %7.2f landed %7.2f at %7.2f\n", ao_test_max_height, ao_test_max_height_time, - ao_test_main_height, ao_test_main_height_time); - printf ("\tComputed: apogee: %d at %7.2f main: %d at %7.2f\n", - drogue_height, drogue_time, main_height, main_time); + ao_test_main_height, ao_test_main_height_time, + ao_test_landed_height, ao_test_landed_time); + printf ("\tComputed: apogee: %d at %7.2f main: %d at %7.2f landed %7.2f at %7.2f\n", + drogue_height, drogue_time, main_height, main_time, + landed_height, landed_time); exit (1); } exit(0); @@ -274,14 +290,27 @@ ao_insert(void) if (ao_test_max_height < height) { ao_test_max_height = height; ao_test_max_height_time = time; + ao_test_landed_height = height; + ao_test_landed_time = time; } if (height > ao_config.main_deploy) { ao_test_main_height_time = time; ao_test_main_height = height; } + if (ao_test_landed_height > height) { + ao_test_landed_height = height; + ao_test_landed_time = time; + } + + if (ao_flight_state == ao_flight_landed && !landed_set) { + landed_set = 1; + landed_time = time; + landed_height = height; + } + if (!ao_summary) { - printf("%7.2f height %g accel %g state %s k_height %g k_speed %g k_accel %g drogue %d main %d error %d\n", + printf("%7.2f height %8.2f accel %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\n", time, height, accel, @@ -289,11 +318,13 @@ ao_insert(void) 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 (ao_flight_state == ao_flight_landed) - ao_test_exit(); + +// if (ao_flight_state == ao_flight_landed) +// ao_test_exit(); } } } @@ -495,6 +526,17 @@ ao_sleep(void *wchan) a = atoi(words[12]); b = atoi(words[14]); } + } else if (nword == 3 && strcmp(words[0], "BARO") == 0) { + tick = strtol(words[1], NULL, 16); + a = 16384 - 328; + b = strtol(words[2], NULL, 10); + type = 'A'; + if (!ao_flight_started) { + ao_flight_ground_accel = 16384 - 328; + ao_config.accel_plus_g = 16384 - 328; + ao_config.accel_minus_g = 16384 + 328; + ao_flight_started = 1; + } } else if (nword == 2 && strcmp(words[0], "TELEM") == 0) { char *hex = words[1]; char elt[3]; diff --git a/src/ao_kalman.c b/src/ao_kalman.c index 4e6cbb06..ab97fc34 100644 --- a/src/ao_kalman.c +++ b/src/ao_kalman.c @@ -38,6 +38,8 @@ __pdata int16_t ao_height; __pdata int16_t ao_speed; __pdata int16_t ao_accel; __pdata int16_t ao_max_height; +static __pdata int32_t ao_avg_height_scaled; +__pdata int16_t ao_avg_height; __pdata int16_t ao_error_h; __pdata int16_t ao_error_h_sq_avg; @@ -276,6 +278,18 @@ ao_kalman(void) if (ao_height > ao_max_height) ao_max_height = ao_height; #ifdef AO_FLIGHT_TEST + if (ao_sample_tick - ao_sample_prev_tick > 50) { + ao_avg_height = ao_height; + } else if (ao_sample_tick - ao_sample_prev_tick > 5) { + ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height; + ao_avg_height = (ao_avg_height_scaled + 3) >> 2; + } else +#endif + { + ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height; + ao_avg_height = (ao_avg_height_scaled + 15) >> 5; + } +#ifdef AO_FLIGHT_TEST ao_sample_prev_tick = ao_sample_tick; #endif } |