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

Replay: split off a base class for reading dataflash logs

Browse Source
This commit is contained in:
Peter Barker 2015-06-12 22:52:52 +10:00
parent 0b5e645075
commit d41b21cd19
6 changed files with 239 additions and 188 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
Tools/Replay/DataFlashFileReader.cpp Normal file
View File

@ -0,0 +1,63 @@
#include <DataFlashFileReader.h>
#include <fcntl.h>
#include <string.h>
#include <sys/types.h>
#include <stdio.h>
#include <unistd.h>
DataFlashFileReader::DataFlashFileReader() : fd(-1) {}
bool DataFlashFileReader::open_log(const char *logfile)
{
fd = ::open(logfile, O_RDONLY);
if (fd == -1) {
return false;
}
return true;
}
bool DataFlashFileReader::update(char type[5])
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format f;
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
memcpy(&formats[f.type], &f, sizeof(formats[f.type]));
strncpy(type, "FMT", 3);
type[3] = 0;
return handle_log_format_msg(f);
}
const struct log_Format &f = formats[hdr[2]];
if (f.length == 0) {
// can't just throw these away as the format specifies the
// number of bytes in the message
::printf("No format defined for type (%d)\n", hdr[2]);
exit(1);
}
uint8_t msg[f.length];
memcpy(msg, hdr, 3);
if (::read(fd, &msg[3], f.length-3) != f.length-3) {
return false;
}
strncpy(type, f.name, 4);
type[4] = 0;
return handle_msg(f,msg);
}

24
Tools/Replay/DataFlashFileReader.h Normal file
View File

@ -0,0 +1,24 @@
#ifndef REPLAY_DATAFLASHREADER_H
#define REPLAY_DATAFLASHREADER_H
#include <DataFlash.h>
class DataFlashFileReader
{
public:
DataFlashFileReader();
bool open_log(const char *logfile);
bool update(char type[5]);
virtual bool handle_log_format_msg(const struct log_Format &f) = 0;
virtual bool handle_msg(const struct log_Format &f, uint8_t *msg) = 0;
protected:
int fd;
#define LOGREADER_MAX_FORMATS 255 // must be >= highest MESSAGE
struct log_Format formats[LOGREADER_MAX_FORMATS];
};
#endif

102
Tools/Replay/LogReader.cpp
View File

