Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Replay: merge MessageHandler_* into MsgHandler.h/MsgHandler.cpp

Browse Source 
These were trivial classes; having lots of these files around made
it painful to work with.
This commit is contained in:
Peter Barker 2015-05-18 20:00:30 +10:00 committed by Andrew Tridgell
parent d1808c645d
commit 91b761cfb0
43 changed files with 580 additions and 611 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
Tools/Replay/LogReader.cpp
View File

@ -17,23 +17,6 @@
#include <fcntl.h> #include <fcntl.h>
#include "MsgHandler.h" #include "MsgHandler.h"
#include "MsgHandler_PARM.h"
#include "MsgHandler_GPS.h"
#include "MsgHandler_GPS2.h"
#include "MsgHandler_MSG.h"
#include "MsgHandler_IMU.h"
#include "MsgHandler_IMU2.h"
#include "MsgHandler_IMU3.h"
#include "MsgHandler_SIM.h"
#include "MsgHandler_BARO.h"
#include "MsgHandler_ARM.h"
#include "MsgHandler_Event.h"
#include "MsgHandler_AHR2.h"
#include "MsgHandler_ATT.h"
#include "MsgHandler_MAG.h"
#include "MsgHandler_MAG2.h"
#include "MsgHandler_NTUN_Copter.h"
#include "MsgHandler_ARSP.h"
#define streq(x, y) (!strcmp(x, y)) #define streq(x, y) (!strcmp(x, y))

280
Tools/Replay/MsgHandler.cpp
View File

