Replay: move LR_MsgHandlers into their own files

This commit is contained in:
Peter Barker 2015-06-14 17:17:42 +10:00
parent 7eb588e9ff
commit f996bf4307
5 changed files with 668 additions and 662 deletions

View 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");
}

View 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

View File

@ -1,5 +1,6 @@
#include <VehicleType.h>
#include <DataFlashFileReader.h>
#include <LR_MsgHandler.h>
class LogReader : public DataFlashFileReader
{

View File

@ -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");
}

View File

@ -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