diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 1da4a02cac..1eadb4cc29 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -144,7 +144,7 @@ void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length) memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.time_ms); compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z)); - compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z); + compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z)); break; } @@ -186,7 +186,7 @@ void LogReader::process_rover(uint8_t type, uint8_t *data, uint16_t length) memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.time_ms); compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z)); - compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z); + compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z)); break; } @@ -216,7 +216,7 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length) memcpy(&msg, data, sizeof(msg)); wait_timestamp(msg.time_ms); compass.setHIL(Vector3f(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z)); - compass.set_and_save_offsets(0, msg.offset_x, msg.offset_y, msg.offset_z); + compass.set_offsets(0, Vector3f(msg.offset_x, msg.offset_y, msg.offset_z)); break; } @@ -250,6 +250,10 @@ void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length) bool LogReader::set_parameter(const char *name, float value) { + if (strcmp(name, "GPS_TYPE") == 0) { + // ignore this one + return true; + } enum ap_var_type var_type; AP_Param *vp = AP_Param::find(name, &var_type); if (vp == NULL) { @@ -470,6 +474,18 @@ bool LogReader::update(uint8_t &type) break; } + case LOG_AHR2_MSG: { + struct log_AHRS msg; + if(sizeof(msg) != f.length) { + printf("Bad AHR2 length %u should be %u\n", (unsigned)f.length, (unsigned)sizeof(msg)); + exit(1); + } + memcpy(&msg, data, sizeof(msg)); + wait_timestamp(msg.time_ms); + ahr2_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f); + break; + } + default: if (vehicle == VEHICLE_PLANE) { diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 9fc9898fab..15e2391191 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -3,6 +3,7 @@ enum log_messages { // plane specific messages LOG_PLANE_ATTITUDE_MSG = 9, LOG_PLANE_COMPASS_MSG = 11, + LOG_PLANE_COMPASS2_MSG = 15, LOG_PLANE_AIRSPEED_MSG = 17, // copter specific messages @@ -25,6 +26,7 @@ public: bool wait_type(uint8_t type); const Vector3f &get_attitude(void) const { return attitude; } + const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; } 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; } @@ -57,6 +59,7 @@ private: struct log_Format formats[LOGREADER_MAX_FORMATS]; Vector3f attitude; + Vector3f ahr2_attitude; Vector3f sim_attitude; Vector3f inavpos; float rel_altitude; diff --git a/Tools/Replay/Parameters.h b/Tools/Replay/Parameters.h index 3a79b0f17b..e445c2defb 100644 --- a/Tools/Replay/Parameters.h +++ b/Tools/Replay/Parameters.h @@ -15,7 +15,8 @@ public: k_param_ins, k_param_ahrs, k_param_airspeed, - k_param_NavEKF + k_param_NavEKF, + k_param_compass }; AP_Int8 dummy; }; diff --git a/Tools/Replay/Parameters.pde b/Tools/Replay/Parameters.pde index 23561cabed..94b2672a7a 100644 --- a/Tools/Replay/Parameters.pde +++ b/Tools/Replay/Parameters.pde @@ -34,6 +34,10 @@ const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp GOBJECTN(ahrs.get_NavEKF(), NavEKF, "EKF_", NavEKF), + // @Group: COMPASS_ + // @Path: ../libraries/AP_Compass/AP_Compass.cpp + GOBJECT(compass, "COMPASS_", Compass), + AP_VAREND }; diff --git a/Tools/Replay/Replay.pde b/Tools/Replay/Replay.pde index c37628e42e..d4df21fbf1 100644 --- a/Tools/Replay/Replay.pde +++ b/Tools/Replay/Replay.pde @@ -103,6 +103,8 @@ static struct { float value; } user_parameters[100]; +// setup the var_info table +AP_Param param_loader(var_info); static void usage(void) { @@ -228,7 +230,7 @@ void 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 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 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(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"); @@ -391,10 +393,13 @@ void loop() barometer.get_altitude(), LogReader.get_attitude().x, LogReader.get_attitude().y, - LogReader.get_attitude().z, + 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, degrees(DCM_attitude.x), degrees(DCM_attitude.y), degrees(DCM_attitude.z),