Replay: move LR_MsgHandlers into their own files
This commit is contained in:
Peter Barker
parent
7eb588e9ff
commit
f996bf4307
334
Tools/Replay/LR_MsgHandler.cpp
Normal file
334
Tools/Replay/LR_MsgHandler.cpp
Normal file
@ -0,0 +1,334 @@
|
||||
#include <LR_MsgHandler.h>
|
||||
|
||||
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 <AP_AHRS.h>
|
||||
#include <VehicleType.h>
|
||||
|
||||
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; i<sizeof(ignore_parms)/sizeof(ignore_parms[0]); i++) {
|
||||
if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
|
||||
::printf("Ignoring set of %s to %f\n", name, value);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
enum ap_var_type var_type;
|
||||
AP_Param *vp = AP_Param::find(name, &var_type);
|
||||
if (vp == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (var_type == AP_PARAM_FLOAT) {
|
||||
((AP_Float *)vp)->set(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");
|
||||
}
|
||||
333
Tools/Replay/LR_MsgHandler.h
Normal file
333
Tools/Replay/LR_MsgHandler.h
Normal file
@ -0,0 +1,333 @@
|
||||
#ifndef AP_LR_MSGHANDLER_H
|
||||
#define AP_LR_MSGHANDLER_H
|
||||
|
||||
#include <MsgHandler.h>
|
||||
|
||||
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
|
||||
@ -1,5 +1,6 @@
|
||||
#include <VehicleType.h>
|
||||
#include <DataFlashFileReader.h>
|
||||
#include <LR_MsgHandler.h>
|
||||
|
||||
class LogReader : public DataFlashFileReader
|
||||
{
|
||||
|
||||
@ -1,7 +1,5 @@
|
||||
#include <MsgHandler.h>
|
||||
|
||||
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 <AP_AHRS.h>
|
||||
#include <VehicleType.h>
|
||||
|
||||
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; i<sizeof(ignore_parms)/sizeof(ignore_parms[0]); i++) {
|
||||
if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
|
||||
::printf("Ignoring set of %s to %f\n", name, value);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
enum ap_var_type var_type;
|
||||
AP_Param *vp = AP_Param::find(name, &var_type);
|
||||
if (vp == NULL) {
|
||||
return false;
|
||||
}
|
||||
if (var_type == AP_PARAM_FLOAT) {
|
||||
((AP_Float *)vp)->set(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");
|
||||
}
|
||||
|
||||
|
||||
@ -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<typename R>
|
||||
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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user