@ -287,3 +287,283 @@ void MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
require_field(msg, "TimeMS", timestamp); require_field(msg, "TimeMS", timestamp);
wait_timestamp(timestamp); wait_timestamp(timestamp);
} }
/*
* subclasses to handle specific messages below here
*/
void MsgHandler_AHR2::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw");
}
void MsgHandler_ARM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint8_t ArmState = require_field_uint8_t(msg, "ArmState");
hal.util->set_soft_armed(ArmState);
printf("Armed state: %u at %lu\n",
(unsigned)ArmState,
(unsigned long)hal.scheduler->millis());
dataflash.WriteBlock(msg, f.length);
}
void MsgHandler_ARSP::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
airspeed.setHIL(require_field_float(msg, "Airspeed"),
require_field_float(msg, "DiffPress"),
require_field_float(msg, "Temp"));
dataflash.WriteBlock(msg, f.length);
}
void MsgHandler_ATT::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw");
}
void MsgHandler_BARO::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
baro.setHIL(0,
require_field_float(msg, "Press"),
require_field_int16_t(msg, "Temp") * 0.01f);
dataflash.WriteBlock(msg, f.length);
}
#define DATA_ARMED 10
#define DATA_DISARMED 11
void MsgHandler_Event::process_message(uint8_t *msg)
{
uint8_t id = require_field_uint8_t(msg, "Id");
if (id == DATA_ARMED) {
hal.util->set_soft_armed(true);
printf("Armed at %lu\n",
(unsigned long)hal.scheduler->millis());
} else if (id == DATA_DISARMED) {
hal.util->set_soft_armed(false);
printf("Disarmed at %lu\n",
(unsigned long)hal.scheduler->millis());
}
dataflash.WriteBlock(msg, f.length);
}
void MsgHandler_GPS2::process_message(uint8_t *msg)
{
// only LOG_GPS_MSG gives us relative altitude. We still log
// the relative altitude when we get a LOG_GPS2_MESSAGE - but
// the value we use (probably) comes from the most recent
// LOG_GPS_MESSAGE message!
update_from_msg_gps(1, msg, false);
}
void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt)
{
uint32_t timestamp;
require_field(msg, "T", timestamp);
wait_timestamp(timestamp);
Location loc;
location_from_msg(msg, loc, "Lat", "Lng", "Alt");
Vector3f vel;
ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ");
uint8_t status = require_field_uint8_t(msg, "Status");
gps.setHIL(gps_offset,
(AP_GPS::GPS_Status)status,
timestamp,
loc,
vel,
require_field_uint8_t(msg, "NSats"),
require_field_uint8_t(msg, "HDop"),
require_field_float(msg, "VZ") != 0);
if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
ground_alt_cm = require_field_int32_t(msg, "Alt");
}
if (responsible_for_relalt) {
// this could possibly check for the presence of "RelAlt" label?
rel_altitude = 0.01f * require_field_int32_t(msg, "RelAlt");
}
dataflash.WriteBlock(msg, f.length);
}
void MsgHandler_GPS::process_message(uint8_t *msg)
{
update_from_msg_gps(0, msg, true);
}
void MsgHandler_IMU2::process_message(uint8_t *msg)
{
update_from_msg_imu(1, msg);
}
void MsgHandler_IMU3::process_message(uint8_t *msg)
{
update_from_msg_imu(2, msg);
}
void MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint8_t this_imu_mask = 1 << imu_offset;
if (gyro_mask & this_imu_mask) {
Vector3f gyro;
require_field(msg, "Gyr", gyro);
ins.set_gyro(imu_offset, gyro);
}
if (accel_mask & this_imu_mask) {
Vector3f accel2;
require_field(msg, "Acc", accel2);
ins.set_accel(imu_offset, accel2);
}
dataflash.WriteBlock(msg, f.length);
}
void MsgHandler_IMU::process_message(uint8_t *msg)
{
update_from_msg_imu(0, msg);
}
void MsgHandler_MAG2::process_message(uint8_t *msg)
{
update_from_msg_compass(1, msg);
}
void MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
Vector3f mag;
require_field(msg, "Mag", mag);
Vector3f mag_offset;
require_field(msg, "Ofs", mag_offset);
compass.setHIL(compass_offset, mag - mag_offset);
// compass_offset is which compass we are setting info for;
// mag_offset is a vector indicating the compass' calibration...
compass.set_offsets(compass_offset, mag_offset);
dataflash.WriteBlock(msg, f.length);
}
void MsgHandler_MAG::process_message(uint8_t *msg)
{
update_from_msg_compass(0, msg);
}
#include <AP_AHRS.h>
#include <VehicleType.h>
void MsgHandler_MSG::process_message(uint8_t *msg)
{
const uint8_t msg_text_len = 64;
char msg_text[msg_text_len];
require_field(msg, "Message", msg_text, msg_text_len);
if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VehicleType::VEHICLE_PLANE;
::printf("Detected Plane\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_fly_forward(true);
} else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 ||
strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) {
vehicle = VehicleType::VEHICLE_COPTER;
::printf("Detected Copter\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
ahrs.set_fly_forward(false);
} else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) {
vehicle = VehicleType::VEHICLE_ROVER;
::printf("Detected Rover\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
ahrs.set_fly_forward(true);
}
dataflash.Log_Write_Message(msg_text);
}
void MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
{
inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f,
require_field_float(msg, "PosY") * 0.01f,
0);
}
bool MsgHandler::set_parameter(const char *name, float value)
{
const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE" };
for (uint8_t i=0; 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 MsgHandler_PARM::process_message(uint8_t *msg)
{
const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term
char parameter_name[parameter_name_len];
require_field(msg, "Name", parameter_name, parameter_name_len);
set_parameter(parameter_name, require_field_float(msg, "Value"));
}
void MsgHandler_SIM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, sim_attitude, "Roll", "Pitch", "Yaw");
}

300
Tools/Replay/MsgHandler.h
View File

