From b31e896ef3884d80c3a0c702e841391d99d74821 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 May 2016 08:26:02 +1000 Subject: [PATCH] Replay: closer to goal of bit-accurate replay --- Tools/Replay/LR_MsgHandler.cpp | 29 ++++++--------- Tools/Replay/LR_MsgHandler.h | 23 +++++------- Tools/Replay/LogReader.cpp | 19 ++++++---- Tools/Replay/Replay.cpp | 68 +++++++++++++++++----------------- 4 files changed, 66 insertions(+), 73 deletions(-) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index e5f34de652..49afa0cf57 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -125,15 +125,11 @@ void LR_MsgHandler_Event::process_message(uint8_t *msg) void LR_MsgHandler_GPS2::process_message(uint8_t *msg) { - // only LOG_GPS_MSG gives us relative altitude. We still log - // the relative altitude when we get a LOG_GPS2_MESSAGE - but - // the value we use (probably) comes from the most recent - // LOG_GPS_MESSAGE message! - update_from_msg_gps(1, msg, false); + update_from_msg_gps(1, msg); } -void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt) +void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg) { uint64_t time_us; if (! field_value(msg, "TimeUS", time_us)) { @@ -159,9 +155,17 @@ void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *ms ! field_value(msg, "numSV", nsats)) { field_not_found(msg, "NSats"); } + uint16_t GWk; + uint32_t GMS; + if (! field_value(msg, "GWk", GWk)) { + field_not_found(msg, "GWk"); + } + if (! field_value(msg, "GMS", GMS)) { + field_not_found(msg, "GMS"); + } gps.setHIL(gps_offset, (AP_GPS::GPS_Status)status, - uint32_t(time_us/1000), + AP_GPS::time_epoch_convert(GWk, GMS), loc, vel, nsats, @@ -170,22 +174,13 @@ void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *ms if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) { ground_alt_cm = require_field_int32_t(msg, "Alt"); } - - if (responsible_for_relalt) { - // this could possibly check for the presence of "RelAlt" label? - int32_t tmp; - if (! field_value(msg, "RAlt", tmp)) { - tmp = require_field_int32_t(msg, "RelAlt"); - } - rel_altitude = 0.01f * tmp; - } } void LR_MsgHandler_GPS::process_message(uint8_t *msg) { - update_from_msg_gps(0, msg, true); + update_from_msg_gps(0, msg); } diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index c57cc1b6d1..f67037a562 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -141,18 +141,16 @@ class LR_MsgHandler_GPS_Base : public LR_MsgHandler public: LR_MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash, uint64_t &_last_timestamp_usec, AP_GPS &_gps, - uint32_t &_ground_alt_cm, float &_rel_altitude) + uint32_t &_ground_alt_cm) : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), - gps(_gps), ground_alt_cm(_ground_alt_cm), - rel_altitude(_rel_altitude) { }; + gps(_gps), ground_alt_cm(_ground_alt_cm) { }; protected: - void update_from_msg_gps(uint8_t imu_offset, uint8_t *data, bool responsible_for_relalt); + void update_from_msg_gps(uint8_t imu_offset, uint8_t *data); private: AP_GPS &gps; uint32_t &ground_alt_cm; - float &rel_altitude; }; @@ -161,17 +159,16 @@ class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base public: LR_MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash, uint64_t &_last_timestamp_usec, AP_GPS &_gps, - uint32_t &_ground_alt_cm, float &_rel_altitude) + uint32_t &_ground_alt_cm) : LR_MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec, - _gps, _ground_alt_cm, _rel_altitude), - gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { }; + _gps, _ground_alt_cm), + gps(_gps), ground_alt_cm(_ground_alt_cm) { }; void process_message(uint8_t *msg); private: AP_GPS &gps; uint32_t &ground_alt_cm; - float &rel_altitude; }; // it would be nice to use the same parser for both GPS message types @@ -183,16 +180,14 @@ class LR_MsgHandler_GPS2 : public LR_MsgHandler_GPS_Base public: LR_MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash, uint64_t &_last_timestamp_usec, AP_GPS &_gps, - uint32_t &_ground_alt_cm, float &_rel_altitude) + uint32_t &_ground_alt_cm) : LR_MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec, - _gps, _ground_alt_cm, - _rel_altitude), gps(_gps), - ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { }; + _gps, _ground_alt_cm), gps(_gps), + ground_alt_cm(_ground_alt_cm) { }; virtual void process_message(uint8_t *msg); private: AP_GPS &gps; uint32_t &ground_alt_cm; - float &rel_altitude; }; diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 13cdcd9280..8897214686 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -88,7 +88,12 @@ LR_MsgHandler_PARM *parameter_handler; static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5", "NKF1", "NKF2", "NKF3", "NKF4", "NKF5", "NKF6", "NKF7", "NKF8", "NKF9", "NKF10", - "AHR2", "POS", "CHEK", NULL }; + "AHR2", "POS", "CHEK", + "IMT", "IMT2", + "MAG", "MAG2", + "BARO", "BAR2", + "GPS", + "NKA", "NKV", NULL }; /* see if a type is in a list of types @@ -166,15 +171,13 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) msgparser[f.type] = parameter_handler; } else if (streq(name, "GPS")) { msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type], - dataflash, - last_timestamp_usec, - gps, ground_alt_cm, - rel_altitude); + dataflash, + last_timestamp_usec, + gps, ground_alt_cm); } else if (streq(name, "GPS2")) { msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], dataflash, - last_timestamp_usec, - gps, ground_alt_cm, - rel_altitude); + last_timestamp_usec, + gps, ground_alt_cm); } else if (streq(name, "MSG")) { msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], dataflash, last_timestamp_usec, diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index e3407b7c3f..097d1459c4 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -196,6 +196,7 @@ 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"); @@ -228,7 +229,9 @@ public: */ struct log_information { uint16_t update_rate; - bool have_imu2; + bool have_imu2:1; + bool have_imt:1; + bool have_imt2:1; } log_info {}; private: @@ -253,9 +256,6 @@ private: bool done_home_init; int32_t arm_time_ms = -1; bool ahrs_healthy; - bool have_imt = false; - bool have_imt2 = false; - bool have_fram = false; bool use_imt = true; bool check_generate = false; float tolerance_euler = 3; @@ -552,9 +552,15 @@ bool Replay::find_log_info(struct log_information &info) } } - if (streq(type, "IMU2") && !info.have_imu2) { + if (streq(type, "IMU2")) { info.have_imu2 = true; } + if (streq(type, "IMT")) { + info.have_imt = true; + } + if (streq(type, "IMT2")) { + info.have_imt2 = true; + } } if (smallest_delta == 0) { ::printf("Unable to determine log rate - insufficient IMU/IMT messages? (need=%d got=%d)", samples_required, samplecount); @@ -623,6 +629,17 @@ void Replay::setup() _vehicle.setup(); inhibit_gyro_cal(); + + if (log_info.update_rate == 400) { + // assume copter for 400Hz + _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER); + _vehicle.ahrs.set_fly_forward(false); + } else if (log_info.update_rate == 50) { + // assume copter for 400Hz + _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING); + _vehicle.ahrs.set_fly_forward(true); + } + set_ins_update_rate(log_info.update_rate); feenableexcept(FE_INVALID | FE_OVERFLOW); @@ -635,7 +652,7 @@ void Replay::setup() ekf3f = fopen("EKF3.dat", "w"); ekf4f = fopen("EKF4.dat", "w"); - fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n"); + fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n"); fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n"); fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n"); fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n"); @@ -680,12 +697,6 @@ void Replay::read_sensors(const char *type) done_parameters = true; set_user_parameters(); } - if (use_imt && streq(type,"IMT")) { - have_imt = true; - } - if (use_imt && streq(type,"IMT2")) { - have_imt2 = true; - } if (!done_home_init) { if (streq(type, "GPS") && @@ -721,26 +732,16 @@ void Replay::read_sensors(const char *type) } bool run_ahrs = false; - if (streq(type,"FRAM")) { - if (!have_fram) { - have_fram = true; - printf("Have FRAM framing\n"); - } - run_ahrs = true; - } - - if (have_imt) { - if ((streq(type,"IMT") && !have_imt2) || - (streq(type,"IMT2") && have_imt2)) { - run_ahrs = true; - } - } - - // special handling of IMU messages as these trigger an ahrs.update() - if (!have_fram && - !have_imt && - ((streq(type,"IMU") && !log_info.have_imu2) || (streq(type, "IMU2") && log_info.have_imu2))) { - run_ahrs = true; + if (log_info.have_imt2) { + run_ahrs = streq(type, "IMT2"); + _vehicle.ahrs.force_ekf_start(); + } else if (log_info.have_imt) { + run_ahrs = streq(type, "IMT"); + _vehicle.ahrs.force_ekf_start(); + } else if (log_info.have_imu2) { + run_ahrs = streq(type, "IMU2"); + } else { + run_ahrs = streq(type, "IMU"); } /* @@ -903,7 +904,7 @@ void Replay::loop() float temp = degrees(ekf_euler.z); if (temp < 0.0f) temp = temp + 360.0f; - fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n", + fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n", AP_HAL::millis() * 0.001f, logreader.get_sim_attitude().x, logreader.get_sim_attitude().y, @@ -914,7 +915,6 @@ void Replay::loop() wrap_180_cd(logreader.get_attitude().z*100)*0.01f, logreader.get_inavpos().x, logreader.get_inavpos().y, - logreader.get_relalt(), logreader.get_ahr2_attitude().x, logreader.get_ahr2_attitude().y, wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f,