Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Replay: fixed loading of users parameters and parameter override 

use compass.set_offsets() to avoid trying to write to storage
This commit is contained in:
Andrew Tridgell 2014-10-15 09:17:22 +11:00
parent b437977547
commit 6e0e56a6ef
5 changed files with 35 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
Tools/Replay/LogReader.cpp
View File

@ -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) {

3
Tools/Replay/LogReader.h
View File

@ -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;

3
Tools/Replay/Parameters.h
View File

@ -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;
};

4
Tools/Replay/Parameters.pde
View File

@ -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
};

9
Tools/Replay/Replay.pde
View File

@ -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),