From 91b761cfb06d02d76ca58ae31ec9158f4b40a7e6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 18 May 2015 20:00:30 +1000 Subject: [PATCH] Replay: merge MessageHandler_* into MsgHandler.h/MsgHandler.cpp These were trivial classes; having lots of these files around made it painful to work with. --- Tools/Replay/LogReader.cpp | 17 -- Tools/Replay/MsgHandler.cpp | 280 ++++++++++++++++++++++ Tools/Replay/MsgHandler.h | 300 ++++++++++++++++++++++++ Tools/Replay/MsgHandler_AHR2.cpp | 7 - Tools/Replay/MsgHandler_AHR2.h | 15 -- Tools/Replay/MsgHandler_ARM.cpp | 14 -- Tools/Replay/MsgHandler_ARM.h | 11 - Tools/Replay/MsgHandler_ARSP.cpp | 11 - Tools/Replay/MsgHandler_ARSP.h | 14 -- Tools/Replay/MsgHandler_ATT.cpp | 7 - Tools/Replay/MsgHandler_ATT.h | 14 -- Tools/Replay/MsgHandler_BARO.cpp | 10 - Tools/Replay/MsgHandler_BARO.h | 14 -- Tools/Replay/MsgHandler_Event.cpp | 21 -- Tools/Replay/MsgHandler_Event.h | 11 - Tools/Replay/MsgHandler_GPS.cpp | 6 - Tools/Replay/MsgHandler_GPS.h | 19 -- Tools/Replay/MsgHandler_GPS2.cpp | 10 - Tools/Replay/MsgHandler_GPS2.h | 22 -- Tools/Replay/MsgHandler_GPS_Base.cpp | 34 --- Tools/Replay/MsgHandler_GPS_Base.h | 27 --- Tools/Replay/MsgHandler_IMU.cpp | 6 - Tools/Replay/MsgHandler_IMU.h | 14 -- Tools/Replay/MsgHandler_IMU2.cpp | 6 - Tools/Replay/MsgHandler_IMU2.h | 14 -- Tools/Replay/MsgHandler_IMU3.cpp | 6 - Tools/Replay/MsgHandler_IMU3.h | 14 -- Tools/Replay/MsgHandler_IMU_Base.cpp | 21 -- Tools/Replay/MsgHandler_IMU_Base.h | 26 -- Tools/Replay/MsgHandler_MAG.cpp | 6 - Tools/Replay/MsgHandler_MAG.h | 11 - Tools/Replay/MsgHandler_MAG2.cpp | 6 - Tools/Replay/MsgHandler_MAG2.h | 11 - Tools/Replay/MsgHandler_MAG_Base.cpp | 19 -- Tools/Replay/MsgHandler_MAG_Base.h | 20 -- Tools/Replay/MsgHandler_MSG.cpp | 29 --- Tools/Replay/MsgHandler_MSG.h | 19 -- Tools/Replay/MsgHandler_NTUN_Copter.cpp | 8 - Tools/Replay/MsgHandler_NTUN_Copter.h | 14 -- Tools/Replay/MsgHandler_PARM.cpp | 44 ---- Tools/Replay/MsgHandler_PARM.h | 9 - Tools/Replay/MsgHandler_SIM.cpp | 7 - Tools/Replay/MsgHandler_SIM.h | 17 -- 43 files changed, 580 insertions(+), 611 deletions(-) delete mode 100644 Tools/Replay/MsgHandler_AHR2.cpp delete mode 100644 Tools/Replay/MsgHandler_AHR2.h delete mode 100644 Tools/Replay/MsgHandler_ARM.cpp delete mode 100644 Tools/Replay/MsgHandler_ARM.h delete mode 100644 Tools/Replay/MsgHandler_ARSP.cpp delete mode 100644 Tools/Replay/MsgHandler_ARSP.h delete mode 100644 Tools/Replay/MsgHandler_ATT.cpp delete mode 100644 Tools/Replay/MsgHandler_ATT.h delete mode 100644 Tools/Replay/MsgHandler_BARO.cpp delete mode 100644 Tools/Replay/MsgHandler_BARO.h delete mode 100644 Tools/Replay/MsgHandler_Event.cpp delete mode 100644 Tools/Replay/MsgHandler_Event.h delete mode 100644 Tools/Replay/MsgHandler_GPS.cpp delete mode 100644 Tools/Replay/MsgHandler_GPS.h delete mode 100644 Tools/Replay/MsgHandler_GPS2.cpp delete mode 100644 Tools/Replay/MsgHandler_GPS2.h delete mode 100644 Tools/Replay/MsgHandler_GPS_Base.cpp delete mode 100644 Tools/Replay/MsgHandler_GPS_Base.h delete mode 100644 Tools/Replay/MsgHandler_IMU.cpp delete mode 100644 Tools/Replay/MsgHandler_IMU.h delete mode 100644 Tools/Replay/MsgHandler_IMU2.cpp delete mode 100644 Tools/Replay/MsgHandler_IMU2.h delete mode 100644 Tools/Replay/MsgHandler_IMU3.cpp delete mode 100644 Tools/Replay/MsgHandler_IMU3.h delete mode 100644 Tools/Replay/MsgHandler_IMU_Base.cpp delete mode 100644 Tools/Replay/MsgHandler_IMU_Base.h delete mode 100644 Tools/Replay/MsgHandler_MAG.cpp delete mode 100644 Tools/Replay/MsgHandler_MAG.h delete mode 100644 Tools/Replay/MsgHandler_MAG2.cpp delete mode 100644 Tools/Replay/MsgHandler_MAG2.h delete mode 100644 Tools/Replay/MsgHandler_MAG_Base.cpp delete mode 100644 Tools/Replay/MsgHandler_MAG_Base.h delete mode 100644 Tools/Replay/MsgHandler_MSG.cpp delete mode 100644 Tools/Replay/MsgHandler_MSG.h delete mode 100644 Tools/Replay/MsgHandler_NTUN_Copter.cpp delete mode 100644 Tools/Replay/MsgHandler_NTUN_Copter.h delete mode 100644 Tools/Replay/MsgHandler_PARM.cpp delete mode 100644 Tools/Replay/MsgHandler_PARM.h delete mode 100644 Tools/Replay/MsgHandler_SIM.cpp delete mode 100644 Tools/Replay/MsgHandler_SIM.h diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index b744470bcc..d6d199d280 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -17,23 +17,6 @@ #include #include "MsgHandler.h" -#include "MsgHandler_PARM.h" -#include "MsgHandler_GPS.h" -#include "MsgHandler_GPS2.h" -#include "MsgHandler_MSG.h" -#include "MsgHandler_IMU.h" -#include "MsgHandler_IMU2.h" -#include "MsgHandler_IMU3.h" -#include "MsgHandler_SIM.h" -#include "MsgHandler_BARO.h" -#include "MsgHandler_ARM.h" -#include "MsgHandler_Event.h" -#include "MsgHandler_AHR2.h" -#include "MsgHandler_ATT.h" -#include "MsgHandler_MAG.h" -#include "MsgHandler_MAG2.h" -#include "MsgHandler_NTUN_Copter.h" -#include "MsgHandler_ARSP.h" #define streq(x, y) (!strcmp(x, y)) diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index d3031336a5..edb340338d 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -287,3 +287,283 @@ void MsgHandler::wait_timestamp_from_msg(uint8_t *msg) require_field(msg, "TimeMS", timestamp); wait_timestamp(timestamp); } + + + +/* + * subclasses to handle specific messages below here +*/ + +void 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) +{ + wait_timestamp_from_msg(msg); + uint8_t ArmState = require_field_uint8_t(msg, "ArmState"); + hal.util->set_soft_armed(ArmState); + printf("Armed state: %u at %lu\n", + (unsigned)ArmState, + (unsigned long)hal.scheduler->millis()); + dataflash.WriteBlock(msg, f.length); +} + + +void MsgHandler_ARSP::process_message(uint8_t *msg) +{ + wait_timestamp_from_msg(msg); + + airspeed.setHIL(require_field_float(msg, "Airspeed"), + require_field_float(msg, "DiffPress"), + require_field_float(msg, "Temp")); + dataflash.WriteBlock(msg, f.length); +} + + +void 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) +{ + wait_timestamp_from_msg(msg); + baro.setHIL(0, + require_field_float(msg, "Press"), + require_field_int16_t(msg, "Temp") * 0.01f); + dataflash.WriteBlock(msg, f.length); +} + + +#define DATA_ARMED 10 +#define DATA_DISARMED 11 + +void MsgHandler_Event::process_message(uint8_t *msg) +{ + uint8_t id = require_field_uint8_t(msg, "Id"); + if (id == DATA_ARMED) { + hal.util->set_soft_armed(true); + printf("Armed at %lu\n", + (unsigned long)hal.scheduler->millis()); + } else if (id == DATA_DISARMED) { + hal.util->set_soft_armed(false); + printf("Disarmed at %lu\n", + (unsigned long)hal.scheduler->millis()); + } + dataflash.WriteBlock(msg, f.length); +} + + +void 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 + // the value we use (probably) comes from the most recent + // LOG_GPS_MESSAGE message! + update_from_msg_gps(1, msg, false); +} + + +void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt) +{ + uint32_t timestamp; + require_field(msg, "T", timestamp); + wait_timestamp(timestamp); + + Location loc; + location_from_msg(msg, loc, "Lat", "Lng", "Alt"); + Vector3f vel; + ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ"); + + uint8_t status = require_field_uint8_t(msg, "Status"); + gps.setHIL(gps_offset, + (AP_GPS::GPS_Status)status, + timestamp, + loc, + vel, + require_field_uint8_t(msg, "NSats"), + require_field_uint8_t(msg, "HDop"), + require_field_float(msg, "VZ") != 0); + if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) { + ground_alt_cm = require_field_int32_t(msg, "Alt"); + } + + if (responsible_for_relalt) { + // this could possibly check for the presence of "RelAlt" label? + rel_altitude = 0.01f * require_field_int32_t(msg, "RelAlt"); + } + + dataflash.WriteBlock(msg, f.length); +} + + + +void MsgHandler_GPS::process_message(uint8_t *msg) +{ + update_from_msg_gps(0, msg, true); +} + + +void MsgHandler_IMU2::process_message(uint8_t *msg) +{ + update_from_msg_imu(1, msg); +} + + +void 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) +{ + wait_timestamp_from_msg(msg); + + uint8_t this_imu_mask = 1 << imu_offset; + + if (gyro_mask & this_imu_mask) { + Vector3f gyro; + require_field(msg, "Gyr", gyro); + ins.set_gyro(imu_offset, gyro); + } + if (accel_mask & this_imu_mask) { + Vector3f accel2; + require_field(msg, "Acc", accel2); + ins.set_accel(imu_offset, accel2); + } + + dataflash.WriteBlock(msg, f.length); +} + + +void MsgHandler_IMU::process_message(uint8_t *msg) +{ + update_from_msg_imu(0, msg); +} + + +void 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) +{ + wait_timestamp_from_msg(msg); + + Vector3f mag; + require_field(msg, "Mag", mag); + Vector3f mag_offset; + require_field(msg, "Ofs", mag_offset); + + compass.setHIL(compass_offset, mag - mag_offset); + // compass_offset is which compass we are setting info for; + // mag_offset is a vector indicating the compass' calibration... + compass.set_offsets(compass_offset, mag_offset); + + dataflash.WriteBlock(msg, f.length); +} + + + +void MsgHandler_MAG::process_message(uint8_t *msg) +{ + update_from_msg_compass(0, msg); +} + +#include +#include + +void MsgHandler_MSG::process_message(uint8_t *msg) +{ + const uint8_t msg_text_len = 64; + char msg_text[msg_text_len]; + require_field(msg, "Message", msg_text, msg_text_len); + + if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) { + vehicle = VehicleType::VEHICLE_PLANE; + ::printf("Detected Plane\n"); + ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING); + ahrs.set_fly_forward(true); + } else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 || + strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) { + vehicle = VehicleType::VEHICLE_COPTER; + ::printf("Detected Copter\n"); + ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER); + ahrs.set_fly_forward(false); + } else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) { + vehicle = VehicleType::VEHICLE_ROVER; + ::printf("Detected Rover\n"); + ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND); + ahrs.set_fly_forward(true); + } + dataflash.Log_Write_Message(msg_text); +} + + +void MsgHandler_NTUN_Copter::process_message(uint8_t *msg) +{ + inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f, + require_field_float(msg, "PosY") * 0.01f, + 0); +} + + +bool MsgHandler::set_parameter(const char *name, float value) +{ + const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE" }; + for (uint8_t i=0; iset(value); + ::printf("Set %s to %f\n", name, value); + } else if (var_type == AP_PARAM_INT32) { + ((AP_Int32 *)vp)->set(value); + ::printf("Set %s to %d\n", name, (int)value); + } else if (var_type == AP_PARAM_INT16) { + ((AP_Int16 *)vp)->set(value); + ::printf("Set %s to %d\n", name, (int)value); + } else if (var_type == AP_PARAM_INT8) { + ((AP_Int8 *)vp)->set(value); + ::printf("Set %s to %d\n", name, (int)value); + } else { + // we don't support mavlink set on this parameter + return false; + } + return true; +} + +void 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]; + + require_field(msg, "Name", parameter_name, parameter_name_len); + + set_parameter(parameter_name, require_field_float(msg, "Value")); +} + + +void 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 67dc1e772f..71db10cc0b 100644 --- a/Tools/Replay/MsgHandler.h +++ b/Tools/Replay/MsgHandler.h @@ -2,6 +2,7 @@ #define AP_MSGHANDLER_H #include +#include #include @@ -158,4 +159,303 @@ inline void MsgHandler::field_value_for_type_at_offset(uint8_t *msg, } } + +/* subclasses below this point */ + +class MsgHandler_AHR2 : public MsgHandler +{ +public: + MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude) + : MsgHandler(_f, _dataflash,_last_timestamp_usec), + ahr2_attitude(_ahr2_attitude) { }; + + virtual void process_message(uint8_t *msg); + +private: + Vector3f &ahr2_attitude; +}; + + +class MsgHandler_ARM : public MsgHandler +{ +public: + MsgHandler_ARM(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_ARSP : public MsgHandler +{ +public: + MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) : + MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { }; + + virtual void process_message(uint8_t *msg); + +private: + AP_Airspeed &airspeed; +}; + + +class MsgHandler_ATT : public MsgHandler +{ +public: + MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Vector3f &_attitude) + : MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude) + { }; + virtual void process_message(uint8_t *msg); + +private: + Vector3f &attitude; +}; + + +class MsgHandler_BARO : public MsgHandler +{ +public: + MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, AP_Baro &_baro) + : MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { }; + + virtual void process_message(uint8_t *msg); + +private: + AP_Baro &baro; +}; + + +class MsgHandler_Event : public MsgHandler +{ +public: + MsgHandler_Event(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_GPS_Base : public 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), + gps(_gps), ground_alt_cm(_ground_alt_cm), + rel_altitude(_rel_altitude) { }; + +protected: + void update_from_msg_gps(uint8_t imu_offset, uint8_t *data, bool responsible_for_relalt); + +private: + AP_GPS &gps; + uint32_t &ground_alt_cm; + float &rel_altitude; +}; + + +class MsgHandler_GPS : public MsgHandler_GPS_Base +{ +public: + 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, + _gps, _ground_alt_cm, _rel_altitude), + gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { }; + + void process_message(uint8_t *msg); + +private: + AP_GPS &gps; + uint32_t &ground_alt_cm; + float &rel_altitude; +}; + +// it would be nice to use the same parser for both GPS message types +// (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 +{ +public: + 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), + ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { }; + virtual void process_message(uint8_t *msg); +private: + AP_GPS &gps; + uint32_t &ground_alt_cm; + float &rel_altitude; +}; + + + +class MsgHandler_IMU_Base : public MsgHandler +{ +public: + 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), + accel_mask(_accel_mask), + gyro_mask(_gyro_mask), + ins(_ins) { }; + void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg); + +private: + uint8_t &accel_mask; + uint8_t &gyro_mask; + AP_InertialSensor &ins; +}; + +class MsgHandler_IMU : public MsgHandler_IMU_Base +{ +public: + 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, + _accel_mask, _gyro_mask, _ins) { }; + + void process_message(uint8_t *msg); +}; + +class MsgHandler_IMU2 : public MsgHandler_IMU_Base +{ +public: + 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, + _accel_mask, _gyro_mask, _ins) {}; + + virtual void process_message(uint8_t *msg); +}; + +class MsgHandler_IMU3 : public MsgHandler_IMU_Base +{ +public: + 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, + _accel_mask, _gyro_mask, _ins) {}; + + virtual void process_message(uint8_t *msg); +}; + + + +class MsgHandler_MAG_Base : public MsgHandler +{ +public: + MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Compass &_compass) + : MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { }; + +protected: + void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg); + +private: + Compass &compass; +}; + +class MsgHandler_MAG : public MsgHandler_MAG_Base +{ +public: + MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Compass &_compass) + : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; + + virtual void process_message(uint8_t *msg); +}; + +class MsgHandler_MAG2 : public MsgHandler_MAG_Base +{ +public: + MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Compass &_compass) + : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; + + virtual void process_message(uint8_t *msg); +}; + + + +class MsgHandler_MSG : public MsgHandler +{ +public: + 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), + vehicle(_vehicle), ahrs(_ahrs) { } + + + virtual void process_message(uint8_t *msg); + +private: + VehicleType::vehicle_type &vehicle; + AP_AHRS &ahrs; +}; + + +class MsgHandler_NTUN_Copter : public MsgHandler +{ +public: + MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Vector3f &_inavpos) + : MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {}; + + virtual void process_message(uint8_t *msg); + +private: + Vector3f &inavpos; +}; + + +class MsgHandler_PARM : public MsgHandler +{ +public: + MsgHandler_PARM(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_SIM : public MsgHandler +{ +public: + MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, + Vector3f &_sim_attitude) + : MsgHandler(_f, _dataflash, _last_timestamp_usec), + sim_attitude(_sim_attitude) + { }; + + virtual void process_message(uint8_t *msg); + +private: + Vector3f &sim_attitude; +}; + #endif diff --git a/Tools/Replay/MsgHandler_AHR2.cpp b/Tools/Replay/MsgHandler_AHR2.cpp deleted file mode 100644 index fff4820d44..0000000000 --- a/Tools/Replay/MsgHandler_AHR2.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "MsgHandler_AHR2.h" - -void MsgHandler_AHR2::process_message(uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw"); -} diff --git a/Tools/Replay/MsgHandler_AHR2.h b/Tools/Replay/MsgHandler_AHR2.h deleted file mode 100644 index f13c626de2..0000000000 --- a/Tools/Replay/MsgHandler_AHR2.h +++ /dev/null @@ -1,15 +0,0 @@ -#include "MsgHandler.h" - -class MsgHandler_AHR2 : public MsgHandler -{ -public: - MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude) - : MsgHandler(_f, _dataflash,_last_timestamp_usec), - ahr2_attitude(_ahr2_attitude) { }; - - virtual void process_message(uint8_t *msg); - -private: - Vector3f &ahr2_attitude; -}; diff --git a/Tools/Replay/MsgHandler_ARM.cpp b/Tools/Replay/MsgHandler_ARM.cpp deleted file mode 100644 index 5522655ab2..0000000000 --- a/Tools/Replay/MsgHandler_ARM.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "MsgHandler_ARM.h" - -extern const AP_HAL::HAL& hal; - -void MsgHandler_ARM::process_message(uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - uint8_t ArmState = require_field_uint8_t(msg, "ArmState"); - hal.util->set_soft_armed(ArmState); - printf("Armed state: %u at %lu\n", - (unsigned)ArmState, - (unsigned long)hal.scheduler->millis()); - dataflash.WriteBlock(msg, f.length); -} diff --git a/Tools/Replay/MsgHandler_ARM.h b/Tools/Replay/MsgHandler_ARM.h deleted file mode 100644 index f3b429b694..0000000000 --- a/Tools/Replay/MsgHandler_ARM.h +++ /dev/null @@ -1,11 +0,0 @@ -#include "MsgHandler.h" - -class MsgHandler_ARM : public MsgHandler -{ -public: - MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec) - : MsgHandler(_f, _dataflash, _last_timestamp_usec) { }; - - virtual void process_message(uint8_t *msg); -}; diff --git a/Tools/Replay/MsgHandler_ARSP.cpp b/Tools/Replay/MsgHandler_ARSP.cpp deleted file mode 100644 index 800ccddd12..0000000000 --- a/Tools/Replay/MsgHandler_ARSP.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "MsgHandler_ARSP.h" - -void MsgHandler_ARSP::process_message(uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - - airspeed.setHIL(require_field_float(msg, "Airspeed"), - require_field_float(msg, "DiffPress"), - require_field_float(msg, "Temp")); - dataflash.WriteBlock(msg, f.length); -} diff --git a/Tools/Replay/MsgHandler_ARSP.h b/Tools/Replay/MsgHandler_ARSP.h deleted file mode 100644 index 2107d4706e..0000000000 --- a/Tools/Replay/MsgHandler_ARSP.h +++ /dev/null @@ -1,14 +0,0 @@ -#include "MsgHandler.h" - -class MsgHandler_ARSP : public MsgHandler -{ -public: - MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) : - MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { }; - - virtual void process_message(uint8_t *msg); - -private: - AP_Airspeed &airspeed; -}; diff --git a/Tools/Replay/MsgHandler_ATT.cpp b/Tools/Replay/MsgHandler_ATT.cpp deleted file mode 100644 index b2cff70ee4..0000000000 --- a/Tools/Replay/MsgHandler_ATT.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "MsgHandler_ATT.h" - -void MsgHandler_ATT::process_message(uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw"); -} diff --git a/Tools/Replay/MsgHandler_ATT.h b/Tools/Replay/MsgHandler_ATT.h deleted file mode 100644 index f383347bd0..0000000000 --- a/Tools/Replay/MsgHandler_ATT.h +++ /dev/null @@ -1,14 +0,0 @@ -#include "MsgHandler.h" - -class MsgHandler_ATT : public MsgHandler -{ -public: - MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Vector3f &_attitude) - : MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude) - { }; - virtual void process_message(uint8_t *msg); - -private: - Vector3f &attitude; -}; diff --git a/Tools/Replay/MsgHandler_BARO.cpp b/Tools/Replay/MsgHandler_BARO.cpp deleted file mode 100644 index 73b8a0f4a5..0000000000 --- a/Tools/Replay/MsgHandler_BARO.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "MsgHandler_BARO.h" - -void MsgHandler_BARO::process_message(uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - baro.setHIL(0, - require_field_float(msg, "Press"), - require_field_int16_t(msg, "Temp") * 0.01f); - dataflash.WriteBlock(msg, f.length); -} diff --git a/Tools/Replay/MsgHandler_BARO.h b/Tools/Replay/MsgHandler_BARO.h deleted file mode 100644 index f4bf6010de..0000000000 --- a/Tools/Replay/MsgHandler_BARO.h +++ /dev/null @@ -1,14 +0,0 @@ -#include "MsgHandler.h" - -class MsgHandler_BARO : public MsgHandler -{ -public: - MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, AP_Baro &_baro) - : MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { }; - - virtual void process_message(uint8_t *msg); - -private: - AP_Baro &baro; -}; diff --git a/Tools/Replay/MsgHandler_Event.cpp b/Tools/Replay/MsgHandler_Event.cpp deleted file mode 100644 index d3bf9f67ba..0000000000 --- a/Tools/Replay/MsgHandler_Event.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "MsgHandler_Event.h" - -extern const AP_HAL::HAL& hal; - -#define DATA_ARMED 10 -#define DATA_DISARMED 11 - -void MsgHandler_Event::process_message(uint8_t *msg) -{ - uint8_t id = require_field_uint8_t(msg, "Id"); - if (id == DATA_ARMED) { - hal.util->set_soft_armed(true); - printf("Armed at %lu\n", - (unsigned long)hal.scheduler->millis()); - } else if (id == DATA_DISARMED) { - hal.util->set_soft_armed(false); - printf("Disarmed at %lu\n", - (unsigned long)hal.scheduler->millis()); - } - dataflash.WriteBlock(msg, f.length); -} diff --git a/Tools/Replay/MsgHandler_Event.h b/Tools/Replay/MsgHandler_Event.h deleted file mode 100644 index 9e15527f4a..0000000000 --- a/Tools/Replay/MsgHandler_Event.h +++ /dev/null @@ -1,11 +0,0 @@ -#include "MsgHandler.h" - -class MsgHandler_Event : public MsgHandler -{ -public: - MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec) - : MsgHandler(_f, _dataflash, _last_timestamp_usec) { }; - - virtual void process_message(uint8_t *msg); -}; diff --git a/Tools/Replay/MsgHandler_GPS.cpp b/Tools/Replay/MsgHandler_GPS.cpp deleted file mode 100644 index d8fca439ed..0000000000 --- a/Tools/Replay/MsgHandler_GPS.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "MsgHandler_GPS.h" - -void MsgHandler_GPS::process_message(uint8_t *msg) -{ - update_from_msg_gps(0, msg, true); -} diff --git a/Tools/Replay/MsgHandler_GPS.h b/Tools/Replay/MsgHandler_GPS.h deleted file mode 100644 index 0a258bf289..0000000000 --- a/Tools/Replay/MsgHandler_GPS.h +++ /dev/null @@ -1,19 +0,0 @@ -#include "MsgHandler_GPS_Base.h" - -class MsgHandler_GPS : public MsgHandler_GPS_Base -{ -public: - 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, - _gps, _ground_alt_cm, _rel_altitude), - gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { }; - - void process_message(uint8_t *msg); - -private: - AP_GPS &gps; - uint32_t &ground_alt_cm; - float &rel_altitude; -}; diff --git a/Tools/Replay/MsgHandler_GPS2.cpp b/Tools/Replay/MsgHandler_GPS2.cpp deleted file mode 100644 index f986c352cb..0000000000 --- a/Tools/Replay/MsgHandler_GPS2.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include - -void 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 - // the value we use (probably) comes from the most recent - // LOG_GPS_MESSAGE message! - update_from_msg_gps(1, msg, false); -} diff --git a/Tools/Replay/MsgHandler_GPS2.h b/Tools/Replay/MsgHandler_GPS2.h deleted file mode 100644 index 12bbd05a52..0000000000 --- a/Tools/Replay/MsgHandler_GPS2.h +++ /dev/null @@ -1,22 +0,0 @@ -#include "MsgHandler_GPS_Base.h" - -// it would be nice to use the same parser for both GPS message types -// (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 -{ -public: - 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), - ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { }; - virtual void process_message(uint8_t *msg); -private: - AP_GPS &gps; - uint32_t &ground_alt_cm; - float &rel_altitude; -}; diff --git a/Tools/Replay/MsgHandler_GPS_Base.cpp b/Tools/Replay/MsgHandler_GPS_Base.cpp deleted file mode 100644 index 6a70ec3cfa..0000000000 --- a/Tools/Replay/MsgHandler_GPS_Base.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "MsgHandler_GPS_Base.h" - -void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt) -{ - uint32_t timestamp; - require_field(msg, "T", timestamp); - wait_timestamp(timestamp); - - Location loc; - location_from_msg(msg, loc, "Lat", "Lng", "Alt"); - Vector3f vel; - ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ"); - - uint8_t status = require_field_uint8_t(msg, "Status"); - gps.setHIL(gps_offset, - (AP_GPS::GPS_Status)status, - timestamp, - loc, - vel, - require_field_uint8_t(msg, "NSats"), - require_field_uint8_t(msg, "HDop"), - require_field_float(msg, "VZ") != 0); - if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) { - ground_alt_cm = require_field_int32_t(msg, "Alt"); - } - - if (responsible_for_relalt) { - // this could possibly check for the presence of "RelAlt" label? - rel_altitude = 0.01f * require_field_int32_t(msg, "RelAlt"); - } - - dataflash.WriteBlock(msg, f.length); -} - diff --git a/Tools/Replay/MsgHandler_GPS_Base.h b/Tools/Replay/MsgHandler_GPS_Base.h deleted file mode 100644 index 03eb42205c..0000000000 --- a/Tools/Replay/MsgHandler_GPS_Base.h +++ /dev/null @@ -1,27 +0,0 @@ -#include - -#ifndef AP_MSGHANDLER_GPS_BASE_H -#define AP_MSGHANDLER_GPS_BASE_H - -class MsgHandler_GPS_Base : public 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), - gps(_gps), ground_alt_cm(_ground_alt_cm), - rel_altitude(_rel_altitude) { }; - -protected: - void update_from_msg_gps(uint8_t imu_offset, uint8_t *data, bool responsible_for_relalt); - -private: - AP_GPS &gps; - uint32_t &ground_alt_cm; - float &rel_altitude; -}; - -#endif - diff --git a/Tools/Replay/MsgHandler_IMU.cpp b/Tools/Replay/MsgHandler_IMU.cpp deleted file mode 100644 index 35973f4c4a..0000000000 --- a/Tools/Replay/MsgHandler_IMU.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "MsgHandler_IMU.h" - -void MsgHandler_IMU::process_message(uint8_t *msg) -{ - update_from_msg_imu(0, msg); -} diff --git a/Tools/Replay/MsgHandler_IMU.h b/Tools/Replay/MsgHandler_IMU.h deleted file mode 100644 index 6ae502ac69..0000000000 --- a/Tools/Replay/MsgHandler_IMU.h +++ /dev/null @@ -1,14 +0,0 @@ -#include "MsgHandler_IMU_Base.h" - -class MsgHandler_IMU : public MsgHandler_IMU_Base -{ -public: - 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, - _accel_mask, _gyro_mask, _ins) { }; - - void process_message(uint8_t *msg); -}; diff --git a/Tools/Replay/MsgHandler_IMU2.cpp b/Tools/Replay/MsgHandler_IMU2.cpp deleted file mode 100644 index 6594ab7744..0000000000 --- a/Tools/Replay/MsgHandler_IMU2.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "MsgHandler_IMU2.h" - -void MsgHandler_IMU2::process_message(uint8_t *msg) -{ - update_from_msg_imu(1, msg); -} diff --git a/Tools/Replay/MsgHandler_IMU2.h b/Tools/Replay/MsgHandler_IMU2.h deleted file mode 100644 index 3501568ca7..0000000000 --- a/Tools/Replay/MsgHandler_IMU2.h +++ /dev/null @@ -1,14 +0,0 @@ -#include "MsgHandler_IMU_Base.h" - -class MsgHandler_IMU2 : public MsgHandler_IMU_Base -{ -public: - 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, - _accel_mask, _gyro_mask, _ins) {}; - - virtual void process_message(uint8_t *msg); -}; diff --git a/Tools/Replay/MsgHandler_IMU3.cpp b/Tools/Replay/MsgHandler_IMU3.cpp deleted file mode 100644 index 2feb3fa809..0000000000 --- a/Tools/Replay/MsgHandler_IMU3.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "MsgHandler_IMU3.h" - -void MsgHandler_IMU3::process_message(uint8_t *msg) -{ - update_from_msg_imu(2, msg); -} diff --git a/Tools/Replay/MsgHandler_IMU3.h b/Tools/Replay/MsgHandler_IMU3.h deleted file mode 100644 index ba0d84c357..0000000000 --- a/Tools/Replay/MsgHandler_IMU3.h +++ /dev/null @@ -1,14 +0,0 @@ -#include "MsgHandler_IMU_Base.h" - -class MsgHandler_IMU3 : public MsgHandler_IMU_Base -{ -public: - 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, - _accel_mask, _gyro_mask, _ins) {}; - - virtual void process_message(uint8_t *msg); -}; diff --git a/Tools/Replay/MsgHandler_IMU_Base.cpp b/Tools/Replay/MsgHandler_IMU_Base.cpp deleted file mode 100644 index e3ef65698b..0000000000 --- a/Tools/Replay/MsgHandler_IMU_Base.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "MsgHandler_IMU_Base.h" - -void MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - - uint8_t this_imu_mask = 1 << imu_offset; - - if (gyro_mask & this_imu_mask) { - Vector3f gyro; - require_field(msg, "Gyr", gyro); - ins.set_gyro(imu_offset, gyro); - } - if (accel_mask & this_imu_mask) { - Vector3f accel2; - require_field(msg, "Acc", accel2); - ins.set_accel(imu_offset, accel2); - } - - dataflash.WriteBlock(msg, f.length); -} diff --git a/Tools/Replay/MsgHandler_IMU_Base.h b/Tools/Replay/MsgHandler_IMU_Base.h deleted file mode 100644 index 24dd9b3718..0000000000 --- a/Tools/Replay/MsgHandler_IMU_Base.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef AP_MSGHANDLER_IMU_BASE_H -#define AP_MSGHANDLER_IMU_BASE_H - -#include - -class MsgHandler_IMU_Base : public MsgHandler -{ -public: - 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), - accel_mask(_accel_mask), - gyro_mask(_gyro_mask), - ins(_ins) { }; - void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg); - -private: - uint8_t &accel_mask; - uint8_t &gyro_mask; - AP_InertialSensor &ins; -}; - -#endif - diff --git a/Tools/Replay/MsgHandler_MAG.cpp b/Tools/Replay/MsgHandler_MAG.cpp deleted file mode 100644 index 6cac0d0c73..0000000000 --- a/Tools/Replay/MsgHandler_MAG.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "MsgHandler_MAG.h" - -void MsgHandler_MAG::process_message(uint8_t *msg) -{ - update_from_msg_compass(0, msg); -} diff --git a/Tools/Replay/MsgHandler_MAG.h b/Tools/Replay/MsgHandler_MAG.h deleted file mode 100644 index 4e98ae7835..0000000000 --- a/Tools/Replay/MsgHandler_MAG.h +++ /dev/null @@ -1,11 +0,0 @@ -#include "MsgHandler_MAG_Base.h" - -class MsgHandler_MAG : public MsgHandler_MAG_Base -{ -public: - MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Compass &_compass) - : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; - - virtual void process_message(uint8_t *msg); -}; diff --git a/Tools/Replay/MsgHandler_MAG2.cpp b/Tools/Replay/MsgHandler_MAG2.cpp deleted file mode 100644 index e4455c27ed..0000000000 --- a/Tools/Replay/MsgHandler_MAG2.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "MsgHandler_MAG2.h" - -void MsgHandler_MAG2::process_message(uint8_t *msg) -{ - update_from_msg_compass(1, msg); -} diff --git a/Tools/Replay/MsgHandler_MAG2.h b/Tools/Replay/MsgHandler_MAG2.h deleted file mode 100644 index 94e6cb63fd..0000000000 --- a/Tools/Replay/MsgHandler_MAG2.h +++ /dev/null @@ -1,11 +0,0 @@ -#include "MsgHandler_MAG_Base.h" - -class MsgHandler_MAG2 : public MsgHandler_MAG_Base -{ -public: - MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Compass &_compass) - : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; - - virtual void process_message(uint8_t *msg); -}; diff --git a/Tools/Replay/MsgHandler_MAG_Base.cpp b/Tools/Replay/MsgHandler_MAG_Base.cpp deleted file mode 100644 index 5177a41529..0000000000 --- a/Tools/Replay/MsgHandler_MAG_Base.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "MsgHandler_MAG_Base.h" - -void MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - - Vector3f mag; - require_field(msg, "Mag", mag); - Vector3f mag_offset; - require_field(msg, "Ofs", mag_offset); - - compass.setHIL(compass_offset, mag - mag_offset); - // compass_offset is which compass we are setting info for; - // mag_offset is a vector indicating the compass' calibration... - compass.set_offsets(compass_offset, mag_offset); - - dataflash.WriteBlock(msg, f.length); -} - diff --git a/Tools/Replay/MsgHandler_MAG_Base.h b/Tools/Replay/MsgHandler_MAG_Base.h deleted file mode 100644 index 41e30a1834..0000000000 --- a/Tools/Replay/MsgHandler_MAG_Base.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef MSGHANDLER_MAG_BASE_H -#define MSGHANDLER_MAG_BASE_H - -#include "MsgHandler.h" - -class MsgHandler_MAG_Base : public MsgHandler -{ -public: - MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Compass &_compass) - : MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { }; - -protected: - void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg); - -private: - Compass &compass; -}; - -#endif // MSGHANDLER_MAG_BASE diff --git a/Tools/Replay/MsgHandler_MSG.cpp b/Tools/Replay/MsgHandler_MSG.cpp deleted file mode 100644 index 9e78f35684..0000000000 --- a/Tools/Replay/MsgHandler_MSG.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "MsgHandler_MSG.h" -#include -#include - -void MsgHandler_MSG::process_message(uint8_t *msg) -{ - const uint8_t msg_text_len = 64; - char msg_text[msg_text_len]; - require_field(msg, "Message", msg_text, msg_text_len); - - if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) { - vehicle = VehicleType::VEHICLE_PLANE; - ::printf("Detected Plane\n"); - ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING); - ahrs.set_fly_forward(true); - } else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 || - strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) { - vehicle = VehicleType::VEHICLE_COPTER; - ::printf("Detected Copter\n"); - ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER); - ahrs.set_fly_forward(false); - } else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) { - vehicle = VehicleType::VEHICLE_ROVER; - ::printf("Detected Rover\n"); - ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND); - ahrs.set_fly_forward(true); - } - dataflash.Log_Write_Message(msg_text); -} diff --git a/Tools/Replay/MsgHandler_MSG.h b/Tools/Replay/MsgHandler_MSG.h deleted file mode 100644 index 410f2571d7..0000000000 --- a/Tools/Replay/MsgHandler_MSG.h +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include - -class MsgHandler_MSG : public MsgHandler -{ -public: - 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), - vehicle(_vehicle), ahrs(_ahrs) { } - - - virtual void process_message(uint8_t *msg); - -private: - VehicleType::vehicle_type &vehicle; - AP_AHRS &ahrs; -}; diff --git a/Tools/Replay/MsgHandler_NTUN_Copter.cpp b/Tools/Replay/MsgHandler_NTUN_Copter.cpp deleted file mode 100644 index fdc312e388..0000000000 --- a/Tools/Replay/MsgHandler_NTUN_Copter.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "MsgHandler_NTUN_Copter.h" - -void MsgHandler_NTUN_Copter::process_message(uint8_t *msg) -{ - inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f, - require_field_float(msg, "PosY") * 0.01f, - 0); -} diff --git a/Tools/Replay/MsgHandler_NTUN_Copter.h b/Tools/Replay/MsgHandler_NTUN_Copter.h deleted file mode 100644 index 70e8d0f6d6..0000000000 --- a/Tools/Replay/MsgHandler_NTUN_Copter.h +++ /dev/null @@ -1,14 +0,0 @@ -#include "MsgHandler.h" - -class MsgHandler_NTUN_Copter : public MsgHandler -{ -public: - MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Vector3f &_inavpos) - : MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {}; - - virtual void process_message(uint8_t *msg); - -private: - Vector3f &inavpos; -}; diff --git a/Tools/Replay/MsgHandler_PARM.cpp b/Tools/Replay/MsgHandler_PARM.cpp deleted file mode 100644 index 4bbcc2eb6d..0000000000 --- a/Tools/Replay/MsgHandler_PARM.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "MsgHandler_PARM.h" - -bool MsgHandler::set_parameter(const char *name, float value) -{ - const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE" }; - for (uint8_t i=0; iset(value); - ::printf("Set %s to %f\n", name, value); - } else if (var_type == AP_PARAM_INT32) { - ((AP_Int32 *)vp)->set(value); - ::printf("Set %s to %d\n", name, (int)value); - } else if (var_type == AP_PARAM_INT16) { - ((AP_Int16 *)vp)->set(value); - ::printf("Set %s to %d\n", name, (int)value); - } else if (var_type == AP_PARAM_INT8) { - ((AP_Int8 *)vp)->set(value); - ::printf("Set %s to %d\n", name, (int)value); - } else { - // we don't support mavlink set on this parameter - return false; - } - return true; -} - -void 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]; - - require_field(msg, "Name", parameter_name, parameter_name_len); - - set_parameter(parameter_name, require_field_float(msg, "Value")); -} diff --git a/Tools/Replay/MsgHandler_PARM.h b/Tools/Replay/MsgHandler_PARM.h deleted file mode 100644 index 677ddf54de..0000000000 --- a/Tools/Replay/MsgHandler_PARM.h +++ /dev/null @@ -1,9 +0,0 @@ -#include - -class MsgHandler_PARM : public MsgHandler -{ -public: - MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t _last_timestamp_usec) : MsgHandler(_f, _dataflash, _last_timestamp_usec) {}; - - virtual void process_message(uint8_t *msg); -}; diff --git a/Tools/Replay/MsgHandler_SIM.cpp b/Tools/Replay/MsgHandler_SIM.cpp deleted file mode 100644 index 4c757e2073..0000000000 --- a/Tools/Replay/MsgHandler_SIM.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "MsgHandler_SIM.h" - -void 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_SIM.h b/Tools/Replay/MsgHandler_SIM.h deleted file mode 100644 index f626f11f5b..0000000000 --- a/Tools/Replay/MsgHandler_SIM.h +++ /dev/null @@ -1,17 +0,0 @@ -#include "MsgHandler.h" - -class MsgHandler_SIM : public MsgHandler -{ -public: - MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, - Vector3f &_sim_attitude) - : MsgHandler(_f, _dataflash, _last_timestamp_usec), - sim_attitude(_sim_attitude) - { }; - - virtual void process_message(uint8_t *msg); - -private: - Vector3f &sim_attitude; -};