Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Replay: cope with up to 3 IMUs

Browse Source
This commit is contained in:
Andrew Tridgell 2014-11-16 12:31:05 +11:00
parent 7497b4dfb1
commit c52578426e
1 changed files with 25 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
Tools/Replay/LogReader.cpp
View File

@ -27,8 +27,8 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro_HIL &_baro
compass(_compass),
gps(_gps),
airspeed(_airspeed),
accel_mask(3),
gyro_mask(3)
accel_mask(7),
gyro_mask(7)
{}
bool LogReader::open_log(const char *logfile)
@ -258,9 +258,12 @@ 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;
const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE" };
for (uint8_t i=0; i<sizeof(ignore_parms)/sizeof(ignore_parms[0]); i++) {
if (strcmp(name, ignore_parms[i]) == 0) {
::printf("Ignoring set of %s to %f\n", name, value);
return true;
}
}
enum ap_var_type var_type;
AP_Param *vp = AP_Param::find(name, &var_type);
@ -386,6 +389,23 @@ bool LogReader::update(uint8_t &type)
break;
}
case LOG_IMU3_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU3 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 4) {
ins.set_gyro(2, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 4) {
ins.set_accel(2, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}
case LOG_GPS_MSG: {
struct log_GPS msg;
if(sizeof(msg) != f.length) {