From f2756ecb226297ff3a36e930b7b131979cd59cd9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 26 Jun 2015 15:12:17 +1000 Subject: [PATCH] Replay: added checking of solution --- Tools/Replay/LR_MsgHandler.cpp | 12 +++ Tools/Replay/LR_MsgHandler.h | 22 +++++ Tools/Replay/LogReader.cpp | 4 + Tools/Replay/LogReader.h | 3 + Tools/Replay/Replay.cpp | 147 ++++++++++++++++++++++++++++++--- 5 files changed, 176 insertions(+), 12 deletions(-) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index 873ee72917f..9d861d3e10c 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -83,6 +83,18 @@ void LR_MsgHandler_ATT::process_message(uint8_t *msg) attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw"); } +void LR_MsgHandler_CHEK::process_message(uint8_t *msg) +{ + wait_timestamp_from_msg(msg); + check_state.time_us = hal.scheduler->micros64(); + attitude_from_msg(msg, check_state.euler, "Roll", "Pitch", "Yaw"); + check_state.euler *= radians(1); + location_from_msg(msg, check_state.pos, "Lat", "Lng", "Alt"); + require_field(msg, "VN", check_state.velocity.x); + require_field(msg, "VE", check_state.velocity.y); + require_field(msg, "VD", check_state.velocity.z); +} + void LR_MsgHandler_BARO::process_message(uint8_t *msg) { diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 656d650520a..df26491b957 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -11,6 +11,14 @@ public: virtual void process_message(uint8_t *msg) = 0; bool set_parameter(const char *name, float value); + // state for CHEK message + struct CheckState { + uint64_t time_us; + Vector3f euler; + Location pos; + Vector3f velocity; + }; + protected: DataFlash_Class &dataflash; void wait_timestamp(uint32_t timestamp); @@ -87,6 +95,20 @@ private: }; +class LR_MsgHandler_CHEK : public LR_MsgHandler +{ +public: + LR_MsgHandler_CHEK(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, CheckState &_check_state) + : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), + check_state(_check_state) + { }; + virtual void process_message(uint8_t *msg); + +private: + CheckState &check_state; +}; + class LR_MsgHandler_BARO : public LR_MsgHandler { public: diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index a55ccf07f3d..b8a513455e2 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -187,6 +187,10 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) { } else if (streq(name, "FRAM")) { msgparser[f.type] = new LR_MsgHandler_FRAM(formats[f.type], dataflash, last_timestamp_usec); + } else if (streq(name, "CHEK")) { + msgparser[f.type] = new LR_MsgHandler_CHEK(formats[f.type], dataflash, + last_timestamp_usec, + check_state); } else { ::printf(" No parser for (%s)\n", name); } diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 69787df2d11..c173452bbc2 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -13,6 +13,7 @@ public: const Vector3f &get_inavpos(void) const { return inavpos; } const Vector3f &get_sim_attitude(void) const { return sim_attitude; } const float &get_relalt(void) const { return rel_altitude; } + const LR_MsgHandler::CheckState &get_check_state(void) const { return check_state; } VehicleType::vehicle_type vehicle; @@ -50,6 +51,8 @@ private: float rel_altitude; uint64_t last_timestamp_usec; + LR_MsgHandler::CheckState check_state; + bool installed_vehicle_specific_parsers; void maybe_install_vehicle_specific_parsers(); diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index ba87450755a..e6488099870 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -53,7 +54,6 @@ #include #include #include -#include #include #include // for optind only #include @@ -228,7 +228,20 @@ private: bool have_imt2 = false; bool have_fram = false; bool use_imt = true; - bool chek_generate = false; + bool check_generate = false; + bool check_solution = false; + float tolerance_euler = 3; + float tolerance_pos = 2; + float tolerance_vel = 2; + + struct { + float max_roll_error; + float max_pitch_error; + float max_yaw_error; + float max_pos_error; + float max_alt_error; + float max_vel_error; + } check_result {}; void _parse_command_line(uint8_t argc, char * const argv[]); @@ -243,7 +256,9 @@ private: void usage(void); void set_user_parameters(void); void read_sensors(const char *type); - void log_chek_generate(); + void log_check_generate(); + void log_check_solution(); + void report_checks(); }; Replay replay(replayvehicle); @@ -258,12 +273,19 @@ void Replay::usage(void) ::printf("\t--arm-time time arm at time (milliseconds)\n"); ::printf("\t--no-imt don't use IMT data\n"); ::printf("\t--check-generate generate CHEK messages in output\n"); + ::printf("\t--check check solution against CHEK messages\n"); + ::printf("\t--tolerance-euler tolerance for euler angles in degrees\n"); + ::printf("\t--tolerance-pos tolerance for position in meters\n"); + ::printf("\t--tolerance-vel tolerance for velocity in meters/second\n"); } enum { - OPT_CHECK = 100, - OPT_CHEK_GENERATE + OPT_CHECK = 128, + OPT_CHECK_GENERATE, + OPT_TOLERANCE_EULER, + OPT_TOLERANCE_POS, + OPT_TOLERANCE_VEL }; void Replay::_parse_command_line(uint8_t argc, char * const argv[]) @@ -277,7 +299,11 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) {"gyro-mask", true, 0, 'g'}, {"arm-time", true, 0, 'A'}, {"no-imt", false, 0, 'n'}, - {"chek-generate", false, 0, OPT_CHEK_GENERATE}, + {"check-generate", false, 0, OPT_CHECK_GENERATE}, + {"check", false, 0, OPT_CHECK}, + {"tolerance-euler", true, 0, OPT_TOLERANCE_EULER}, + {"tolerance-pos", true, 0, OPT_TOLERANCE_POS}, + {"tolerance-vel", true, 0, OPT_TOLERANCE_VEL}, {0, false, 0, 0} }; @@ -325,8 +351,24 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[]) break; } - case OPT_CHEK_GENERATE: - chek_generate = true; + case OPT_CHECK_GENERATE: + check_generate = true; + break; + + case OPT_CHECK: + check_solution = true; + break; + + case OPT_TOLERANCE_EULER: + tolerance_euler = atof(gopt.optarg); + break; + + case OPT_TOLERANCE_POS: + tolerance_pos = atof(gopt.optarg); + break; + + case OPT_TOLERANCE_VEL: + tolerance_vel = atof(gopt.optarg); break; case 'h': @@ -588,6 +630,14 @@ void Replay::read_sensors(const char *type) ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) { run_ahrs = true; } + + /* + always run AHRS on CHECK messages when checking the solution + */ + if (check_solution) { + run_ahrs = streq(type, "CHEK"); + } + if (run_ahrs) { _vehicle.ahrs.update(); if (_vehicle.ahrs.get_home().lat != 0) { @@ -602,8 +652,10 @@ void Replay::read_sensors(const char *type) (unsigned)ahrs_healthy, (unsigned long)hal.scheduler->millis()); } - if (chek_generate) { - log_chek_generate(); + if (check_generate) { + log_check_generate(); + } else if (check_solution) { + log_check_solution(); } } } @@ -612,7 +664,7 @@ void Replay::read_sensors(const char *type) /* copy current data to CHEK message */ -void Replay::log_chek_generate(void) +void Replay::log_check_generate(void) { Vector3f euler; Vector3f velocity; @@ -639,6 +691,35 @@ void Replay::log_chek_generate(void) _vehicle.dataflash.WriteBlock(&packet, sizeof(packet)); } + +/* + check current solution against CHEK message + */ +void Replay::log_check_solution(void) +{ + const LR_MsgHandler::CheckState &check_state = logreader.get_check_state(); + Vector3f euler; + Vector3f velocity; + Location loc {}; + + _vehicle.EKF.getEulerAngles(euler); + _vehicle.EKF.getVelNED(velocity); + _vehicle.EKF.getLLH(loc); + + float roll_error = degrees(fabsf(euler.x - check_state.euler.x)); + float pitch_error = degrees(fabsf(euler.y - check_state.euler.y)); + float yaw_error = degrees(fabsf(euler.z - check_state.euler.z)); + float vel_error = (velocity - check_state.velocity).length(); + float pos_error = get_distance(check_state.pos, loc); + + check_result.max_roll_error = max(check_result.max_roll_error, roll_error); + check_result.max_pitch_error = max(check_result.max_pitch_error, pitch_error); + check_result.max_yaw_error = max(check_result.max_yaw_error, yaw_error); + check_result.max_vel_error = max(check_result.max_vel_error, vel_error); + check_result.max_pos_error = max(check_result.max_pos_error, pos_error); +} + + void Replay::loop() { while (true) { @@ -654,7 +735,7 @@ void Replay::loop() if (!logreader.update(type)) { ::printf("End of log at %.1f seconds\n", hal.scheduler->millis()*0.001f); fclose(plotf); - exit(0); + break; } read_sensors(type); @@ -869,6 +950,48 @@ void Replay::loop() (int)faultStatus); } } + + if (check_solution) { + report_checks(); + } + exit(0); +} + +/* + report results of --check + */ +void Replay::report_checks(void) +{ + bool failed = false; + if (tolerance_euler < 0.01f) { + tolerance_euler = 0.01f; + } + if (check_result.max_roll_error > tolerance_euler) { + printf("Roll error %.2f > %.2f\n", check_result.max_roll_error, tolerance_euler); + failed = true; + } + if (check_result.max_pitch_error > tolerance_euler) { + printf("Pitch error %.2f > %.2f\n", check_result.max_pitch_error, tolerance_euler); + failed = true; + } + if (check_result.max_yaw_error > tolerance_euler) { + printf("Yaw error %.2f > %.2f\n", check_result.max_yaw_error, tolerance_euler); + failed = true; + } + if (check_result.max_pos_error > tolerance_pos) { + printf("Position error %.2f > %.2f\n", check_result.max_pos_error, tolerance_pos); + failed = true; + } + if (check_result.max_vel_error > tolerance_vel) { + printf("Velocity error %.2f > %.2f\n", check_result.max_vel_error, tolerance_vel); + failed = true; + } + if (failed) { + printf("Checks failed\n"); + exit(1); + } else { + printf("Checks passed\n"); + } } /*