diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 3e37c0e908..e12ab83b48 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -335,7 +335,7 @@ void LR_MsgHandler_NTUN_Copter::process_message(uint8_t *msg) bool LR_MsgHandler::set_parameter(const char *name, float value) { - const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE", + const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_TYPE", "EK2_ENABLE", "COMPASS_ORIENT", "COMPASS_ORIENT2", "COMPASS_ORIENT3"}; for (uint8_t i=0; i < ARRAY_SIZE(ignore_parms); i++) { diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index e7e68ac9bb..19753fb852 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -85,7 +85,8 @@ LR_MsgHandler_PARM *parameter_handler; /* messages which we will be generating, so should be discarded */ -static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5", +static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5", + "NKF1", "NKF2", "NKF3", "NKF4", "NKF5", "AHR2", "POS", "CHEK", NULL }; /* diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 4ac13e6fb4..18a0b8e180 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -182,6 +182,8 @@ static const struct LogStructure log_structure[] PROGMEM = { void ReplayVehicle::setup(void) { + load_parameters(); + // we pass zero log structures, as we will be outputting the log // structures we need manually, to prevent FMT duplicates dataflash.Init(log_structure, 0); @@ -192,7 +194,8 @@ void ReplayVehicle::setup(void) ahrs.set_wind_estimation(true); ahrs.set_correct_centrifugal(true); ahrs.set_ekf_use(true); - + EKF2.set_enable(true); + printf("Starting disarmed\n"); hal.util->set_soft_armed(false); @@ -587,6 +590,7 @@ void Replay::setup() } _vehicle.setup(); + set_ins_update_rate(log_info.update_rate); feenableexcept(FE_INVALID | FE_OVERFLOW);