zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Replay: closer to goal of bit-accurate replay

master
Andrew Tridgell 9 years ago
parent
7ab1367ec4
commit
b31e896ef3
4 changed files with 66 additions and 73 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 29
      Tools/Replay/LR_MsgHandler.cpp
  2. 23
      Tools/Replay/LR_MsgHandler.h
  3. 19
      Tools/Replay/LogReader.cpp
  4. 68
      Tools/Replay/Replay.cpp

29
Tools/Replay/LR_MsgHandler.cpp
Unescape View File

@ -125,15 +125,11 @@ void LR_MsgHandler_Event::process_message(uint8_t *msg)
void LR_MsgHandler_GPS2::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 update_from_msg_gps(1, msg);
// 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);
} }
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; uint64_t time_us;
if (! field_value(msg, "TimeUS", 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_value(msg, "numSV", nsats)) {
field_not_found(msg, "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, gps.setHIL(gps_offset,
(AP_GPS::GPS_Status)status, (AP_GPS::GPS_Status)status,
uint32_t(time_us/1000), AP_GPS::time_epoch_convert(GWk, GMS),
loc, loc,
vel, vel,
nsats, 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) { if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
ground_alt_cm = require_field_int32_t(msg, "Alt"); 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) void LR_MsgHandler_GPS::process_message(uint8_t *msg)
{ {
update_from_msg_gps(0, msg, true); update_from_msg_gps(0, msg);
} }

23
Tools/Replay/LR_MsgHandler.h
Unescape View File

@ -141,18 +141,16 @@ class LR_MsgHandler_GPS_Base : public LR_MsgHandler
public: public:
LR_MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash, LR_MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps, 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), : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
gps(_gps), ground_alt_cm(_ground_alt_cm), gps(_gps), ground_alt_cm(_ground_alt_cm) { };
rel_altitude(_rel_altitude) { };
protected: 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: private:
AP_GPS &gps; AP_GPS &gps;
uint32_t &ground_alt_cm; uint32_t &ground_alt_cm;
float &rel_altitude;
}; };
@ -161,17 +159,16 @@ class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base
public: public:
LR_MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash, LR_MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps, 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, : LR_MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
_gps, _ground_alt_cm, _rel_altitude), _gps, _ground_alt_cm),
gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { }; gps(_gps), ground_alt_cm(_ground_alt_cm) { };
void process_message(uint8_t *msg); void process_message(uint8_t *msg);
private: private:
AP_GPS &gps; AP_GPS &gps;
uint32_t &ground_alt_cm; uint32_t &ground_alt_cm;
float &rel_altitude;
}; };
// it would be nice to use the same parser for both GPS message types // 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: public:
LR_MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash, LR_MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps, 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, : LR_MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
_gps, _ground_alt_cm, _gps, _ground_alt_cm), gps(_gps),
_rel_altitude), gps(_gps), ground_alt_cm(_ground_alt_cm) { };
ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
virtual void process_message(uint8_t *msg); virtual void process_message(uint8_t *msg);
private: private:
AP_GPS &gps; AP_GPS &gps;
uint32_t &ground_alt_cm; uint32_t &ground_alt_cm;
float &rel_altitude;
}; };

19
Tools/Replay/LogReader.cpp
Unescape View File

@ -88,7 +88,12 @@ LR_MsgHandler_PARM *parameter_handler;
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", "NKF1", "NKF2", "NKF3", "NKF4", "NKF5",
"NKF6", "NKF7", "NKF8", "NKF9", "NKF10", "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 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; msgparser[f.type] = parameter_handler;
} else if (streq(name, "GPS")) { } else if (streq(name, "GPS")) {
msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type], msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type],
dataflash, dataflash,
last_timestamp_usec, last_timestamp_usec,
gps, ground_alt_cm, gps, ground_alt_cm);
rel_altitude);
} else if (streq(name, "GPS2")) { } else if (streq(name, "GPS2")) {
msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], dataflash, msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], dataflash,
last_timestamp_usec, last_timestamp_usec,
gps, ground_alt_cm, gps, ground_alt_cm);
rel_altitude);
} else if (streq(name, "MSG")) { } else if (streq(name, "MSG")) {
msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], dataflash, msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], dataflash,
last_timestamp_usec, last_timestamp_usec,

