aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Replay: added replay of GPS speed accuracy

Browse Source
This commit is contained in:
Andrew Tridgell 2016-05-04 18:16:25 +10:00
parent 4e5f1374da
commit 827551c99f
3 changed files with 81 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
Tools/Replay/LR_MsgHandler.cpp
View File

@ -128,7 +128,6 @@ void LR_MsgHandler_GPS2::process_message(uint8_t *msg)
update_from_msg_gps(1, msg);
}
void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg)
{
uint64_t time_us;
@ -184,6 +183,34 @@ void LR_MsgHandler_GPS::process_message(uint8_t *msg)
}
void LR_MsgHandler_GPA_Base::update_from_msg_gpa(uint8_t gps_offset, uint8_t *msg)
{
uint64_t time_us;
require_field(msg, "TimeUS", time_us);
wait_timestamp_usec(time_us);
uint16_t vdop, hacc, vacc, sacc;
require_field(msg, "VDop", vdop);
require_field(msg, "HAcc", hacc);
require_field(msg, "VAcc", vacc);
require_field(msg, "SAcc", sacc);
gps.setHIL_Accuracy(gps_offset, vdop*0.01f, hacc*0.01f, vacc*0.01f, sacc*0.01f);
}
void LR_MsgHandler_GPA::process_message(uint8_t *msg)
{
update_from_msg_gpa(0, msg);
}
void LR_MsgHandler_GPA2::process_message(uint8_t *msg)
{
update_from_msg_gpa(1, msg);
}
void LR_MsgHandler_IMU2::process_message(uint8_t *msg)
{
update_from_msg_imu(1, msg);

45
Tools/Replay/LR_MsgHandler.h
View File

@ -153,7 +153,6 @@ private:
uint32_t &ground_alt_cm;
};
class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base
{
public:
@ -190,6 +189,50 @@ private:
uint32_t &ground_alt_cm;
};
class LR_MsgHandler_GPA_Base : public LR_MsgHandler
{
public:
LR_MsgHandler_GPA_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps)
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), gps(_gps) { };
protected:
void update_from_msg_gpa(uint8_t imu_offset, uint8_t *data);
private:
AP_GPS &gps;
};
class LR_MsgHandler_GPA : public LR_MsgHandler_GPA_Base
{
public:
LR_MsgHandler_GPA(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps)
: LR_MsgHandler_GPA_Base(_f, _dataflash,_last_timestamp_usec,
_gps), gps(_gps) { };
void process_message(uint8_t *msg);
private:
AP_GPS &gps;
};
class LR_MsgHandler_GPA2 : public LR_MsgHandler_GPA_Base
{
public:
LR_MsgHandler_GPA2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps)
: LR_MsgHandler_GPA_Base(_f, _dataflash, _last_timestamp_usec,
_gps), gps(_gps) { };
virtual void process_message(uint8_t *msg);
private:
AP_GPS &gps;
};
class LR_MsgHandler_IMU_Base : public LR_MsgHandler

9
Tools/Replay/LogReader.cpp
View File

@ -178,6 +178,15 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], dataflash,
last_timestamp_usec,
gps, ground_alt_cm);
} else if (streq(name, "GPA")) {
msgparser[f.type] = new LR_MsgHandler_GPA(formats[f.type],
dataflash,
last_timestamp_usec,
gps);
} else if (streq(name, "GPA2")) {
msgparser[f.type] = new LR_MsgHandler_GPA2(formats[f.type], dataflash,
last_timestamp_usec,
gps);
} else if (streq(name, "MSG")) {
msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], dataflash,
last_timestamp_usec,