Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Replay: cope with FRAM messages

Browse Source 
use them to trigger ahrs updates
This commit is contained in:
Andrew Tridgell 2015-05-27 21:20:38 +10:00
parent 6b65aa465b
commit 2007d2b6eb
4 changed files with 34 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/Replay/LogReader.cpp
View File

@ -200,6 +200,9 @@ bool LogReader::update(char type[5])
msgparser[f.type] = new MsgHandler_ARSP(formats[f.type], dataflash,
last_timestamp_usec,
airspeed);
} else if (streq(name, "FRAM")) {
msgparser[f.type] = new MsgHandler_FRAM(formats[f.type], dataflash,
last_timestamp_usec);
} else {
::printf(" No parser for (%s)\n", name);
}

6
Tools/Replay/MsgHandler.cpp
View File

@ -345,6 +345,12 @@ void MsgHandler_ARSP::process_message(uint8_t *msg)
dataflash.WriteBlock(msg, f.length);
}
void MsgHandler_FRAM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
dataflash.WriteBlock(msg, f.length);
}
void MsgHandler_ATT::process_message(uint8_t *msg)
{

10
Tools/Replay/MsgHandler.h
View File

@ -206,6 +206,16 @@ private:
AP_Airspeed &airspeed;
};
class MsgHandler_FRAM : public MsgHandler
{
public:
MsgHandler_FRAM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec) :
MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};
class MsgHandler_ATT : public MsgHandler
{

16
Tools/Replay/Replay.pde
View File

@ -103,6 +103,7 @@ static uint16_t update_rate = 50;
static uint32_t arm_time_ms;
static bool ahrs_healthy;
static bool have_imu2;
static bool have_fram;
static uint8_t num_user_parameters;
static struct {
@ -316,10 +317,23 @@ static void read_sensors(const char *type)
::printf("Barometer initialised\n");
barometer.update_calibration();
}
}
bool run_ahrs = false;
if (streq(type,"FRAM")) {
if (!have_fram) {
have_fram = true;
printf("Have FRAM framing\n");
}
run_ahrs = true;
}
// special handling of IMU messages as these trigger an ahrs.update()
if ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2)) {
if (!have_fram &&
((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) {
run_ahrs = true;
}
if (run_ahrs) {
ahrs.update();
if (ahrs.get_home().lat != 0) {
inertial_nav.update(ins.get_delta_time());