@ -14,7 +14,6 @@
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include "MsgHandler.h"
@ -24,7 +23,6 @@ extern const AP_HAL::HAL& hal;
LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash) :
vehicle(VehicleType::VEHICLE_UNKNOWN),
fd(-1),
ahrs(_ahrs),
ins(_ins),
baro(_baro),
@ -38,15 +36,6 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Co
installed_vehicle_specific_parsers(false)
{}
bool LogReader::open_log(const char *logfile)
{
fd = ::open(logfile, O_RDONLY);
if (fd == -1) {
return false;
}
return true;
}
struct log_Format deferred_formats[LOGREADER_MAX_FORMATS];
// some log entries (e.g. "NTUN") are used by the different vehicle
@ -61,7 +50,7 @@ void LogReader::maybe_install_vehicle_specific_parsers() {
case VehicleType::VEHICLE_COPTER:
for (uint8_t i = 0; i<LOGREADER_MAX_FORMATS; i++) {
if (deferred_formats[i].type != 0) {
msgparser[i] = new MsgHandler_NTUN_Copter
msgparser[i] = new LR_MsgHandler_NTUN_Copter
(deferred_formats[i], dataflash, last_timestamp_usec,
inavpos);
}
@ -78,12 +67,12 @@ void LogReader::maybe_install_vehicle_specific_parsers() {
}
}
MsgHandler_PARM *parameter_handler;
LR_MsgHandler_PARM *parameter_handler;
/*
messages which we will be generating, so should be discarded
*/
static const char *generated_types[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5",
static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5",
"AHR2", "POS", NULL };
/*
@ -99,33 +88,13 @@ bool LogReader::in_list(const char *type, const char *list[])
return false;
}
bool LogReader::update(char type[5])
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format f;
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
memcpy(&formats[f.type], &f, sizeof(formats[f.type]));
strncpy(type, f.name, 4);
type[4] = 0;
bool LogReader::handle_log_format_msg(const struct log_Format &f) {
char name[5];
memset(name, '\0', 5);
memcpy(name, f.name, 4);
::printf("Defining log format for type (%d) (%s)\n", f.type, name);
if (!in_list(type, generated_types)) {
if (!in_list(name, generated_names)) {
// any messages which we won't be generating internally in
// replay should get the original FMT header
dataflash.WriteBlock(&f, sizeof(f));
@ -133,64 +102,64 @@ bool LogReader::update(char type[5])
// map from format name to a parser subclass:
if (streq(name, "PARM")) {
parameter_handler = new MsgHandler_PARM(formats[f.type], dataflash,
parameter_handler = new LR_MsgHandler_PARM(formats[f.type], dataflash,
last_timestamp_usec);
msgparser[f.type] = parameter_handler;
} else if (streq(name, "GPS")) {
msgparser[f.type] = new MsgHandler_GPS(formats[f.type],
msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type],
dataflash,
last_timestamp_usec,
gps, ground_alt_cm,
rel_altitude);
} else if (streq(name, "GPS2")) {
msgparser[f.type] = new MsgHandler_GPS2(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], dataflash,
last_timestamp_usec,
gps, ground_alt_cm,
rel_altitude);
} else if (streq(name, "MSG")) {
msgparser[f.type] = new MsgHandler_MSG(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], dataflash,
last_timestamp_usec,
vehicle, ahrs);
} else if (streq(name, "IMU")) {
msgparser[f.type] = new MsgHandler_IMU(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_IMU(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMU2")) {
msgparser[f.type] = new MsgHandler_IMU2(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_IMU2(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMU3")) {
msgparser[f.type] = new MsgHandler_IMU3(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_IMU3(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "SIM")) {
msgparser[f.type] = new MsgHandler_SIM(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_SIM(formats[f.type], dataflash,
last_timestamp_usec,
sim_attitude);
} else if (streq(name, "BARO")) {
msgparser[f.type] = new MsgHandler_BARO(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_BARO(formats[f.type], dataflash,
last_timestamp_usec, baro);
} else if (streq(name, "ARM")) {
msgparser[f.type] = new MsgHandler_ARM(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_ARM(formats[f.type], dataflash,
last_timestamp_usec);
} else if (streq(name, "EV")) {
msgparser[f.type] = new MsgHandler_Event(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_Event(formats[f.type], dataflash,
last_timestamp_usec);
} else if (streq(name, "AHR2")) {
msgparser[f.type] = new MsgHandler_AHR2(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_AHR2(formats[f.type], dataflash,
last_timestamp_usec,
ahr2_attitude);
} else if (streq(name, "ATT")) {
// this parser handles *all* attitude messages - the common one,
// and also the rover/copter/plane-specific (old) messages
msgparser[f.type] = new MsgHandler_ATT(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_ATT(formats[f.type], dataflash,
last_timestamp_usec,
attitude);
} else if (streq(name, "MAG")) {
msgparser[f.type] = new MsgHandler_MAG(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_MAG(formats[f.type], dataflash,
last_timestamp_usec, compass);
} else if (streq(name, "MAG2")) {
msgparser[f.type] = new MsgHandler_MAG2(formats[f.type], dataflash,
msgparser[f.type] = new LR_MsgHandler_MAG2(formats[f.type], dataflash,
last_timestamp_usec, compass);
} else if (streq(name, "NTUN")) {
// the label "NTUN" is used by rover, copter and plane -
@ -200,38 +169,25 @@ bool LogReader::update(char type[5])
memcpy(&deferred_formats[f.type], &formats[f.type],
sizeof(struct log_Format));
} else if (streq(name, "ARSP")) { // plane-specific(?!)
msgparser[f.type] = new MsgHandler_ARSP(formats[f.type], dataflash,
msgparser[f.type] = new LR_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,
msgparser[f.type] = new LR_MsgHandler_FRAM(formats[f.type], dataflash,
last_timestamp_usec);
} else {
::printf(" No parser for (%s)\n", name);
}
return true;
}
}
const struct log_Format &f = formats[hdr[2]];
if (f.length == 0) {
// can't just throw these away as the format specifies the
// number of bytes in the message
::printf("No format defined for type (%d)\n", hdr[2]);
exit(1);
}
bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
char name[5];
memset(name, '\0', 5);
memcpy(name, f.name, 4);
uint8_t msg[f.length];
memcpy(msg, hdr, 3);
if (::read(fd, &msg[3], f.length-3) != f.length-3) {
return false;
}
strncpy(type, f.name, 4);
type[4] = 0;
if (!in_list(type, generated_types)) {
if (!in_list(name, generated_names)) {
dataflash.WriteBlock(msg, f.length);
// a MsgHandler would probably have found a timestamp and
// caled stop_clock. This runs IO, clearing dataflash's
@ -239,7 +195,7 @@ bool LogReader::update(char type[5])
hal.scheduler->stop_clock(last_timestamp_usec);
}
MsgHandler *p = msgparser[f.type];
LR_MsgHandler *p = msgparser[f.type];
if (p == NULL) {
return true;
}

14
Tools/Replay/LogReader.h
View File

@ -1,11 +1,10 @@
#include <VehicleType.h>
#include <DataFlashFileReader.h>
class LogReader
class LogReader : public DataFlashFileReader
{
public:
LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash);
bool open_log(const char *logfile);
bool update(char type[5]);
bool wait_type(const char *type);
const Vector3f &get_attitude(void) const { return attitude; }
@ -22,9 +21,10 @@ public:
void set_gyro_mask(uint8_t mask) { gyro_mask = mask; }
uint64_t last_timestamp_us(void) const { return last_timestamp_usec; }
virtual bool handle_log_format_msg(const struct log_Format &f);
virtual bool handle_msg(const struct log_Format &f, uint8_t *msg);
private:
int fd;
AP_AHRS &ahrs;
AP_InertialSensor &ins;
AP_Baro &baro;
@ -41,9 +41,7 @@ private:
uint32_t ground_alt_cm;
#define LOGREADER_MAX_FORMATS 255 // must be >= highest MESSAGE
struct log_Format formats[LOGREADER_MAX_FORMATS];
class MsgHandler *msgparser[LOGREADER_MAX_FORMATS];
class LR_MsgHandler *msgparser[LOGREADER_MAX_FORMATS];
template <typename R>
void require_field(class MsgHandler *p, uint8_t *msg, const char *label, R &ret);
@ -83,8 +81,6 @@ private:
void wait_timestamp(uint32_t timestamp);
void update_rover(uint8_t type, uint8_t *data, uint16_t length);
bool installed_vehicle_specific_parsers;
void maybe_install_vehicle_specific_parsers();

60
Tools/Replay/MsgHandler.cpp
View File

@ -60,14 +60,18 @@ struct MsgHandler::format_field_info *MsgHandler::find_field_info(const char *la
return NULL;
}
MsgHandler::MsgHandler(struct log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: next_field(0), f(_f), dataflash(_dataflash), last_timestamp_usec(_last_timestamp_usec)
MsgHandler::MsgHandler(const struct log_Format &_f) : next_field(0), f(_f)
{
init_field_types();
parse_format_fields();
}
LR_MsgHandler::LR_MsgHandler(struct log_Format &_f,
DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec) :
dataflash(_dataflash), last_timestamp_usec(_last_timestamp_usec),
MsgHandler(_f) {
}
void MsgHandler::add_field(const char *_label, uint8_t _type, uint8_t _offset,
uint8_t _length)
@ -195,13 +199,13 @@ MsgHandler::~MsgHandler()
}
}
void MsgHandler::wait_timestamp_usec(uint64_t timestamp)
void LR_MsgHandler::wait_timestamp_usec(uint64_t timestamp)
{
last_timestamp_usec = timestamp;
hal.scheduler->stop_clock(timestamp);
}
void MsgHandler::wait_timestamp(uint32_t timestamp)
void LR_MsgHandler::wait_timestamp(uint32_t timestamp)
{
uint64_t usecs = timestamp*1000UL;
wait_timestamp_usec(usecs);
@ -293,7 +297,7 @@ int16_t MsgHandler::require_field_int16_t(uint8_t *msg, const char *label)
return ret;
}
void MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
void LR_MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
{
uint64_t time_us;
uint64_t time_ms;
@ -316,14 +320,14 @@ void MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
* subclasses to handle specific messages below here
*/
void MsgHandler_AHR2::process_message(uint8_t *msg)
void LR_MsgHandler_AHR2::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw");
}
void MsgHandler_ARM::process_message(uint8_t *msg)
void LR_MsgHandler_ARM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint8_t ArmState = require_field_uint8_t(msg, "ArmState");
@ -334,7 +338,7 @@ void MsgHandler_ARM::process_message(uint8_t *msg)
}
void MsgHandler_ARSP::process_message(uint8_t *msg)
void LR_MsgHandler_ARSP::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
@ -343,20 +347,20 @@ void MsgHandler_ARSP::process_message(uint8_t *msg)
require_field_float(msg, "Temp"));
}
void MsgHandler_FRAM::process_message(uint8_t *msg)
void LR_MsgHandler_FRAM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
}
void MsgHandler_ATT::process_message(uint8_t *msg)
void LR_MsgHandler_ATT::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw");
}
void MsgHandler_BARO::process_message(uint8_t *msg)
void LR_MsgHandler_BARO::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
baro.setHIL(0,
@ -368,7 +372,7 @@ void MsgHandler_BARO::process_message(uint8_t *msg)
#define DATA_ARMED 10
#define DATA_DISARMED 11
void MsgHandler_Event::process_message(uint8_t *msg)
void LR_MsgHandler_Event::process_message(uint8_t *msg)
{
uint8_t id = require_field_uint8_t(msg, "Id");
if (id == DATA_ARMED) {
@ -383,7 +387,7 @@ void MsgHandler_Event::process_message(uint8_t *msg)
}
void MsgHandler_GPS2::process_message(uint8_t *msg)
void LR_MsgHandler_GPS2::process_message(uint8_t *msg)
{
// only LOG_GPS_MSG gives us relative altitude. We still log
// the relative altitude when we get a LOG_GPS2_MESSAGE - but
@ -393,7 +397,7 @@ void MsgHandler_GPS2::process_message(uint8_t *msg)
}
void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt)
void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt)
{
uint64_t time_us;
if (! field_value(msg, "TimeUS", time_us)) {
@ -433,25 +437,25 @@ void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg,
void MsgHandler_GPS::process_message(uint8_t *msg)
void LR_MsgHandler_GPS::process_message(uint8_t *msg)
{
update_from_msg_gps(0, msg, true);
}
void MsgHandler_IMU2::process_message(uint8_t *msg)
void LR_MsgHandler_IMU2::process_message(uint8_t *msg)
{
update_from_msg_imu(1, msg);
}
void MsgHandler_IMU3::process_message(uint8_t *msg)
void LR_MsgHandler_IMU3::process_message(uint8_t *msg)
{
update_from_msg_imu(2, msg);
}
void MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
void LR_MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
@ -470,19 +474,19 @@ void MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
}
void MsgHandler_IMU::process_message(uint8_t *msg)
void LR_MsgHandler_IMU::process_message(uint8_t *msg)
{
update_from_msg_imu(0, msg);
}
void MsgHandler_MAG2::process_message(uint8_t *msg)
void LR_MsgHandler_MAG2::process_message(uint8_t *msg)
{
update_from_msg_compass(1, msg);
}
void MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
void LR_MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
@ -499,7 +503,7 @@ void MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_
void MsgHandler_MAG::process_message(uint8_t *msg)
void LR_MsgHandler_MAG::process_message(uint8_t *msg)
{
update_from_msg_compass(0, msg);
}
@ -507,7 +511,7 @@ void MsgHandler_MAG::process_message(uint8_t *msg)
#include <AP_AHRS.h>
#include <VehicleType.h>
void MsgHandler_MSG::process_message(uint8_t *msg)
void LR_MsgHandler_MSG::process_message(uint8_t *msg)
{
const uint8_t msg_text_len = 64;
char msg_text[msg_text_len];
@ -533,7 +537,7 @@ void MsgHandler_MSG::process_message(uint8_t *msg)
}
void MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
void LR_MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
{
inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f,
require_field_float(msg, "PosY") * 0.01f,
@ -541,7 +545,7 @@ void MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
}
bool MsgHandler::set_parameter(const char *name, float value)
bool LR_MsgHandler::set_parameter(const char *name, float value)
{
const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE",
"COMPASS_ORIENT", "COMPASS_ORIENT2",
@ -576,7 +580,7 @@ bool MsgHandler::set_parameter(const char *name, float value)
return true;
}
void MsgHandler_PARM::process_message(uint8_t *msg)
void LR_MsgHandler_PARM::process_message(uint8_t *msg)
{
const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term
char parameter_name[parameter_name_len];
@ -599,7 +603,7 @@ void MsgHandler_PARM::process_message(uint8_t *msg)
}
void MsgHandler_SIM::process_message(uint8_t *msg)
void LR_MsgHandler_SIM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, sim_attitude, "Roll", "Pitch", "Yaw");