@ -2,6 +2,7 @@
#define AP_MSGHANDLER_H #define AP_MSGHANDLER_H
#include <DataFlash.h> #include <DataFlash.h>
#include <VehicleType.h>
#include <stdio.h> #include <stdio.h>
@ -158,4 +159,303 @@ inline void MsgHandler::field_value_for_type_at_offset(uint8_t *msg,
} }
} }
/* subclasses below this point */
class MsgHandler_AHR2 : public MsgHandler
{
public:
MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude)
: MsgHandler(_f, _dataflash,_last_timestamp_usec),
ahr2_attitude(_ahr2_attitude) { };
virtual void process_message(uint8_t *msg);
private:
Vector3f &ahr2_attitude;
};
class MsgHandler_ARM : public MsgHandler
{
public:
MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};
class MsgHandler_ARSP : public MsgHandler
{
public:
MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) :
MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { };
virtual void process_message(uint8_t *msg);
private:
AP_Airspeed &airspeed;
};
class MsgHandler_ATT : public MsgHandler
{
public:
MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_attitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude)
{ };
virtual void process_message(uint8_t *msg);
private:
Vector3f &attitude;
};
class MsgHandler_BARO : public MsgHandler
{
public:
MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_Baro &_baro)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { };
virtual void process_message(uint8_t *msg);
private:
AP_Baro &baro;
};
class MsgHandler_Event : public MsgHandler
{
public:
MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};
class MsgHandler_GPS_Base : public MsgHandler
{
public:
MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec),
gps(_gps), ground_alt_cm(_ground_alt_cm),
rel_altitude(_rel_altitude) { };
protected:
void update_from_msg_gps(uint8_t imu_offset, uint8_t *data, bool responsible_for_relalt);
private:
AP_GPS &gps;
uint32_t &ground_alt_cm;
float &rel_altitude;
};
class MsgHandler_GPS : public MsgHandler_GPS_Base
{
public:
MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
_gps, _ground_alt_cm, _rel_altitude),
gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
void process_message(uint8_t *msg);
private:
AP_GPS &gps;
uint32_t &ground_alt_cm;
float &rel_altitude;
};
// it would be nice to use the same parser for both GPS message types
// (and other packets, too...). I*think* the contructor can simply
// take e.g. &gps[1]... problems are going to arise if we don't
// actually have that many gps' compiled in!
class MsgHandler_GPS2 : public MsgHandler_GPS_Base
{
public:
MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
_gps, _ground_alt_cm,
_rel_altitude), gps(_gps),
ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
virtual void process_message(uint8_t *msg);
private:
AP_GPS &gps;
uint32_t &ground_alt_cm;
float &rel_altitude;
};
class MsgHandler_IMU_Base : public MsgHandler
{
public:
MsgHandler_IMU_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins) :
MsgHandler(_f, _dataflash, _last_timestamp_usec),
accel_mask(_accel_mask),
gyro_mask(_gyro_mask),
ins(_ins) { };
void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg);
private:
uint8_t &accel_mask;
uint8_t &gyro_mask;
AP_InertialSensor &ins;
};
class MsgHandler_IMU : public MsgHandler_IMU_Base
{
public:
MsgHandler_IMU(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) { };
void process_message(uint8_t *msg);
};
class MsgHandler_IMU2 : public MsgHandler_IMU_Base
{
public:
MsgHandler_IMU2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_IMU3 : public MsgHandler_IMU_Base
{
public:
MsgHandler_IMU3(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_MAG_Base : public MsgHandler
{
public:
MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { };
protected:
void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg);
private:
Compass &compass;
};
class MsgHandler_MAG : public MsgHandler_MAG_Base
{
public:
MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_MAG2 : public MsgHandler_MAG_Base
{
public:
MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_MSG : public MsgHandler
{
public:
MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) :
MsgHandler(_f, _dataflash, _last_timestamp_usec),
vehicle(_vehicle), ahrs(_ahrs) { }
virtual void process_message(uint8_t *msg);
private:
VehicleType::vehicle_type &vehicle;
AP_AHRS &ahrs;
};
class MsgHandler_NTUN_Copter : public MsgHandler
{
public:
MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_inavpos)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {};
virtual void process_message(uint8_t *msg);
private:
Vector3f &inavpos;
};
class MsgHandler_PARM : public MsgHandler
{
public:
MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t _last_timestamp_usec) : MsgHandler(_f, _dataflash, _last_timestamp_usec) {};
virtual void process_message(uint8_t *msg);
};
class MsgHandler_SIM : public MsgHandler
{
public:
MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
Vector3f &_sim_attitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec),
sim_attitude(_sim_attitude)
{ };
virtual void process_message(uint8_t *msg);
private:
Vector3f &sim_attitude;
};
#endif #endif

7
Tools/Replay/MsgHandler_AHR2.cpp
View File

@ -1,7 +0,0 @@
#include "MsgHandler_AHR2.h"
void MsgHandler_AHR2::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw");
}

15
Tools/Replay/MsgHandler_AHR2.h
View File

@ -1,15 +0,0 @@
#include "MsgHandler.h"
class MsgHandler_AHR2 : public MsgHandler
{
public:
MsgHandler_AHR2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude)
: MsgHandler(_f, _dataflash,_last_timestamp_usec),
ahr2_attitude(_ahr2_attitude) { };
virtual void process_message(uint8_t *msg);
private:
Vector3f &ahr2_attitude;
};

14
Tools/Replay/MsgHandler_ARM.cpp
View File