68
Tools/Replay/Replay.cpp
Unescape View File

@ -196,6 +196,7 @@ void ReplayVehicle::setup(void)
ahrs.set_wind_estimation(true); ahrs.set_wind_estimation(true);
ahrs.set_correct_centrifugal(true); ahrs.set_correct_centrifugal(true);
ahrs.set_ekf_use(true); ahrs.set_ekf_use(true);
EKF2.set_enable(true); EKF2.set_enable(true);
printf("Starting disarmed\n"); printf("Starting disarmed\n");
@ -228,7 +229,9 @@ public:
*/ */
struct log_information { struct log_information {
uint16_t update_rate; uint16_t update_rate;
bool have_imu2; bool have_imu2:1;
bool have_imt:1;
bool have_imt2:1;
} log_info {}; } log_info {};
private: private:
@ -253,9 +256,6 @@ private:
bool done_home_init; bool done_home_init;
int32_t arm_time_ms = -1; int32_t arm_time_ms = -1;
bool ahrs_healthy; bool ahrs_healthy;
bool have_imt = false;
bool have_imt2 = false;
bool have_fram = false;
bool use_imt = true; bool use_imt = true;
bool check_generate = false; bool check_generate = false;
float tolerance_euler = 3; 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; info.have_imu2 = true;
} }
if (streq(type, "IMT")) {
info.have_imt = true;
}
if (streq(type, "IMT2")) {
info.have_imt2 = true;
}
} }
if (smallest_delta == 0) { if (smallest_delta == 0) {
::printf("Unable to determine log rate - insufficient IMU/IMT messages? (need=%d got=%d)", samples_required, samplecount); ::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(); _vehicle.setup();
inhibit_gyro_cal(); 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); set_ins_update_rate(log_info.update_rate);
feenableexcept(FE_INVALID | FE_OVERFLOW); feenableexcept(FE_INVALID | FE_OVERFLOW);
@ -635,7 +652,7 @@ void Replay::setup()
ekf3f = fopen("EKF3.dat", "w"); ekf3f = fopen("EKF3.dat", "w");
ekf4f = fopen("EKF4.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(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(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"); 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; done_parameters = true;
set_user_parameters(); 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 (!done_home_init) {
if (streq(type, "GPS") && if (streq(type, "GPS") &&
@ -721,26 +732,16 @@ void Replay::read_sensors(const char *type)
} }
bool run_ahrs = false; bool run_ahrs = false;
if (streq(type,"FRAM")) { if (log_info.have_imt2) {
if (!have_fram) { run_ahrs = streq(type, "IMT2");
have_fram = true; _vehicle.ahrs.force_ekf_start();
printf("Have FRAM framing\n"); } else if (log_info.have_imt) {
} run_ahrs = streq(type, "IMT");
run_ahrs = true; _vehicle.ahrs.force_ekf_start();
} } else if (log_info.have_imu2) {
run_ahrs = streq(type, "IMU2");
if (have_imt) { } else {
if ((streq(type,"IMT") && !have_imt2) || run_ahrs = streq(type, "IMU");
(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;
} }
/* /*
@ -903,7 +904,7 @@ void Replay::loop()
float temp = degrees(ekf_euler.z); float temp = degrees(ekf_euler.z);
if (temp < 0.0f) temp = temp + 360.0f; 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, AP_HAL::millis() * 0.001f,
logreader.get_sim_attitude().x, logreader.get_sim_attitude().x,
logreader.get_sim_attitude().y, logreader.get_sim_attitude().y,
@ -914,7 +915,6 @@ void Replay::loop()
wrap_180_cd(logreader.get_attitude().z*100)*0.01f, wrap_180_cd(logreader.get_attitude().z*100)*0.01f,
logreader.get_inavpos().x, logreader.get_inavpos().x,
logreader.get_inavpos().y, logreader.get_inavpos().y,
logreader.get_relalt(),
logreader.get_ahr2_attitude().x, logreader.get_ahr2_attitude().x,
logreader.get_ahr2_attitude().y, logreader.get_ahr2_attitude().y,
wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f, wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f,

Write Preview
Loading…
Cancel
Save