164
Tools/Replay/MsgHandler.h
View File

@ -13,14 +13,11 @@
class MsgHandler {
public:
// constructor - create a parser for a MavLink message format
MsgHandler(struct log_Format &f, DataFlash_Class &_dataflash,
uint64_t &last_timestamp_usec);
MsgHandler(const struct log_Format &f);
// retrieve a comma-separated list of all labels
void string_for_labels(char *buffer, uint bufferlen);
virtual void process_message(uint8_t *msg) = 0;
// field_value - retrieve the value of a field from the supplied message
// these return false if the field was not found
template<typename R>
@ -44,8 +41,6 @@ public:
uint16_t require_field_uint16_t(uint8_t *msg, const char *label);
int16_t require_field_int16_t(uint8_t *msg, const char *label);
bool set_parameter(const char *name, float value);
private:
void add_field(const char *_label, uint8_t _type, uint8_t _offset,
@ -77,10 +72,6 @@ private:
protected:
struct log_Format f; // the format we are a parser for
~MsgHandler();
void wait_timestamp(uint32_t timestamp);
void wait_timestamp_usec(uint64_t timestamp);
uint64_t &last_timestamp_usec;
void location_from_msg(uint8_t *msg, Location &loc, const char *label_lat,
const char *label_long, const char *label_alt);
@ -90,8 +81,6 @@ protected:
const char *label_speed,
const char *label_course,
const char *label_vz);
DataFlash_Class &dataflash;
void wait_timestamp_from_msg(uint8_t *msg);
void attitude_from_msg(uint8_t *msg,
Vector3f &att,
@ -100,6 +89,25 @@ protected:
const char *label_yaw);
};
class LR_MsgHandler : public MsgHandler {
public:
LR_MsgHandler(struct log_Format &f,
DataFlash_Class &_dataflash,
uint64_t &last_timestamp_usec);
virtual void process_message(uint8_t *msg) = 0;
bool set_parameter(const char *name, float value);
protected:
DataFlash_Class &dataflash;
void wait_timestamp(uint32_t timestamp);
void wait_timestamp_usec(uint64_t timestamp);
void wait_timestamp_from_msg(uint8_t *msg);
uint64_t &last_timestamp_usec;
};
template<typename R>
bool MsgHandler::field_value(uint8_t *msg, const char *label, R &ret)
{
@ -167,12 +175,12 @@ inline void MsgHandler::field_value_for_type_at_offset(uint8_t *msg,
/* subclasses below this point */
class MsgHandler_AHR2 : public MsgHandler
class LR_MsgHandler_AHR2 : public LR_MsgHandler
{
public:
MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude)
: MsgHandler(_f, _dataflash,_last_timestamp_usec),
: LR_MsgHandler(_f, _dataflash,_last_timestamp_usec),
ahr2_attitude(_ahr2_attitude) { };
virtual void process_message(uint8_t *msg);
@ -182,23 +190,23 @@ private:
};
class MsgHandler_ARM : public MsgHandler
class LR_MsgHandler_ARM : public LR_MsgHandler
{
public:
MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};
class MsgHandler_ARSP : public MsgHandler
class LR_MsgHandler_ARSP : public LR_MsgHandler
{
public:
MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) :
MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { };
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { };
virtual void process_message(uint8_t *msg);
@ -206,23 +214,23 @@ private:
AP_Airspeed &airspeed;
};
class MsgHandler_FRAM : public MsgHandler
class LR_MsgHandler_FRAM : public LR_MsgHandler
{
public:
MsgHandler_FRAM(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_FRAM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec) :
MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};
class MsgHandler_ATT : public MsgHandler
class LR_MsgHandler_ATT : public LR_MsgHandler
{
public:
MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_attitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude)
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude)
{ };
virtual void process_message(uint8_t *msg);
@ -231,12 +239,12 @@ private:
};
class MsgHandler_BARO : public MsgHandler
class LR_MsgHandler_BARO : public LR_MsgHandler
{
public:
MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_Baro &_baro)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { };
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { };
virtual void process_message(uint8_t *msg);
@ -245,12 +253,12 @@ private:
};
class MsgHandler_Event : public MsgHandler
class LR_MsgHandler_Event : public LR_MsgHandler
{
public:
MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};
@ -258,14 +266,14 @@ public:
class MsgHandler_GPS_Base : public MsgHandler
class LR_MsgHandler_GPS_Base : public LR_MsgHandler
{
public:
MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec),
LR_MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
gps(_gps), ground_alt_cm(_ground_alt_cm),
rel_altitude(_rel_altitude) { };
@ -279,13 +287,13 @@ private:
};
class MsgHandler_GPS : public MsgHandler_GPS_Base
class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base
{
public:
MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
: LR_MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
_gps, _ground_alt_cm, _rel_altitude),
gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
@ -301,15 +309,15 @@ private:
// (and other packets, too...). I*think* the contructor can simply
// take e.g. &gps[1]... problems are going to arise if we don't
// actually have that many gps' compiled in!
class MsgHandler_GPS2 : public MsgHandler_GPS_Base
class LR_MsgHandler_GPS2 : public LR_MsgHandler_GPS_Base
{
public:
MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
_gps, _ground_alt_cm,
_rel_altitude), gps(_gps),
: LR_MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
_gps, _ground_alt_cm,
_rel_altitude), gps(_gps),
ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
virtual void process_message(uint8_t *msg);
private:
@ -320,14 +328,14 @@ private:
class MsgHandler_IMU_Base : public MsgHandler
class LR_MsgHandler_IMU_Base : public LR_MsgHandler
{
public:
MsgHandler_IMU_Base(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_IMU_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins) :
MsgHandler(_f, _dataflash, _last_timestamp_usec),
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
accel_mask(_accel_mask),
gyro_mask(_gyro_mask),
ins(_ins) { };
@ -339,40 +347,40 @@ private:
AP_InertialSensor &ins;
};
class MsgHandler_IMU : public MsgHandler_IMU_Base
class LR_MsgHandler_IMU : public LR_MsgHandler_IMU_Base
{
public:
MsgHandler_IMU(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_IMU(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
: LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) { };
void process_message(uint8_t *msg);
};
class MsgHandler_IMU2 : public MsgHandler_IMU_Base
class LR_MsgHandler_IMU2 : public LR_MsgHandler_IMU_Base
{
public:
MsgHandler_IMU2(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_IMU2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
: LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_IMU3 : public MsgHandler_IMU_Base
class LR_MsgHandler_IMU3 : public LR_MsgHandler_IMU_Base
{
public:
MsgHandler_IMU3(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_IMU3(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
: LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) {};
virtual void process_message(uint8_t *msg);
@ -380,12 +388,12 @@ public:
class MsgHandler_MAG_Base : public MsgHandler
class LR_MsgHandler_MAG_Base : public LR_MsgHandler
{
public:
MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { };
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { };
protected:
void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg);
@ -394,35 +402,35 @@ private:
Compass &compass;
};
class MsgHandler_MAG : public MsgHandler_MAG_Base
class LR_MsgHandler_MAG : public LR_MsgHandler_MAG_Base
{
public:
MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
: LR_MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_MAG2 : public MsgHandler_MAG_Base
class LR_MsgHandler_MAG2 : public LR_MsgHandler_MAG_Base
{
public:
MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
: LR_MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_MSG : public MsgHandler
class LR_MsgHandler_MSG : public LR_MsgHandler
{
public:
MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) :
MsgHandler(_f, _dataflash, _last_timestamp_usec),
LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
vehicle(_vehicle), ahrs(_ahrs) { }
@ -434,12 +442,12 @@ private:
};
class MsgHandler_NTUN_Copter : public MsgHandler
class LR_MsgHandler_NTUN_Copter : public LR_MsgHandler
{
public:
MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_inavpos)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {};
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {};
virtual void process_message(uint8_t *msg);
@ -448,22 +456,22 @@ private:
};
class MsgHandler_PARM : public MsgHandler
class LR_MsgHandler_PARM : public LR_MsgHandler
{
public:
MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t &_last_timestamp_usec) : MsgHandler(_f, _dataflash, _last_timestamp_usec) {};
LR_MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t &_last_timestamp_usec) : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_SIM : public MsgHandler
class LR_MsgHandler_SIM : public LR_MsgHandler
{
public:
MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash,
LR_MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
Vector3f &_sim_attitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec),
: LR_MsgHandler(_f, _dataflash, _last_timestamp_usec),
sim_attitude(_sim_attitude)
{ };