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

Replay: support IMT messages

This commit is contained in:
Andrew Tridgell 2015-06-15 18:13:23 +10:00
parent 727be87d84
commit c7cb980498
4 changed files with 127 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
Tools/Replay/LR_MsgHandler.cpp
View File

@ -203,6 +203,44 @@ void LR_MsgHandler_IMU::process_message(uint8_t *msg)
update_from_msg_imu(0, msg);
}
void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint8_t this_imu_mask = 1 << imu_offset;
float delta_time;
require_field(msg, "DelT", delta_time);
ins.set_delta_time(delta_time);
if (gyro_mask & this_imu_mask) {
Vector3f d_angle;
require_field(msg, "DelA", d_angle);
ins.set_delta_angle(imu_offset, d_angle);
}
if (accel_mask & this_imu_mask) {
float dvt;
require_field(msg, "DelvT", dvt);
Vector3f d_velocity;
require_field(msg, "DelV", d_velocity);
ins.set_delta_velocity(imu_offset, dvt, d_velocity);
}
}
void LR_MsgHandler_IMT::process_message(uint8_t *msg)
{
update_from_msg_imt(0, msg);
}
void LR_MsgHandler_IMT2::process_message(uint8_t *msg)
{
update_from_msg_imt(1, msg);
}
void LR_MsgHandler_IMT3::process_message(uint8_t *msg)
{
update_from_msg_imt(2, msg);
}
void LR_MsgHandler_MAG2::process_message(uint8_t *msg)
{

60
Tools/Replay/LR_MsgHandler.h
View File

@ -187,7 +187,7 @@ public:
accel_mask(_accel_mask),
gyro_mask(_gyro_mask),
ins(_ins) { };
void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg);
void update_from_msg_imu(uint8_t imu_offset, uint8_t *msg);
private:
uint8_t &accel_mask;
@ -235,6 +235,64 @@ public:
};
class LR_MsgHandler_IMT_Base : public LR_MsgHandler
{
public:
LR_MsgHandler_IMT_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins) :
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
accel_mask(_accel_mask),
gyro_mask(_gyro_mask),
ins(_ins) { };
void update_from_msg_imt(uint8_t imu_offset, uint8_t *msg);
private:
uint8_t &accel_mask;
uint8_t &gyro_mask;
AP_InertialSensor &ins;
};
class LR_MsgHandler_IMT : public LR_MsgHandler_IMT_Base
{
public:
LR_MsgHandler_IMT(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) { };
void process_message(uint8_t *msg);
};
class LR_MsgHandler_IMT2 : public LR_MsgHandler_IMT_Base
{
public:
LR_MsgHandler_IMT2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) { };
void process_message(uint8_t *msg);
};
class LR_MsgHandler_IMT3 : public LR_MsgHandler_IMT_Base
{
public:
LR_MsgHandler_IMT3(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: LR_MsgHandler_IMT_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) { };
void process_message(uint8_t *msg);
};
class LR_MsgHandler_MAG_Base : public LR_MsgHandler
{

12
Tools/Replay/LogReader.cpp
View File

@ -132,6 +132,18 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) {
msgparser[f.type] = new LR_MsgHandler_IMU3(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMT")) {
msgparser[f.type] = new LR_MsgHandler_IMT(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMT2")) {
msgparser[f.type] = new LR_MsgHandler_IMT2(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMT3")) {
msgparser[f.type] = new LR_MsgHandler_IMT3(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "SIM")) {
msgparser[f.type] = new LR_MsgHandler_SIM(formats[f.type], dataflash,
last_timestamp_usec,

20
Tools/Replay/Replay.cpp
View File

@ -114,8 +114,10 @@ private:
uint16_t update_rate = 0;
int32_t arm_time_ms = -1;
bool ahrs_healthy;
bool have_imu2;
bool have_fram;
bool have_imu2 = false;
bool have_imt = false;
bool have_imt2 = false;
bool have_fram = false;
void _parse_command_line(uint8_t argc, char * const argv[]);
@ -473,6 +475,12 @@ void Replay::read_sensors(const char *type)
if (streq(type,"IMU2")) {
have_imu2 = true;
}
if (streq(type,"IMT")) {
have_imt = true;
}
if (streq(type,"IMT2")) {
have_imt2 = true;
}
if (streq(type,"GPS")) {
gps.update();
@ -501,8 +509,16 @@ void Replay::read_sensors(const char *type)
run_ahrs = true;
}
if (have_imt) {
if ((streq(type,"IMT") && !have_imt2) ||
(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") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) {
run_ahrs = true;
}