@ -1,14 +0,0 @@
#include "MsgHandler_ARM.h"
extern const AP_HAL::HAL& hal;
void MsgHandler_ARM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint8_t ArmState = require_field_uint8_t(msg, "ArmState");
hal.util->set_soft_armed(ArmState);
printf("Armed state: %u at %lu\n",
(unsigned)ArmState,
(unsigned long)hal.scheduler->millis());
dataflash.WriteBlock(msg, f.length);
}

11
Tools/Replay/MsgHandler_ARM.h
View File

@ -1,11 +0,0 @@
#include "MsgHandler.h"
class MsgHandler_ARM : public MsgHandler
{
public:
MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};

11
Tools/Replay/MsgHandler_ARSP.cpp
View File

@ -1,11 +0,0 @@
#include "MsgHandler_ARSP.h"
void MsgHandler_ARSP::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
airspeed.setHIL(require_field_float(msg, "Airspeed"),
require_field_float(msg, "DiffPress"),
require_field_float(msg, "Temp"));
dataflash.WriteBlock(msg, f.length);
}

14
Tools/Replay/MsgHandler_ARSP.h
View File

@ -1,14 +0,0 @@
#include "MsgHandler.h"
class MsgHandler_ARSP : public MsgHandler
{
public:
MsgHandler_ARSP(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) :
MsgHandler(_f, _dataflash, _last_timestamp_usec), airspeed(_airspeed) { };
virtual void process_message(uint8_t *msg);
private:
AP_Airspeed &airspeed;
};

7
Tools/Replay/MsgHandler_ATT.cpp
View File

@ -1,7 +0,0 @@
#include "MsgHandler_ATT.h"
void MsgHandler_ATT::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw");
}

14
Tools/Replay/MsgHandler_ATT.h
View File

@ -1,14 +0,0 @@
#include "MsgHandler.h"
class MsgHandler_ATT : public MsgHandler
{
public:
MsgHandler_ATT(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_attitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), attitude(_attitude)
{ };
virtual void process_message(uint8_t *msg);
private:
Vector3f &attitude;
};

10
Tools/Replay/MsgHandler_BARO.cpp
View File

@ -1,10 +0,0 @@
#include "MsgHandler_BARO.h"
void MsgHandler_BARO::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
baro.setHIL(0,
require_field_float(msg, "Press"),
require_field_int16_t(msg, "Temp") * 0.01f);
dataflash.WriteBlock(msg, f.length);
}

14
Tools/Replay/MsgHandler_BARO.h
View File

@ -1,14 +0,0 @@
#include "MsgHandler.h"
class MsgHandler_BARO : public MsgHandler
{
public:
MsgHandler_BARO(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_Baro &_baro)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), baro(_baro) { };
virtual void process_message(uint8_t *msg);
private:
AP_Baro &baro;
};

21
Tools/Replay/MsgHandler_Event.cpp
View File

@ -1,21 +0,0 @@
#include "MsgHandler_Event.h"
extern const AP_HAL::HAL& hal;
#define DATA_ARMED 10
#define DATA_DISARMED 11
void MsgHandler_Event::process_message(uint8_t *msg)
{
uint8_t id = require_field_uint8_t(msg, "Id");
if (id == DATA_ARMED) {
hal.util->set_soft_armed(true);
printf("Armed at %lu\n",
(unsigned long)hal.scheduler->millis());
} else if (id == DATA_DISARMED) {
hal.util->set_soft_armed(false);
printf("Disarmed at %lu\n",
(unsigned long)hal.scheduler->millis());
}
dataflash.WriteBlock(msg, f.length);
}

11
Tools/Replay/MsgHandler_Event.h
View File

@ -1,11 +0,0 @@
#include "MsgHandler.h"
class MsgHandler_Event : public MsgHandler
{
public:
MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};

6
Tools/Replay/MsgHandler_GPS.cpp
View File

@ -1,6 +0,0 @@
#include "MsgHandler_GPS.h"
void MsgHandler_GPS::process_message(uint8_t *msg)
{
update_from_msg_gps(0, msg, true);
}

19
Tools/Replay/MsgHandler_GPS.h
View File

