diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp new file mode 100644 index 0000000000..e1c271752d --- /dev/null +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -0,0 +1,334 @@ +#include + +extern const AP_HAL::HAL& hal; + +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 LR_MsgHandler::wait_timestamp_usec(uint64_t timestamp) +{ + last_timestamp_usec = timestamp; + hal.scheduler->stop_clock(timestamp); +} + +void LR_MsgHandler::wait_timestamp(uint32_t timestamp) +{ + uint64_t usecs = timestamp*1000UL; + wait_timestamp_usec(usecs); +} + +void LR_MsgHandler::wait_timestamp_from_msg(uint8_t *msg) +{ + uint64_t time_us; + uint64_t time_ms; + + if (field_value(msg, "TimeUS", time_us)) { + // 64-bit timestamp present - great! + wait_timestamp_usec(time_us); + } else if (field_value(msg, "TimeMS", time_ms)) { + // there is special rounding code that needs to be crossed in + // wait_timestamp: + wait_timestamp(time_ms); + } else { + ::printf("No timestamp on message"); + } +} + + + +/* + * subclasses to handle specific messages below here +*/ + +void LR_MsgHandler_AHR2::process_message(uint8_t *msg) +{ + wait_timestamp_from_msg(msg); + attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw"); +} + + +void LR_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()); +} + + +void LR_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")); +} + +void LR_MsgHandler_FRAM::process_message(uint8_t *msg) +{ + wait_timestamp_from_msg(msg); +} + + +void LR_MsgHandler_ATT::process_message(uint8_t *msg) +{ + wait_timestamp_from_msg(msg); + attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw"); +} + + +void LR_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); +} + + +#define DATA_ARMED 10 +#define DATA_DISARMED 11 + +void LR_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()); + } +} + + +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 + // the value we use (probably) comes from the most recent + // LOG_GPS_MESSAGE message! + update_from_msg_gps(1, msg, false); +} + + +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)) { + uint32_t timestamp; + require_field(msg, "T", timestamp); + time_us = timestamp * 1000; + } + wait_timestamp_usec(time_us); + + 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, + uint32_t(time_us/1000), + 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? + int32_t tmp; + if (! field_value(msg, "RAlt", tmp)) { + tmp = require_field_int32_t(msg, "RelAlt"); + } + rel_altitude = 0.01f * tmp; + } +} + + + +void LR_MsgHandler_GPS::process_message(uint8_t *msg) +{ + update_from_msg_gps(0, msg, true); +} + + +void LR_MsgHandler_IMU2::process_message(uint8_t *msg) +{ + update_from_msg_imu(1, msg); +} + + +void LR_MsgHandler_IMU3::process_message(uint8_t *msg) +{ + update_from_msg_imu(2, msg); +} + + +void LR_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); + } +} + + +void LR_MsgHandler_IMU::process_message(uint8_t *msg) +{ + update_from_msg_imu(0, msg); +} + + +void LR_MsgHandler_MAG2::process_message(uint8_t *msg) +{ + update_from_msg_compass(1, msg); +} + + +void LR_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); +} + + + +void LR_MsgHandler_MAG::process_message(uint8_t *msg) +{ + update_from_msg_compass(0, msg); +} + +#include +#include + +void LR_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); + } +} + + +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, + 0); +} + + +bool LR_MsgHandler::set_parameter(const char *name, float value) +{ + const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE", + "COMPASS_ORIENT", "COMPASS_ORIENT2", + "COMPASS_ORIENT3"}; + 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 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]; + uint64_t time_us; + + if (field_value(msg, "TimeUS", time_us)) { + wait_timestamp_usec(time_us); + } else { + // older logs can have a lot of FMT and PARM messages up the + // front which don't have timestamps. Since in Replay we run + // DataFlash's IO only when stop_clock is called, we can + // overflow DataFlash's ringbuffer. This should force us to + // do IO: + hal.scheduler->stop_clock(last_timestamp_usec); + } + + require_field(msg, "Name", parameter_name, parameter_name_len); + + set_parameter(parameter_name, require_field_float(msg, "Value")); +} + + +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/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h new file mode 100644 index 0000000000..8e3aeeb945 --- /dev/null +++ b/Tools/Replay/LR_MsgHandler.h @@ -0,0 +1,333 @@ +#ifndef AP_LR_MSGHANDLER_H +#define AP_LR_MSGHANDLER_H + +#include + +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; + +}; + +/* subclasses below this point */ + +class LR_MsgHandler_AHR2 : public LR_MsgHandler +{ +public: + LR_MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude) + : LR_MsgHandler(_f, _dataflash,_last_timestamp_usec), + ahr2_attitude(_ahr2_attitude) { }; + + virtual void process_message(uint8_t *msg); + +private: + Vector3f &ahr2_attitude; +}; + + +class LR_MsgHandler_ARM : public LR_MsgHandler +{ +public: + LR_MsgHandler_ARM(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 LR_MsgHandler_ARSP : public LR_MsgHandler +{ +public: + LR_MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) : + LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { }; + + virtual void process_message(uint8_t *msg); + +private: + AP_Airspeed &airspeed; +}; + +class LR_MsgHandler_FRAM : public LR_MsgHandler +{ +public: + LR_MsgHandler_FRAM(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 LR_MsgHandler_ATT : public LR_MsgHandler +{ +public: + LR_MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Vector3f &_attitude) + : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude) + { }; + virtual void process_message(uint8_t *msg); + +private: + Vector3f &attitude; +}; + + +class LR_MsgHandler_BARO : public LR_MsgHandler +{ +public: + LR_MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, AP_Baro &_baro) + : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { }; + + virtual void process_message(uint8_t *msg); + +private: + AP_Baro &baro; +}; + + +class LR_MsgHandler_Event : public LR_MsgHandler +{ +public: + LR_MsgHandler_Event(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 LR_MsgHandler_GPS_Base : public LR_MsgHandler +{ + +public: + 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) { }; + +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 LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base +{ +public: + 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) + : 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) { }; + + 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 LR_MsgHandler_GPS2 : public LR_MsgHandler_GPS_Base +{ +public: + 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) + : 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: + AP_GPS &gps; + uint32_t &ground_alt_cm; + float &rel_altitude; +}; + + + +class LR_MsgHandler_IMU_Base : public LR_MsgHandler +{ +public: + 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) : + LR_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 LR_MsgHandler_IMU : public LR_MsgHandler_IMU_Base +{ +public: + 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) + : LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec, + _accel_mask, _gyro_mask, _ins) { }; + + void process_message(uint8_t *msg); +}; + +class LR_MsgHandler_IMU2 : public LR_MsgHandler_IMU_Base +{ +public: + 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) + : LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec, + _accel_mask, _gyro_mask, _ins) {}; + + virtual void process_message(uint8_t *msg); +}; + +class LR_MsgHandler_IMU3 : public LR_MsgHandler_IMU_Base +{ +public: + 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) + : LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec, + _accel_mask, _gyro_mask, _ins) {}; + + virtual void process_message(uint8_t *msg); +}; + + + +class LR_MsgHandler_MAG_Base : public LR_MsgHandler +{ +public: + LR_MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_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); + +private: + Compass &compass; +}; + +class LR_MsgHandler_MAG : public LR_MsgHandler_MAG_Base +{ +public: + LR_MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Compass &_compass) + : LR_MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; + + virtual void process_message(uint8_t *msg); +}; + +class LR_MsgHandler_MAG2 : public LR_MsgHandler_MAG_Base +{ +public: + LR_MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Compass &_compass) + : LR_MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; + + virtual void process_message(uint8_t *msg); +}; + + + +class LR_MsgHandler_MSG : public LR_MsgHandler +{ +public: + LR_MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, + VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) : + LR_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 LR_MsgHandler_NTUN_Copter : public LR_MsgHandler +{ +public: + LR_MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Vector3f &_inavpos) + : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {}; + + virtual void process_message(uint8_t *msg); + +private: + Vector3f &inavpos; +}; + + +class LR_MsgHandler_PARM : public LR_MsgHandler +{ +public: + 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 LR_MsgHandler_SIM : public LR_MsgHandler +{ +public: + LR_MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, + Vector3f &_sim_attitude) + : LR_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/LogReader.h b/Tools/Replay/LogReader.h index 64db2525dc..0285268c38 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -1,5 +1,6 @@ #include #include +#include class LogReader : public DataFlashFileReader { diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index e3c8bd33f6..3942896cdd 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -1,7 +1,5 @@ #include -extern const AP_HAL::HAL& hal; - void fatal(const char *msg) { ::printf("%s",msg); ::printf("\n"); @@ -66,13 +64,6 @@ MsgHandler::MsgHandler(const struct log_Format &_f) : next_field(0), f(_f) 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) { @@ -199,18 +190,6 @@ MsgHandler::~MsgHandler() } } -void LR_MsgHandler::wait_timestamp_usec(uint64_t timestamp) -{ - last_timestamp_usec = timestamp; - hal.scheduler->stop_clock(timestamp); -} - -void LR_MsgHandler::wait_timestamp(uint32_t timestamp) -{ - uint64_t usecs = timestamp*1000UL; - wait_timestamp_usec(usecs); -} - void MsgHandler::location_from_msg(uint8_t *msg, Location &loc, const char *label_lat, @@ -296,316 +275,3 @@ int16_t MsgHandler::require_field_int16_t(uint8_t *msg, const char *label) require_field(msg, label, ret); return ret; } - -void LR_MsgHandler::wait_timestamp_from_msg(uint8_t *msg) -{ - uint64_t time_us; - uint64_t time_ms; - - if (field_value(msg, "TimeUS", time_us)) { - // 64-bit timestamp present - great! - wait_timestamp_usec(time_us); - } else if (field_value(msg, "TimeMS", time_ms)) { - // there is special rounding code that needs to be crossed in - // wait_timestamp: - wait_timestamp(time_ms); - } else { - ::printf("No timestamp on message"); - } -} - - - -/* - * subclasses to handle specific messages below here -*/ - -void LR_MsgHandler_AHR2::process_message(uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw"); -} - - -void LR_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()); -} - - -void LR_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")); -} - -void LR_MsgHandler_FRAM::process_message(uint8_t *msg) -{ - wait_timestamp_from_msg(msg); -} - - -void LR_MsgHandler_ATT::process_message(uint8_t *msg) -{ - wait_timestamp_from_msg(msg); - attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw"); -} - - -void LR_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); -} - - -#define DATA_ARMED 10 -#define DATA_DISARMED 11 - -void LR_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()); - } -} - - -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 - // the value we use (probably) comes from the most recent - // LOG_GPS_MESSAGE message! - update_from_msg_gps(1, msg, false); -} - - -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)) { - uint32_t timestamp; - require_field(msg, "T", timestamp); - time_us = timestamp * 1000; - } - wait_timestamp_usec(time_us); - - 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, - uint32_t(time_us/1000), - 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? - int32_t tmp; - if (! field_value(msg, "RAlt", tmp)) { - tmp = require_field_int32_t(msg, "RelAlt"); - } - rel_altitude = 0.01f * tmp; - } -} - - - -void LR_MsgHandler_GPS::process_message(uint8_t *msg) -{ - update_from_msg_gps(0, msg, true); -} - - -void LR_MsgHandler_IMU2::process_message(uint8_t *msg) -{ - update_from_msg_imu(1, msg); -} - - -void LR_MsgHandler_IMU3::process_message(uint8_t *msg) -{ - update_from_msg_imu(2, msg); -} - - -void LR_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); - } -} - - -void LR_MsgHandler_IMU::process_message(uint8_t *msg) -{ - update_from_msg_imu(0, msg); -} - - -void LR_MsgHandler_MAG2::process_message(uint8_t *msg) -{ - update_from_msg_compass(1, msg); -} - - -void LR_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); -} - - - -void LR_MsgHandler_MAG::process_message(uint8_t *msg) -{ - update_from_msg_compass(0, msg); -} - -#include -#include - -void LR_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); - } -} - - -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, - 0); -} - - -bool LR_MsgHandler::set_parameter(const char *name, float value) -{ - const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE", - "COMPASS_ORIENT", "COMPASS_ORIENT2", - "COMPASS_ORIENT3"}; - 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 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]; - uint64_t time_us; - - if (field_value(msg, "TimeUS", time_us)) { - wait_timestamp_usec(time_us); - } else { - // older logs can have a lot of FMT and PARM messages up the - // front which don't have timestamps. Since in Replay we run - // DataFlash's IO only when stop_clock is called, we can - // overflow DataFlash's ringbuffer. This should force us to - // do IO: - hal.scheduler->stop_clock(last_timestamp_usec); - } - - require_field(msg, "Name", parameter_name, parameter_name_len); - - set_parameter(parameter_name, require_field_float(msg, "Value")); -} - - -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 68ab030495..bec367cae5 100644 --- a/Tools/Replay/MsgHandler.h +++ b/Tools/Replay/MsgHandler.h @@ -89,25 +89,6 @@ 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) { @@ -172,313 +153,4 @@ inline void MsgHandler::field_value_for_type_at_offset(uint8_t *msg, } } - -/* subclasses below this point */ - -class LR_MsgHandler_AHR2 : public LR_MsgHandler -{ -public: - LR_MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude) - : LR_MsgHandler(_f, _dataflash,_last_timestamp_usec), - ahr2_attitude(_ahr2_attitude) { }; - - virtual void process_message(uint8_t *msg); - -private: - Vector3f &ahr2_attitude; -}; - - -class LR_MsgHandler_ARM : public LR_MsgHandler -{ -public: - LR_MsgHandler_ARM(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 LR_MsgHandler_ARSP : public LR_MsgHandler -{ -public: - LR_MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) : - LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { }; - - virtual void process_message(uint8_t *msg); - -private: - AP_Airspeed &airspeed; -}; - -class LR_MsgHandler_FRAM : public LR_MsgHandler -{ -public: - LR_MsgHandler_FRAM(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 LR_MsgHandler_ATT : public LR_MsgHandler -{ -public: - LR_MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Vector3f &_attitude) - : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude) - { }; - virtual void process_message(uint8_t *msg); - -private: - Vector3f &attitude; -}; - - -class LR_MsgHandler_BARO : public LR_MsgHandler -{ -public: - LR_MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, AP_Baro &_baro) - : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { }; - - virtual void process_message(uint8_t *msg); - -private: - AP_Baro &baro; -}; - - -class LR_MsgHandler_Event : public LR_MsgHandler -{ -public: - LR_MsgHandler_Event(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 LR_MsgHandler_GPS_Base : public LR_MsgHandler -{ - -public: - 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) { }; - -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 LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base -{ -public: - 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) - : 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) { }; - - 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 LR_MsgHandler_GPS2 : public LR_MsgHandler_GPS_Base -{ -public: - 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) - : 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: - AP_GPS &gps; - uint32_t &ground_alt_cm; - float &rel_altitude; -}; - - - -class LR_MsgHandler_IMU_Base : public LR_MsgHandler -{ -public: - 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) : - LR_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 LR_MsgHandler_IMU : public LR_MsgHandler_IMU_Base -{ -public: - 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) - : LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec, - _accel_mask, _gyro_mask, _ins) { }; - - void process_message(uint8_t *msg); -}; - -class LR_MsgHandler_IMU2 : public LR_MsgHandler_IMU_Base -{ -public: - 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) - : LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec, - _accel_mask, _gyro_mask, _ins) {}; - - virtual void process_message(uint8_t *msg); -}; - -class LR_MsgHandler_IMU3 : public LR_MsgHandler_IMU_Base -{ -public: - 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) - : LR_MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec, - _accel_mask, _gyro_mask, _ins) {}; - - virtual void process_message(uint8_t *msg); -}; - - - -class LR_MsgHandler_MAG_Base : public LR_MsgHandler -{ -public: - LR_MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_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); - -private: - Compass &compass; -}; - -class LR_MsgHandler_MAG : public LR_MsgHandler_MAG_Base -{ -public: - LR_MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Compass &_compass) - : LR_MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; - - virtual void process_message(uint8_t *msg); -}; - -class LR_MsgHandler_MAG2 : public LR_MsgHandler_MAG_Base -{ -public: - LR_MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Compass &_compass) - : LR_MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; - - virtual void process_message(uint8_t *msg); -}; - - - -class LR_MsgHandler_MSG : public LR_MsgHandler -{ -public: - LR_MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, - VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) : - LR_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 LR_MsgHandler_NTUN_Copter : public LR_MsgHandler -{ -public: - LR_MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, Vector3f &_inavpos) - : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {}; - - virtual void process_message(uint8_t *msg); - -private: - Vector3f &inavpos; -}; - - -class LR_MsgHandler_PARM : public LR_MsgHandler -{ -public: - 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 LR_MsgHandler_SIM : public LR_MsgHandler -{ -public: - LR_MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash, - uint64_t &_last_timestamp_usec, - Vector3f &_sim_attitude) - : LR_MsgHandler(_f, _dataflash, _last_timestamp_usec), - sim_attitude(_sim_attitude) - { }; - - virtual void process_message(uint8_t *msg); - -private: - Vector3f &sim_attitude; -}; - #endif