diff --git a/Tools/Replay/DataFlashFileReader.cpp b/Tools/Replay/DataFlashFileReader.cpp new file mode 100644 index 0000000000..911ba2f946 --- /dev/null +++ b/Tools/Replay/DataFlashFileReader.cpp @@ -0,0 +1,63 @@ +#include + +#include +#include +#include +#include +#include + +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); +} diff --git a/Tools/Replay/DataFlashFileReader.h b/Tools/Replay/DataFlashFileReader.h new file mode 100644 index 0000000000..a7e5df6f33 --- /dev/null +++ b/Tools/Replay/DataFlashFileReader.h @@ -0,0 +1,24 @@ +#ifndef REPLAY_DATAFLASHREADER_H +#define REPLAY_DATAFLASHREADER_H + +#include + +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 diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 4a4c2b5811..f5fa387dc1 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -14,7 +14,6 @@ #include #include #include -#include #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; istop_clock(last_timestamp_usec); } - MsgHandler *p = msgparser[f.type]; + LR_MsgHandler *p = msgparser[f.type]; if (p == NULL) { return true; } diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 47bb916f4a..64db2525dc 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -1,11 +1,10 @@ #include +#include -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 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(); diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index 6245bae573..e3c8bd33f6 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -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 #include -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"); diff --git a/Tools/Replay/MsgHandler.h b/Tools/Replay/MsgHandler.h index fff6303ae3..68ab030495 100644 --- a/Tools/Replay/MsgHandler.h +++ b/Tools/Replay/MsgHandler.h @@ -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 @@ -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 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) { };