@ -1,19 +0,0 @@
#include "MsgHandler_GPS_Base.h"
class MsgHandler_GPS : public MsgHandler_GPS_Base
{
public:
MsgHandler_GPS(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler_GPS_Base(_f, _dataflash,_last_timestamp_usec,
_gps, _ground_alt_cm, _rel_altitude),
gps(_gps), ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
void process_message(uint8_t *msg);
private:
AP_GPS &gps;
uint32_t &ground_alt_cm;
float &rel_altitude;
};

10
Tools/Replay/MsgHandler_GPS2.cpp
View File

@ -1,10 +0,0 @@
#include <MsgHandler_GPS2.h>
void MsgHandler_GPS2::process_message(uint8_t *msg)
{
// only LOG_GPS_MSG gives us relative altitude. We still log
// the relative altitude when we get a LOG_GPS2_MESSAGE - but
// the value we use (probably) comes from the most recent
// LOG_GPS_MESSAGE message!
update_from_msg_gps(1, msg, false);
}

22
Tools/Replay/MsgHandler_GPS2.h
View File

@ -1,22 +0,0 @@
#include "MsgHandler_GPS_Base.h"
// it would be nice to use the same parser for both GPS message types
// (and other packets, too...). I*think* the contructor can simply
// take e.g. &gps[1]... problems are going to arise if we don't
// actually have that many gps' compiled in!
class MsgHandler_GPS2 : public MsgHandler_GPS_Base
{
public:
MsgHandler_GPS2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler_GPS_Base(_f, _dataflash, _last_timestamp_usec,
_gps, _ground_alt_cm,
_rel_altitude), gps(_gps),
ground_alt_cm(_ground_alt_cm), rel_altitude(_rel_altitude) { };
virtual void process_message(uint8_t *msg);
private:
AP_GPS &gps;
uint32_t &ground_alt_cm;
float &rel_altitude;
};

34
Tools/Replay/MsgHandler_GPS_Base.cpp
View File

@ -1,34 +0,0 @@
#include "MsgHandler_GPS_Base.h"
void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt)
{
uint32_t timestamp;
require_field(msg, "T", timestamp);
wait_timestamp(timestamp);
Location loc;
location_from_msg(msg, loc, "Lat", "Lng", "Alt");
Vector3f vel;
ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ");
uint8_t status = require_field_uint8_t(msg, "Status");
gps.setHIL(gps_offset,
(AP_GPS::GPS_Status)status,
timestamp,
loc,
vel,
require_field_uint8_t(msg, "NSats"),
require_field_uint8_t(msg, "HDop"),
require_field_float(msg, "VZ") != 0);
if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
ground_alt_cm = require_field_int32_t(msg, "Alt");
}
if (responsible_for_relalt) {
// this could possibly check for the presence of "RelAlt" label?
rel_altitude = 0.01f * require_field_int32_t(msg, "RelAlt");
}
dataflash.WriteBlock(msg, f.length);
}

27
Tools/Replay/MsgHandler_GPS_Base.h
View File

@ -1,27 +0,0 @@
#include <MsgHandler.h>
#ifndef AP_MSGHANDLER_GPS_BASE_H
#define AP_MSGHANDLER_GPS_BASE_H
class MsgHandler_GPS_Base : public MsgHandler
{
public:
MsgHandler_GPS_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, AP_GPS &_gps,
uint32_t &_ground_alt_cm, float &_rel_altitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec),
gps(_gps), ground_alt_cm(_ground_alt_cm),
rel_altitude(_rel_altitude) { };
protected:
void update_from_msg_gps(uint8_t imu_offset, uint8_t *data, bool responsible_for_relalt);
private:
AP_GPS &gps;
uint32_t &ground_alt_cm;
float &rel_altitude;
};
#endif

6
Tools/Replay/MsgHandler_IMU.cpp
View File

@ -1,6 +0,0 @@
#include "MsgHandler_IMU.h"
void MsgHandler_IMU::process_message(uint8_t *msg)
{
update_from_msg_imu(0, msg);
}

14
Tools/Replay/MsgHandler_IMU.h
View File

@ -1,14 +0,0 @@
#include "MsgHandler_IMU_Base.h"
class MsgHandler_IMU : public MsgHandler_IMU_Base
{
public:
MsgHandler_IMU(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) { };
void process_message(uint8_t *msg);
};

6
Tools/Replay/MsgHandler_IMU2.cpp
View File

@ -1,6 +0,0 @@
#include "MsgHandler_IMU2.h"
void MsgHandler_IMU2::process_message(uint8_t *msg)
{
update_from_msg_imu(1, msg);
}

14
Tools/Replay/MsgHandler_IMU2.h
View File

@ -1,14 +0,0 @@
#include "MsgHandler_IMU_Base.h"
class MsgHandler_IMU2 : public MsgHandler_IMU_Base
{
public:
MsgHandler_IMU2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) {};
virtual void process_message(uint8_t *msg);
};

6
Tools/Replay/MsgHandler_IMU3.cpp
View File

@ -1,6 +0,0 @@
#include "MsgHandler_IMU3.h"
void MsgHandler_IMU3::process_message(uint8_t *msg)
{
update_from_msg_imu(2, msg);
}

14
Tools/Replay/MsgHandler_IMU3.h
View File

@ -1,14 +0,0 @@
#include "MsgHandler_IMU_Base.h"
class MsgHandler_IMU3 : public MsgHandler_IMU_Base
{
public:
MsgHandler_IMU3(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins)
: MsgHandler_IMU_Base(_f, _dataflash, _last_timestamp_usec,
_accel_mask, _gyro_mask, _ins) {};
virtual void process_message(uint8_t *msg);
};

21
Tools/Replay/MsgHandler_IMU_Base.cpp
View File

@ -1,21 +0,0 @@
#include "MsgHandler_IMU_Base.h"
void MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
uint8_t this_imu_mask = 1 << imu_offset;
if (gyro_mask & this_imu_mask) {
Vector3f gyro;
require_field(msg, "Gyr", gyro);
ins.set_gyro(imu_offset, gyro);
}
if (accel_mask & this_imu_mask) {
Vector3f accel2;
require_field(msg, "Acc", accel2);
ins.set_accel(imu_offset, accel2);
}
dataflash.WriteBlock(msg, f.length);
}

26
Tools/Replay/MsgHandler_IMU_Base.h
View File

@ -1,26 +0,0 @@
#ifndef AP_MSGHANDLER_IMU_BASE_H
#define AP_MSGHANDLER_IMU_BASE_H
#include <MsgHandler.h>
class MsgHandler_IMU_Base : public MsgHandler
{
public:
MsgHandler_IMU_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
uint8_t &_accel_mask, uint8_t &_gyro_mask,
AP_InertialSensor &_ins) :
MsgHandler(_f, _dataflash, _last_timestamp_usec),
accel_mask(_accel_mask),
gyro_mask(_gyro_mask),
ins(_ins) { };
void update_from_msg_imu(uint8_t gps_offset, uint8_t *msg);
private:
uint8_t &accel_mask;
uint8_t &gyro_mask;
AP_InertialSensor &ins;
};
#endif

6
Tools/Replay/MsgHandler_MAG.cpp
View File

@ -1,6 +0,0 @@
#include "MsgHandler_MAG.h"
void MsgHandler_MAG::process_message(uint8_t *msg)
{
update_from_msg_compass(0, msg);
}

11
Tools/Replay/MsgHandler_MAG.h
View File

@ -1,11 +0,0 @@
#include "MsgHandler_MAG_Base.h"
class MsgHandler_MAG : public MsgHandler_MAG_Base
{
public:
MsgHandler_MAG(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
virtual void process_message(uint8_t *msg);
};

6
Tools/Replay/MsgHandler_MAG2.cpp
View File

@ -1,6 +0,0 @@
#include "MsgHandler_MAG2.h"
void MsgHandler_MAG2::process_message(uint8_t *msg)
{
update_from_msg_compass(1, msg);
}

11
Tools/Replay/MsgHandler_MAG2.h
View File

@ -1,11 +0,0 @@
#include "MsgHandler_MAG_Base.h"
class MsgHandler_MAG2 : public MsgHandler_MAG_Base
{
public:
MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {};
virtual void process_message(uint8_t *msg);
};

19
Tools/Replay/MsgHandler_MAG_Base.cpp
View File

@ -1,19 +0,0 @@
#include "MsgHandler_MAG_Base.h"
void MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
{
wait_timestamp_from_msg(msg);
Vector3f mag;
require_field(msg, "Mag", mag);
Vector3f mag_offset;
require_field(msg, "Ofs", mag_offset);
compass.setHIL(compass_offset, mag - mag_offset);
// compass_offset is which compass we are setting info for;
// mag_offset is a vector indicating the compass' calibration...
compass.set_offsets(compass_offset, mag_offset);
dataflash.WriteBlock(msg, f.length);
}

20
Tools/Replay/MsgHandler_MAG_Base.h
View File

@ -1,20 +0,0 @@
#ifndef MSGHANDLER_MAG_BASE_H
#define MSGHANDLER_MAG_BASE_H
#include "MsgHandler.h"
class MsgHandler_MAG_Base : public MsgHandler
{
public:
MsgHandler_MAG_Base(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Compass &_compass)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), compass(_compass) { };
protected:
void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg);
private:
Compass &compass;
};
#endif // MSGHANDLER_MAG_BASE

29
Tools/Replay/MsgHandler_MSG.cpp
View File

@ -1,29 +0,0 @@
#include "MsgHandler_MSG.h"
#include <AP_AHRS.h>
#include <VehicleType.h>
void MsgHandler_MSG::process_message(uint8_t *msg)
{
const uint8_t msg_text_len = 64;
char msg_text[msg_text_len];
require_field(msg, "Message", msg_text, msg_text_len);
if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VehicleType::VEHICLE_PLANE;
::printf("Detected Plane\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_fly_forward(true);
} else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 ||
strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) {
vehicle = VehicleType::VEHICLE_COPTER;
::printf("Detected Copter\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
ahrs.set_fly_forward(false);
} else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) {
vehicle = VehicleType::VEHICLE_ROVER;
::printf("Detected Rover\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
ahrs.set_fly_forward(true);
}
dataflash.Log_Write_Message(msg_text);
}

19
Tools/Replay/MsgHandler_MSG.h
View File

@ -1,19 +0,0 @@
#include <MsgHandler.h>
#include <VehicleType.h>
class MsgHandler_MSG : public MsgHandler
{
public:
MsgHandler_MSG(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) :
MsgHandler(_f, _dataflash, _last_timestamp_usec),
vehicle(_vehicle), ahrs(_ahrs) { }
virtual void process_message(uint8_t *msg);
private:
VehicleType::vehicle_type &vehicle;
AP_AHRS &ahrs;
};

8
Tools/Replay/MsgHandler_NTUN_Copter.cpp
View File

@ -1,8 +0,0 @@
#include "MsgHandler_NTUN_Copter.h"
void MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
{
inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f,
require_field_float(msg, "PosY") * 0.01f,
0);
}

14
Tools/Replay/MsgHandler_NTUN_Copter.h
View File

@ -1,14 +0,0 @@
#include "MsgHandler.h"
class MsgHandler_NTUN_Copter : public MsgHandler
{
public:
MsgHandler_NTUN_Copter(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec, Vector3f &_inavpos)
: MsgHandler(_f, _dataflash, _last_timestamp_usec), inavpos(_inavpos) {};
virtual void process_message(uint8_t *msg);
private:
Vector3f &inavpos;
};

44
Tools/Replay/MsgHandler_PARM.cpp
View File

@ -1,44 +0,0 @@
#include "MsgHandler_PARM.h"
bool MsgHandler::set_parameter(const char *name, float value)
{
const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_USE" };
for (uint8_t i=0; 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 MsgHandler_PARM::process_message(uint8_t *msg)
{
const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term
char parameter_name[parameter_name_len];
require_field(msg, "Name", parameter_name, parameter_name_len);
set_parameter(parameter_name, require_field_float(msg, "Value"));
}

9
Tools/Replay/MsgHandler_PARM.h
View File

@ -1,9 +0,0 @@
#include <MsgHandler.h>
class MsgHandler_PARM : public MsgHandler
{
public:
MsgHandler_PARM(log_Format &_f, DataFlash_Class &_dataflash, uint64_t _last_timestamp_usec) : MsgHandler(_f, _dataflash, _last_timestamp_usec) {};
virtual void process_message(uint8_t *msg);
};

7
Tools/Replay/MsgHandler_SIM.cpp
View File

@ -1,7 +0,0 @@
#include "MsgHandler_SIM.h"
void MsgHandler_SIM::process_message(uint8_t *msg)
{
wait_timestamp_from_msg(msg);
attitude_from_msg(msg, sim_attitude, "Roll", "Pitch", "Yaw");
}

17
Tools/Replay/MsgHandler_SIM.h
View File

@ -1,17 +0,0 @@
#include "MsgHandler.h"
class MsgHandler_SIM : public MsgHandler
{
public:
MsgHandler_SIM(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec,
Vector3f &_sim_attitude)
: MsgHandler(_f, _dataflash, _last_timestamp_usec),
sim_attitude(_sim_attitude)
{ };
virtual void process_message(uint8_t *msg);
private:
Vector3f &sim_attitude;
};