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

Replay: use the message's name rather than number to determine which message handler to use for a message

Browse Source
This commit is contained in:
Peter Barker 2015-04-27 13:25:16 +10:00 committed by Andrew Tridgell
parent 42351edfa6
commit 56a8bf5460
40 changed files with 1145 additions and 729 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

820
Tools/Replay/LogReader.cpp
View File

@ -16,12 +16,28 @@
#include <sys/stat.h>
#include <fcntl.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_AHR2.h"
#include "MsgHandler_ATT.h"
#include "MsgHandler_MAG.h"
#include "MsgHandler_NTUN_Copter.h"
#include "MsgHandler_ARSP.h"
#define streq(x, y) (!strcmp(x, y))
extern const AP_HAL::HAL& hal;
LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash) :
vehicle(VEHICLE_UNKNOWN),
vehicle(VehicleType::VEHICLE_UNKNOWN),
fd(-1),
ahrs(_ahrs),
ins(_ins),
@ -32,296 +48,10 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Co
dataflash(_dataflash),
accel_mask(7),
gyro_mask(7),
last_timestamp_usec(0)
last_timestamp_usec(0),
installed_vehicle_specific_parsers(false)
{}
void fatal(const char *msg) {
::printf("%s",msg);
::printf("\n");
exit(1);
}
char *xstrdup(const char *string)
{
char *ret = strdup(string);
if (ret == NULL) {
perror("strdup");
fatal("strdup failed");
}
return ret;
}
#define LOGREADER_MAX_FIELDS 30
class MsgParser {
public:
// constructor - create a parser for a MavLink message format
MsgParser(struct log_Format f);
// field_value - retrieve the value of a field from the supplied message
// these return false if the field was not found
template<typename R>
bool field_value(uint8_t *msg, const char *label, R &ret);
bool field_value(uint8_t *msg, const char *label, Vector3f &ret);
bool field_value(uint8_t *msg, const char *label,
char *buffer, uint8_t bufferlen);
// retrieve a comma-separated list of all labels
void string_for_labels(char *buffer, uint bufferlen);
private:
void add_field(const char *_label, uint8_t _type, uint8_t _offset,
uint8_t length);
template<typename R>
void field_value_for_type_at_offset(uint8_t *msg, uint8_t type,
uint8_t offset, R &ret);
struct log_Format f; // the format we are a parser for
struct format_field_info { // parsed field information
char *label;
uint8_t type;
uint8_t offset;
uint8_t length;
};
struct format_field_info field_info[LOGREADER_MAX_FIELDS];
uint8_t next_field;
size_t size_for_type_table[52]; // maps field type (e.g. 'f') to e.g 4 bytes
struct format_field_info *find_field_info(const char *label);
void parse_format_fields();
void init_field_types();
void add_field_type(char type, size_t size);
uint8_t size_for_type(char type);
~MsgParser();
};
void MsgParser::add_field_type(char type, size_t size)
{
size_for_type_table[(type > 'A' ? (type-'A') : (type-'a'))] = size;
}
uint8_t MsgParser::size_for_type(char type)
{
return size_for_type_table[(uint8_t)(type > 'A' ? (type-'A') : (type-'a'))];
}
void MsgParser::init_field_types()
{
add_field_type('b', sizeof(int8_t));
add_field_type('c', sizeof(int16_t));
add_field_type('e', sizeof(int32_t));
add_field_type('f', sizeof(float));
add_field_type('h', sizeof(int16_t));
add_field_type('i', sizeof(int32_t));
add_field_type('n', sizeof(char[4]));
add_field_type('B', sizeof(uint8_t));
add_field_type('C', sizeof(uint16_t));
add_field_type('E', sizeof(uint32_t));
add_field_type('H', sizeof(uint16_t));
add_field_type('I', sizeof(uint32_t));
add_field_type('L', sizeof(int32_t));
add_field_type('M', sizeof(uint8_t));
add_field_type('N', sizeof(char[16]));
add_field_type('Z', sizeof(char[64]));
}
struct MsgParser::format_field_info *MsgParser::find_field_info(const char *label)
{
for(uint8_t i=0; i<next_field; i++) {
if (streq(field_info[i].label, label)) {
return &field_info[i];
}
}
return NULL;
}
MsgParser::MsgParser(struct log_Format fx) : next_field(0), f(fx)
{
init_field_types();
parse_format_fields();
}
void MsgParser::add_field(const char *_label, uint8_t _type, uint8_t _offset,
uint8_t _length)
{
field_info[next_field].label = xstrdup(_label);
field_info[next_field].type = _type;
field_info[next_field].offset = _offset;
field_info[next_field].length = _length;
next_field++;
}
void MsgParser::parse_format_fields()
{
char *labels = xstrdup(f.labels);
char * arg = labels;
uint8_t label_offset = 0;
char *next_label;
uint8_t msg_offset = 3; // 3 bytes for the header
while ((next_label = strtok(arg, ",")) != NULL) {
if (label_offset > strlen(f.format)) {
free(labels);
printf("too few field times for labels %s (format=%s) (labels=%s)\n",
f.name, f.format, f.labels);
exit(1);
}
uint8_t field_type = f.format[label_offset];
uint8_t length = size_for_type(field_type);
add_field(next_label, field_type, msg_offset, length);
arg = NULL;
msg_offset += length;
label_offset++;
}
if (label_offset != strlen(f.format)) {
free(labels);
printf("too few labels for format (format=%s) (labels=%s)\n",
f.format, f.labels);
}
free(labels);
}
template<typename R>
inline void MsgParser::field_value_for_type_at_offset(uint8_t *msg,
uint8_t type,
uint8_t offset,
R &ret)
{
/* we register the types - add_field_type - so can we do without
* this switch statement somehow? */
switch (type) {
case 'B':
ret = (R)(((uint8_t*)&msg[offset])[0]);
break;
case 'c':
case 'h':
ret = (R)(((int16_t*)&msg[offset])[0]);
break;
case 'H':
ret = (R)(((uint16_t*)&msg[offset])[0]);
break;
case 'C':
ret = (R)(((uint16_t*)&msg[offset])[0]);
break;
case 'f':
ret = (R)(((float*)&msg[offset])[0]);
break;
case 'I':
case 'E':
ret = (R)(((uint32_t*)&msg[offset])[0]);
break;
case 'L':
case 'e':
ret = (R)(((int32_t*)&msg[offset])[0]);
break;
default:
::printf("Unhandled format type (%c)\n", type);
exit(1);
}
}
template<typename R>
bool MsgParser::field_value(uint8_t *msg, const char *label, R &ret)
{
struct format_field_info *info = find_field_info(label);
uint8_t offset = info->offset;
if (offset == 0) {
return false;
}
field_value_for_type_at_offset(msg, info->type, offset, ret);
return true;
}
bool MsgParser::field_value(uint8_t *msg, const char *label, char *ret, uint8_t retlen)
{
struct format_field_info *info = find_field_info(label);
uint8_t offset = info->offset;
if (offset == 0) {
return false;
}
memset(ret, '\0', retlen);
memcpy(ret, &msg[offset], (retlen < info->length) ? retlen : info->length);
return true;
}
bool MsgParser::field_value(uint8_t *msg, const char *label, Vector3f &ret)
{
const char *axes = "XYZ";
uint8_t i;
for(i=0; i<next_field; i++) {
if (!strncmp(field_info[i].label, label, strlen(label)) &&
strlen(field_info[i].label) == strlen(label)+1) {
for (uint8_t j=0; j<3; j++) {
if (field_info[i].label[strlen(label)] == axes[j]) {
field_value_for_type_at_offset(msg,
field_info[i].type,
field_info[i].offset,
ret[j]);
break; // break from finding-label loop
}
}
}
if (i == next_field) {
return 0; // not found
}
}
return true;
}
void MsgParser::string_for_labels(char *buffer, uint bufferlen)
{
memset(buffer, '\0', bufferlen);
bufferlen--;
char *pos = buffer;
for (uint8_t k=0; k<LOGREADER_MAX_FIELDS; k++) {
if (field_info[k].label != NULL) {
uint8_t remaining = bufferlen - (pos - buffer);
uint8_t label_length = strlen(field_info[k].label);
uint8_t required = label_length;
if (pos != buffer) { // room for a comma
required++;
}
if (required+1 > remaining) { // null termination
break;
}
if (pos != buffer) {
*pos++ = ',';
}
memcpy(pos, field_info[k].label, label_length);
pos += label_length;
}
}
}
MsgParser::~MsgParser()
{
for (uint8_t k=0; k<LOGREADER_MAX_FIELDS; k++) {
if (field_info[k].label != NULL) {
free(field_info[k].label);
}
}
}
bool LogReader::open_log(const char *logfile)
{
fd = ::open(logfile, O_RDONLY);
@ -331,283 +61,38 @@ bool LogReader::open_log(const char *logfile)
return true;
}
void LogReader::update_plane(uint8_t type, uint8_t *data, uint16_t length)
{
MsgParser *p = msgparser[type];
struct log_Format deferred_formats[LOGREADER_MAX_FORMATS];
switch (type) {
case LOG_PLANE_COMPASS_MSG: {
update_from_msg_compass(0, p, data);
break;
}
case LOG_PLANE_ATTITUDE_MSG: {
wait_timestamp_from_msg(p, data);
attitude_from_msg(p, data, attitude, "Roll", "Pitch", "Yaw");
break;
}
case LOG_PLANE_AIRSPEED_MSG:
case LOG_ARSP_MSG: {
wait_timestamp_from_msg(p, data);
p = msgparser[LOG_ARSP_MSG]; // this actually uses a non-"old" structure
airspeed.setHIL(require_field_float(p, data, "AirSpeed"),
require_field_float(p, data, "DiffPress"),
require_field_float(p, data, "Temp"));
dataflash.Log_Write_Airspeed(airspeed);
break;
}
// some log entries (e.g. "NTUN") are used by the different vehicle
// types with wildy varying payloads. We thus can't use the same
// parser for just any e.g. NTUN message. We defer the registration
// of a parser for these messages until we know what model we're
// dealing with.
void LogReader::maybe_install_vehicle_specific_parsers() {
if (! installed_vehicle_specific_parsers &&
vehicle != VehicleType::VEHICLE_UNKNOWN) {
switch(vehicle) {
case VehicleType::VEHICLE_COPTER:
for (uint8_t i = 0; i<LOGREADER_MAX_FORMATS; i++) {
if (deferred_formats[i].type != 0) {
msgparser[i] = new MsgHandler_NTUN_Copter
(deferred_formats[i], dataflash, last_timestamp_usec,
inavpos);
}
}
break;
case VehicleType::VEHICLE_PLANE:
break;
case VehicleType::VEHICLE_ROVER:
break;
case VehicleType::VEHICLE_UNKNOWN:
break;
}
installed_vehicle_specific_parsers = true;
}
}
void LogReader::update_rover(uint8_t type, uint8_t *data, uint16_t length)
{
MsgParser *p = msgparser[type];
switch (type) {
case LOG_ROVER_COMPASS_MSG: {
wait_timestamp_from_msg(p, data);
update_from_msg_compass(0, p, data);
break;
}
case LOG_ROVER_ATTITUDE_MSG: {
wait_timestamp_from_msg(p, data);
attitude_from_msg(p, data, attitude, "Roll", "Pitch", "Yaw");
break;
}
}
}
void LogReader::update_copter(uint8_t type, uint8_t *data, uint16_t length)
{
MsgParser *p = msgparser[type];
switch (type) {
case LOG_COPTER_COMPASS_MSG: {
wait_timestamp_from_msg(p, data);
update_from_msg_compass(0, p, data);
break;
}
case LOG_COPTER_ATTITUDE_MSG: {
wait_timestamp_from_msg(p, data);
attitude_from_msg(p, data, attitude, "Roll", "Pitch", "Yaw");
break;
}
case LOG_COPTER_NAV_TUNING_MSG: {
inavpos = Vector3f(require_field_float(p, data, "PosX") * 0.01f,
require_field_float(p, data, "PosY") * 0.01f,
0);
break;
}
}
}
bool LogReader::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;
}
template <typename R>
void LogReader::require_field(class MsgParser *p, uint8_t *msg, const char *label, R &ret)
{
if (! p->field_value(msg, label, ret)) {
char all_labels[256];
p->string_for_labels(all_labels, 256);
::printf("Field (%s) not found; options are (%s)\n", label, all_labels);
exit(1);
}
}
void LogReader::require_field(MsgParser *p, uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen)
{
if (! p->field_value(msg, label, buffer, bufferlen)) {
char all_labels[256];
p->string_for_labels(all_labels, 256);
::printf("Field (%s) not found; options are (%s)\n", label, all_labels);
exit(1);
}
}
// start convenience functions for getting values for fields. Also,
// these are required in some places as otherwise we attempt to take
// references to bit-packed data structures...
uint8_t LogReader::require_field_uint8_t(class MsgParser *p, uint8_t *msg, const char *label)
{
uint8_t ret;
require_field(p, msg, label, ret);
return ret;
}
uint16_t LogReader::require_field_uint16_t(class MsgParser *p, uint8_t *msg, const char *label)
{
uint16_t ret;
require_field(p, msg, label, ret);
return ret;
}
int16_t LogReader::require_field_int16_t(class MsgParser *p, uint8_t *msg, const char *label)
{
int16_t ret;
require_field(p, msg, label, ret);
return ret;
}
int32_t LogReader::require_field_int32_t(class MsgParser *p, uint8_t *msg, const char *label)
{
int32_t ret;
require_field(p, msg, label, ret);
return ret;
}
float LogReader::require_field_float(class MsgParser *p, uint8_t *msg, const char *label)
{
float ret;
require_field(p, msg, label, ret);
return ret;
}
void LogReader::wait_timestamp_from_msg(class MsgParser *p, uint8_t *msg)
{
uint32_t timestamp;
require_field(p, msg, "TimeMS", timestamp);
wait_timestamp(timestamp);
}
void LogReader::update_from_msg_imu(uint8_t imu_offset, class MsgParser *p, uint8_t *msg)
{
wait_timestamp_from_msg(p, msg);
uint8_t this_imu_mask = 1 << imu_offset;
if (gyro_mask & this_imu_mask) {
Vector3f gyro;
require_field(p, msg, "Gyr", gyro);
ins.set_gyro(imu_offset, gyro);
}
if (accel_mask & this_imu_mask) {
Vector3f accel2;
require_field(p, msg, "Acc", accel2);
ins.set_accel(imu_offset, accel2);
}
dataflash.Log_Write_IMU(ins);
}
void LogReader::location_from_msg(class MsgParser *p, uint8_t *msg,
Location &loc,
const char *label_lat,
const char *label_long,
const char *label_alt)
{
loc.lat = require_field_int32_t(p, msg, label_lat);
loc.lng = require_field_int32_t(p, msg, label_long);
loc.alt = require_field_int32_t(p, msg, label_alt);
loc.options = 0;
}
void LogReader::ground_vel_from_msg(class MsgParser *p, uint8_t *msg,
Vector3f &vel,
const char *label_speed,
const char *label_course,
const char *label_vz)
{
uint32_t ground_speed;
int32_t ground_course;
require_field(p, msg, label_speed, ground_speed);
require_field(p, msg, label_course, ground_course);
vel[0] = ground_speed*0.01f*cosf(radians(ground_course*0.01f));
vel[1] = ground_speed*0.01f*sinf(radians(ground_course*0.01f));
vel[2] = require_field_float(p, msg, label_vz);
}
void LogReader::attitude_from_msg(class MsgParser *p, uint8_t *msg,
Vector3f &att,
const char *label_roll,
const char *label_pitch,
const char *label_yaw)
{
att[0] = require_field_int16_t(p, msg, label_roll) * 0.01f;
att[1] = require_field_int16_t(p, msg, label_pitch) * 0.01f;
att[2] = require_field_uint16_t(p, msg, label_yaw) * 0.01f;
}
void LogReader::update_from_msg_gps(uint8_t gps_offset, class MsgParser *p, uint8_t *msg, bool responsible_for_relalt)
{
uint32_t timestamp;
require_field(p, msg, "T", timestamp);
wait_timestamp(timestamp);
Location loc;
location_from_msg(p, msg, loc, "Lat", "Lng", "Alt");
Vector3f vel;
ground_vel_from_msg(p, msg, vel, "Spd", "GCrs", "VZ");
uint8_t status = require_field_uint8_t(p, msg, "Status");
gps.setHIL(gps_offset,
(AP_GPS::GPS_Status)status,
timestamp,
loc,
vel,
require_field_uint8_t(p, msg, "NSats"),
require_field_uint8_t(p, msg, "HDop"),
require_field_float(p, msg, "VZ") != 0);
if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
ground_alt_cm = require_field_int32_t(p, msg, "Alt");
}
if (responsible_for_relalt) {
// this could possibly check for the presence of "RelAlt" label?
rel_altitude = 0.01f * require_field_int32_t(p, msg, "RelAlt");
}
dataflash.Log_Write_GPS(gps, gps_offset, rel_altitude);
}
void LogReader::update_from_msg_compass(uint8_t compass_offset, class MsgParser *p, uint8_t *msg)
{
wait_timestamp_from_msg(p, msg);
Vector3f mag;
require_field(p, msg, "Mag", mag);
Vector3f mag_offset;
require_field(p, msg, "Ofs", mag_offset);
compass.setHIL(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.Log_Write_Compass(compass);
}
MsgHandler_PARM *parameter_handler;
bool LogReader::update(uint8_t &type)
{
@ -629,9 +114,77 @@ bool LogReader::update(uint8_t &type)
memcpy(&formats[f.type], &f, sizeof(formats[f.type]));
type = f.type;
msgparser[f.type] = new MsgParser(f);
char name[5];
memset(name, '\0', 5);
memcpy(name, f.name, 4);
::printf("Defining log format for type (%d) (%s)\n", f.type, name);
// ::printf("Defining log format for type (%d)\n", f.type);
// map from format name to a parser subclass:
if (streq(name, "PARM")) {
parameter_handler = new MsgHandler_PARM(formats[f.type], dataflash,
last_timestamp_usec);
msgparser[f.type] = parameter_handler;
} else if (streq(name, "GPS")) {
msgparser[f.type] = new MsgHandler_GPS(formats[f.type],
dataflash,
last_timestamp_usec,
gps, ground_alt_cm,
rel_altitude);
} else if (streq(name, "GPS2")) {
msgparser[f.type] = new MsgHandler_GPS2(formats[f.type], dataflash,
last_timestamp_usec,
gps, ground_alt_cm,
rel_altitude);
} else if (streq(name, "MSG")) {
msgparser[f.type] = new MsgHandler_MSG(formats[f.type], dataflash,
last_timestamp_usec,
vehicle, ahrs);
} else if (streq(name, "IMU")) {
msgparser[f.type] = new MsgHandler_IMU(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMU2")) {
msgparser[f.type] = new MsgHandler_IMU2(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "IMU3")) {
msgparser[f.type] = new MsgHandler_IMU3(formats[f.type], dataflash,
last_timestamp_usec,
accel_mask, gyro_mask, ins);
} else if (streq(name, "SIM")) {
msgparser[f.type] = new MsgHandler_SIM(formats[f.type], dataflash,
last_timestamp_usec,
sim_attitude);
} else if (streq(name, "BARO")) {
msgparser[f.type] = new MsgHandler_BARO(formats[f.type], dataflash,
last_timestamp_usec, baro);
} else if (streq(name, "AHR2")) {
msgparser[f.type] = new MsgHandler_AHR2(formats[f.type], dataflash,
last_timestamp_usec,
ahr2_attitude);
} else if (streq(name, "ATT")) {
// this parser handles *all* attitude messages - the common one,
// and also the rover/copter/plane-specific (old) messages
msgparser[f.type] = new MsgHandler_ATT(formats[f.type], dataflash,
last_timestamp_usec,
ahr2_attitude);
} else if (streq(name, "MAG")) {
msgparser[f.type] = new MsgHandler_MAG(formats[f.type], dataflash,
last_timestamp_usec, compass);
} else if (streq(name, "NTUN")) {
// the label "NTUN" is used by rover, copter and plane -
// and they all look different! creation of a parser is
// deferred until we receive a MSG log entry telling us
// which vehicle type to use. Sucks.
memcpy(&deferred_formats[f.type], &formats[f.type],
sizeof(struct log_Format));
} else if (streq(name, "ARSP")) { // plane-specific(?!)
msgparser[f.type] = new MsgHandler_ARSP(formats[f.type], dataflash,
last_timestamp_usec,
airspeed);
} else {
::printf(" No parser for (%s)\n", name);
}
return true;
}
@ -649,127 +202,21 @@ bool LogReader::update(uint8_t &type)
return false;
}
MsgParser *p = msgparser[f.type];
switch (f.type) {
case LOG_MESSAGE_MSG: {
const uint8_t msg_text_len = 64;
char msg_text[msg_text_len];
require_field(p, msg, "Message", msg_text, msg_text_len);
if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = 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) {
vehicle = 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 = VEHICLE_ROVER;
::printf("Detected Rover\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
ahrs.set_fly_forward(true);
}
dataflash.Log_Write_Message(msg_text);
break;
}
case LOG_IMU_MSG: {
update_from_msg_imu(0, p, msg);
break;
}
case LOG_IMU2_MSG: {
update_from_msg_imu(1, p, msg);
break;
}
case LOG_IMU3_MSG: {
update_from_msg_imu(2, p, msg);
break;
}
case LOG_GPS_MSG: {
update_from_msg_gps(0,p, msg, true);
break;
}
case LOG_GPS2_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, p, msg, false);
break;
}
case LOG_SIMSTATE_MSG: {
wait_timestamp_from_msg(p, msg);
attitude_from_msg(p, msg, sim_attitude, "Roll", "Pitch", "Yaw");
break;
}
case LOG_BARO_MSG: {
wait_timestamp_from_msg(p, msg);
baro.setHIL(0,
require_field_float(p, msg, "Press"),
require_field_int16_t(p, msg, "Temp") * 0.01f);
dataflash.Log_Write_Baro(baro);
break;
}
case LOG_PARAMETER_MSG: {
const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term
char parameter_name[parameter_name_len];
require_field(p, msg, "Name", parameter_name, parameter_name_len);
set_parameter(parameter_name, require_field_float(p, msg, "Value"));
break;
}
case LOG_AHR2_MSG: {
wait_timestamp_from_msg(p, msg);
attitude_from_msg(p, msg, ahr2_attitude, "Roll", "Pitch", "Yaw");
break;
}
case LOG_ATTITUDE_MSG: {
wait_timestamp_from_msg(p, msg);
attitude_from_msg(p, msg, attitude, "Roll", "Pitch", "Yaw");
break;
}
case LOG_COMPASS_MSG: {
update_from_msg_compass(0, p, msg);
break;
}
default:
if (vehicle == VEHICLE_PLANE) {
update_plane(f.type, msg, f.length);
} else if (vehicle == VEHICLE_COPTER) {
update_copter(f.type, msg, f.length);
} else if (vehicle == VEHICLE_ROVER) {
update_rover(f.type, msg, f.length);
}
break;
}
type = f.type;
return true;
}
MsgHandler *p = msgparser[type];
if (p == NULL) {
// I guess this wasn't as self-describing as it could have been....
// ::printf("No format message received for type %d; ignoring message\n",
// type);
return true;
}
void LogReader::wait_timestamp(uint32_t timestamp)
{
uint64_t timestamp_usec = timestamp*1000UL;
timestamp_usec = ((timestamp_usec + 1000) / 2500) * 2500;
last_timestamp_usec = timestamp_usec;
hal.scheduler->stop_clock(timestamp_usec);
p->process_message(msg);
maybe_install_vehicle_specific_parsers();
return true;
}
bool LogReader::wait_type(uint8_t wtype)
@ -786,3 +233,12 @@ bool LogReader::wait_type(uint8_t wtype)
return true;
}
bool LogReader::set_parameter(const char *name, float value)
{
if (parameter_handler == NULL) {
::printf("No parameter format message found");
return false;
}
return parameter_handler->set_parameter(name, value);
}

62
Tools/Replay/LogReader.h
View File

@ -1,4 +1,6 @@
#include <VehicleType.h>
// we don't use these. but Replay.pde currently does
enum log_messages {
// plane specific messages
LOG_PLANE_ATTITUDE_MSG = 9,
@ -17,7 +19,7 @@ enum log_messages {
};
class LogReader
class LogReader
{
public:
LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash);
@ -31,9 +33,7 @@ public:
const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
const float &get_relalt(void) const { return rel_altitude; }
enum vehicle_type { VEHICLE_UNKNOWN, VEHICLE_COPTER, VEHICLE_PLANE, VEHICLE_ROVER };
vehicle_type vehicle;
VehicleType::vehicle_type vehicle;
bool set_parameter(const char *name, float value);
@ -52,10 +52,8 @@ private:
AP_Airspeed &airspeed;
DataFlash_Class &dataflash;
void update_from_msg_imu(uint8_t imu_offset, class MsgParser *p, uint8_t *data);
void update_from_msg_gps(uint8_t imu_offset, class MsgParser *p, uint8_t *data, bool responsible_for_relalt);
void update_from_msg_compass(uint8_t gps_offset, class MsgParser *p, uint8_t *msg);
void wait_timestamp_from_msg(class MsgParser *p, uint8_t *data);
void update_from_msg_compass(uint8_t gps_offset, class MsgHandler *p, uint8_t *msg);
void wait_timestamp_from_msg(class MsgHandler *p, uint8_t *data);
uint8_t accel_mask;
uint8_t gyro_mask;
@ -64,48 +62,31 @@ private:
#define LOGREADER_MAX_FORMATS 255 // must be >= highest MESSAGE
struct log_Format formats[LOGREADER_MAX_FORMATS];
class MsgParser *msgparser[LOGREADER_MAX_FORMATS];
void force_add_format(struct log_Format *formats, MsgParser **parsers,
uint8_t type, const char *format, const char *labels);
// support for the old PLANE formats
void init_old_parsers_plane();
#define LOGREADER_MAX_FORMATS_PLANE 20
struct log_Format formats_plane[LOGREADER_MAX_FORMATS_PLANE];
class MsgParser *msgparser_plane[LOGREADER_MAX_FORMATS_PLANE];
// support for the old ROVER formats
void init_old_parsers_rover();
#define LOGREADER_MAX_FORMATS_ROVER 20
struct log_Format formats_rover[LOGREADER_MAX_FORMATS_ROVER];
class MsgParser *msgparser_rover[LOGREADER_MAX_FORMATS_ROVER];
// support for the old COPTER formats
void init_old_parsers_copter();
#define LOGREADER_MAX_FORMATS_COPTER 20
struct log_Format formats_copter[LOGREADER_MAX_FORMATS_COPTER];
class MsgParser *msgparser_copter[LOGREADER_MAX_FORMATS_COPTER];
class MsgHandler *msgparser[LOGREADER_MAX_FORMATS];
template <typename R>
void require_field(class MsgParser *p, uint8_t *msg, const char *label, R &ret);
void require_field(class MsgParser *p, uint8_t *data, const char *label, char *buffer, uint8_t bufferlen);
void require_field(class MsgHandler *p, uint8_t *msg, const char *label, R &ret);
void require_field(class MsgHandler *p, uint8_t *data, const char *label, char *buffer, uint8_t bufferlen);
// convenience wrappers around require_field
uint16_t require_field_uint16_t(class MsgParser *p, uint8_t *data, const char *label);
int16_t require_field_int16_t(class MsgParser *p, uint8_t *data, const char *label);
uint8_t require_field_uint8_t(class MsgParser *p, uint8_t *data, const char *label);
int32_t require_field_int32_t(class MsgParser *p, uint8_t *data, const char *label);
float require_field_float(class MsgParser *p, uint8_t *data, const char *label);
void location_from_msg(class MsgParser *p, uint8_t *data,
uint16_t require_field_uint16_t(class MsgHandler *p, uint8_t *data, const char *label);
int16_t require_field_int16_t(class MsgHandler *p, uint8_t *data, const char *label);
uint8_t require_field_uint8_t(class MsgHandler *p, uint8_t *data, const char *label);
int32_t require_field_int32_t(class MsgHandler *p, uint8_t *data, const char *label);
float require_field_float(class MsgHandler *p, uint8_t *data, const char *label);
uint8_t require_field_uint8_t(uint8_t *msg, const char *label);
void location_from_msg(class MsgHandler *p, uint8_t *data,
Location &loc,
const char *label_lat,
const char *label_long,
const char *label_alt);
void ground_vel_from_msg(class MsgParser *p, uint8_t *data,
void ground_vel_from_msg(class MsgHandler *p, uint8_t *data,
Vector3f &vel,
const char *label_speed,
const char *label_course,
const char *label_vz);
void attitude_from_msg(class MsgParser *p, uint8_t *data,
void attitude_from_msg(class MsgHandler *p, uint8_t *data,
Vector3f &att,
const char *label_roll,
const char *label_pitch,
@ -121,7 +102,8 @@ private:
void wait_timestamp(uint32_t timestamp);
void update_plane(uint8_t type, uint8_t *data, uint16_t length);
void update_copter(uint8_t type, uint8_t *data, uint16_t length);
void update_rover(uint8_t type, uint8_t *data, uint16_t length);
bool installed_vehicle_specific_parsers;
void maybe_install_vehicle_specific_parsers();
};

290
Tools/Replay/MsgHandler.cpp Normal file
View File

@ -0,0 +1,290 @@
#include <MsgHandler.h>
extern const AP_HAL::HAL& hal;
void fatal(const char *msg) {
::printf("%s",msg);
::printf("\n");
exit(1);
}
char *xstrdup(const char *string)
{
char *ret = strdup(string);
if (ret == NULL) {
perror("strdup");
fatal("strdup failed");
}
return ret;
}
void MsgHandler::add_field_type(char type, size_t size)
{
size_for_type_table[(type > 'A' ? (type-'A') : (type-'a'))] = size;
}
uint8_t MsgHandler::size_for_type(char type)
{
return size_for_type_table[(uint8_t)(type > 'A' ? (type-'A') : (type-'a'))];
}
void MsgHandler::init_field_types()
{
add_field_type('b', sizeof(int8_t));
add_field_type('c', sizeof(int16_t));
add_field_type('e', sizeof(int32_t));
add_field_type('f', sizeof(float));
add_field_type('h', sizeof(int16_t));
add_field_type('i', sizeof(int32_t));
add_field_type('n', sizeof(char[4]));
add_field_type('B', sizeof(uint8_t));
add_field_type('C', sizeof(uint16_t));
add_field_type('E', sizeof(uint32_t));
add_field_type('H', sizeof(uint16_t));
add_field_type('I', sizeof(uint32_t));
add_field_type('L', sizeof(int32_t));
add_field_type('M', sizeof(uint8_t));
add_field_type('N', sizeof(char[16]));
add_field_type('Z', sizeof(char[64]));
}
struct MsgHandler::format_field_info *MsgHandler::find_field_info(const char *label)
{
for(uint8_t i=0; i<next_field; i++) {
if (streq(field_info[i].label, label)) {
return &field_info[i];
}
}
return NULL;
}
MsgHandler::MsgHandler(struct log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: next_field(0), f(_f), dataflash(_dataflash), last_timestamp_usec(_last_timestamp_usec)
{
init_field_types();
parse_format_fields();
}
void MsgHandler::add_field(const char *_label, uint8_t _type, uint8_t _offset,
uint8_t _length)
{
field_info[next_field].label = xstrdup(_label);
field_info[next_field].type = _type;
field_info[next_field].offset = _offset;
field_info[next_field].length = _length;
next_field++;
}
void MsgHandler::parse_format_fields()
{
char *labels = xstrdup(f.labels);
char * arg = labels;
uint8_t label_offset = 0;
char *next_label;
uint8_t msg_offset = 3; // 3 bytes for the header
while ((next_label = strtok(arg, ",")) != NULL) {
if (label_offset > strlen(f.format)) {
free(labels);
printf("too few field times for labels %s (format=%s) (labels=%s)\n",
f.name, f.format, f.labels);
exit(1);
}
uint8_t field_type = f.format[label_offset];
uint8_t length = size_for_type(field_type);
add_field(next_label, field_type, msg_offset, length);
arg = NULL;
msg_offset += length;
label_offset++;
}
if (label_offset != strlen(f.format)) {
free(labels);
printf("too few labels for format (format=%s) (labels=%s)\n",
f.format, f.labels);
}
free(labels);
}
bool MsgHandler::field_value(uint8_t *msg, const char *label, char *ret, uint8_t retlen)
{
struct format_field_info *info = find_field_info(label);
if (info == NULL) {
::printf("No info for (%s)\n",label);
exit(1);
}
uint8_t offset = info->offset;
if (offset == 0) {
return false;
}
memset(ret, '\0', retlen);
memcpy(ret, &msg[offset], (retlen < info->length) ? retlen : info->length);
return true;
}
bool MsgHandler::field_value(uint8_t *msg, const char *label, Vector3f &ret)
{
const char *axes = "XYZ";
uint8_t i;
for(i=0; i<next_field; i++) {
if (!strncmp(field_info[i].label, label, strlen(label)) &&
strlen(field_info[i].label) == strlen(label)+1) {
for (uint8_t j=0; j<3; j++) {
if (field_info[i].label[strlen(label)] == axes[j]) {
field_value_for_type_at_offset(msg,
field_info[i].type,
field_info[i].offset,
ret[j]);
break; // break from finding-label loop
}
}
}
if (i == next_field) {
return 0; // not found
}
}
return true;
}
void MsgHandler::string_for_labels(char *buffer, uint bufferlen)
{
memset(buffer, '\0', bufferlen);
bufferlen--;
char *pos = buffer;
for (uint8_t k=0; k<LOGREADER_MAX_FIELDS; k++) {
if (field_info[k].label != NULL) {
uint8_t remaining = bufferlen - (pos - buffer);
uint8_t label_length = strlen(field_info[k].label);
uint8_t required = label_length;
if (pos != buffer) { // room for a comma
required++;
}
if (required+1 > remaining) { // null termination
break;
}
if (pos != buffer) {
*pos++ = ',';
}
memcpy(pos, field_info[k].label, label_length);
pos += label_length;
}
}
}
MsgHandler::~MsgHandler()
{
for (uint8_t k=0; k<LOGREADER_MAX_FIELDS; k++) {
if (field_info[k].label != NULL) {
free(field_info[k].label);
}
}
}
extern uint64_t last_timestamp_usec; // fixme!
void MsgHandler::wait_timestamp(uint32_t timestamp)
{
uint64_t timestamp_usec = timestamp*1000UL;
timestamp_usec = ((timestamp_usec + 1000) / 2500) * 2500;
last_timestamp_usec = timestamp_usec;
hal.scheduler->stop_clock(timestamp_usec);
}
void MsgHandler::location_from_msg(uint8_t *msg,
Location &loc,
const char *label_lat,
const char *label_long,
const char *label_alt)
{
loc.lat = require_field_int32_t(msg, label_lat);
loc.lng = require_field_int32_t(msg, label_long);
loc.alt = require_field_int32_t(msg, label_alt);
loc.options = 0;
}
void MsgHandler::ground_vel_from_msg(uint8_t *msg,
Vector3f &vel,
const char *label_speed,
const char *label_course,
const char *label_vz)
{
uint32_t ground_speed;
int32_t ground_course;
require_field(msg, label_speed, ground_speed);
require_field(msg, label_course, ground_course);
vel[0] = ground_speed*0.01f*cosf(radians(ground_course*0.01f));
vel[1] = ground_speed*0.01f*sinf(radians(ground_course*0.01f));
vel[2] = require_field_float(msg, label_vz);
}
void MsgHandler::attitude_from_msg(uint8_t *msg,
Vector3f &att,
const char *label_roll,
const char *label_pitch,
const char *label_yaw)
{
att[0] = require_field_int16_t(msg, label_roll) * 0.01f;
att[1] = require_field_int16_t(msg, label_pitch) * 0.01f;
att[2] = require_field_uint16_t(msg, label_yaw) * 0.01f;
}
void MsgHandler::require_field(uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen)
{
if (! field_value(msg, label, buffer, bufferlen)) {
char all_labels[256];
string_for_labels(all_labels, 256);
::printf("Field (%s) not found; options are (%s)\n", label, all_labels);
exit(1);
}
}
float MsgHandler::require_field_float(uint8_t *msg, const char *label)
{
float ret;
require_field(msg, label, ret);
return ret;
}
uint8_t MsgHandler::require_field_uint8_t(uint8_t *msg, const char *label)
{
uint8_t ret;
require_field(msg, label, ret);
return ret;
}
int32_t MsgHandler::require_field_int32_t(uint8_t *msg, const char *label)
{
int32_t ret;
require_field(msg, label, ret);
return ret;
}
uint16_t MsgHandler::require_field_uint16_t(uint8_t *msg, const char *label)
{
uint16_t ret;
require_field(msg, label, ret);
return ret;
}
int16_t MsgHandler::require_field_int16_t(uint8_t *msg, const char *label)
{
int16_t ret;
require_field(msg, label, ret);
return ret;
}
void MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
{
uint32_t timestamp;
require_field(msg, "TimeMS", timestamp);
wait_timestamp(timestamp);
}

157
Tools/Replay/MsgHandler.h Normal file
View File

@ -0,0 +1,157 @@
#ifndef AP_MSGHANDLER_H
#define AP_MSGHANDLER_H
#include <DataFlash.h>
#include <stdio.h>
#define LOGREADER_MAX_FIELDS 30
#define streq(x, y) (!strcmp(x, y))
class MsgHandler {
public:
// constructor - create a parser for a MavLink message format
MsgHandler(struct log_Format &f, DataFlash_Class &_dataflash,
uint64_t &last_timestamp_usec);
// retrieve a comma-separated list of all labels
void string_for_labels(char *buffer, uint bufferlen);
virtual void process_message(uint8_t *msg) = 0;
// field_value - retrieve the value of a field from the supplied message
// these return false if the field was not found
template<typename R>
bool field_value(uint8_t *msg, const char *label, R &ret);
bool field_value(uint8_t *msg, const char *label, Vector3f &ret);
bool field_value(uint8_t *msg, const char *label,
char *buffer, uint8_t bufferlen);
template <typename R>
void require_field(uint8_t *msg, const char *label, R &ret)
{
if (! field_value(msg, label, ret)) {
char all_labels[256];
string_for_labels(all_labels, 256);
::printf("Field (%s) not found; options are (%s)\n", label, all_labels);
exit(1);
}
}
void require_field(uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen);
float require_field_float(uint8_t *msg, const char *label);
uint8_t require_field_uint8_t(uint8_t *msg, const char *label);
int32_t require_field_int32_t(uint8_t *msg, const char *label);
uint16_t require_field_uint16_t(uint8_t *msg, const char *label);
int16_t require_field_int16_t(uint8_t *msg, const char *label);
bool set_parameter(const char *name, float value);
private:
void add_field(const char *_label, uint8_t _type, uint8_t _offset,
uint8_t length);
template<typename R>
void field_value_for_type_at_offset(uint8_t *msg, uint8_t type,
uint8_t offset, R &ret);
struct format_field_info { // parsed field information
char *label;
uint8_t type;
uint8_t offset;
uint8_t length;
};
struct format_field_info field_info[LOGREADER_MAX_FIELDS];
uint8_t next_field;
size_t size_for_type_table[52]; // maps field type (e.g. 'f') to e.g 4 bytes
struct format_field_info *find_field_info(const char *label);
void parse_format_fields();
void init_field_types();
void add_field_type(char type, size_t size);
uint8_t size_for_type(char type);
protected:
struct log_Format f; // the format we are a parser for
~MsgHandler();
void wait_timestamp(uint32_t timestamp);
uint64_t &last_timestamp_usec;
void location_from_msg(uint8_t *msg, Location &loc, const char *label_lat,
const char *label_long, const char *label_alt);
void ground_vel_from_msg(uint8_t *msg,
Vector3f &vel,
const char *label_speed,
const char *label_course,
const char *label_vz);
DataFlash_Class &dataflash;
void wait_timestamp_from_msg(uint8_t *msg);
void attitude_from_msg(uint8_t *msg,
Vector3f &att,
const char *label_roll,
const char *label_pitch,
const char *label_yaw);
};
template<typename R>
bool MsgHandler::field_value(uint8_t *msg, const char *label, R &ret)
{
struct format_field_info *info = find_field_info(label);
uint8_t offset = info->offset;
if (offset == 0) {
return false;
}
field_value_for_type_at_offset(msg, info->type, offset, ret);
return true;
}
template<typename R>
inline void MsgHandler::field_value_for_type_at_offset(uint8_t *msg,
uint8_t type,
uint8_t offset,
R &ret)
{
/* we register the types - add_field_type - so can we do without
* this switch statement somehow? */
switch (type) {
case 'B':
ret = (R)(((uint8_t*)&msg[offset])[0]);
break;
case 'c':
case 'h':
ret = (R)(((int16_t*)&msg[offset])[0]);
break;
case 'H':
ret = (R)(((uint16_t*)&msg[offset])[0]);
break;
case 'C':
ret = (R)(((uint16_t*)&msg[offset])[0]);
break;
case 'f':
ret = (R)(((float*)&msg[offset])[0]);
break;
case 'I':
case 'E':
ret = (R)(((uint32_t*)&msg[offset])[0]);
break;
case 'L':
case 'e':
ret = (R)(((int32_t*)&msg[offset])[0]);
break;
default:
::printf("Unhandled format type (%c)\n", type);
exit(1);
}
}
#endif

7
Tools/Replay/MsgHandler_AHR2.cpp Normal file
View File

@ -0,0 +1,7 @@
#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 Normal file
View File

@ -0,0 +1,15 @@
#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;
};

12
Tools/Replay/MsgHandler_ARSP.cpp Normal file
View File

@ -0,0 +1,12 @@
#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.Log_Write_Airspeed(airspeed);
}

14
Tools/Replay/MsgHandler_ARSP.h Normal file
View File

@ -0,0 +1,14 @@
#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 Normal file
View File

@ -0,0 +1,7 @@
#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 Normal file
View File

@ -0,0 +1,14 @@
#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 Normal file
View File

@ -0,0 +1,10 @@
#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.Log_Write_Baro(baro);
}

14
Tools/Replay/MsgHandler_BARO.h Normal file
View File

@ -0,0 +1,14 @@
#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;
};

6
Tools/Replay/MsgHandler_GPS.cpp Normal file
View File

@ -0,0 +1,6 @@
#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 Normal file
View File

@ -0,0 +1,19 @@
#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 Normal file
View File

@ -0,0 +1,10 @@
#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 Normal file
View File

@ -0,0 +1,22 @@
#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 Normal file
View File

@ -0,0 +1,34 @@
#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.Log_Write_GPS(gps, gps_offset, rel_altitude);
}

27
Tools/Replay/MsgHandler_GPS_Base.h Normal file
View File

@ -0,0 +1,27 @@
#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 Normal file
View File

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

14
Tools/Replay/MsgHandler_IMU.h Normal file
View File

@ -0,0 +1,14 @@
#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 Normal file
View File

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

14
Tools/Replay/MsgHandler_IMU2.h Normal file
View File

@ -0,0 +1,14 @@
#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 Normal file
View File

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

14
Tools/Replay/MsgHandler_IMU3.h Normal file
View File

@ -0,0 +1,14 @@
#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 Normal file
View File

@ -0,0 +1,21 @@
#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.Log_Write_IMU(ins);
}

26
Tools/Replay/MsgHandler_IMU_Base.h Normal file
View File

@ -0,0 +1,26 @@
#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 Normal file
View File

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

11
Tools/Replay/MsgHandler_MAG.h Normal file
View File

@ -0,0 +1,11 @@
#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);
};

19
Tools/Replay/MsgHandler_MAG_Base.cpp Normal file
View File

@ -0,0 +1,19 @@
#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(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.Log_Write_Compass(compass);
}

15
Tools/Replay/MsgHandler_MAG_Base.h Normal file
View File

@ -0,0 +1,15 @@
#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;
};

29
Tools/Replay/MsgHandler_MSG.cpp Normal file
View File

@ -0,0 +1,29 @@
#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"))) {
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 Normal file
View File

@ -0,0 +1,19 @@
#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 Normal file
View File

@ -0,0 +1,8 @@
#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 Normal file
View File

@ -0,0 +1,14 @@
#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 Normal file
View File

@ -0,0 +1,44 @@
#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 Normal file
View File

@ -0,0 +1,9 @@
#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 Normal file
View File

@ -0,0 +1,7 @@
#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 Normal file
View File

@ -0,0 +1,17 @@
#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;
};

15
Tools/Replay/Replay.pde
View File

@ -55,6 +55,7 @@
#include <getopt.h>
#include <errno.h>
#include <fenv.h>
#include <VehicleType.h>
#ifndef INT16_MIN
#define INT16_MIN -32768
@ -311,11 +312,11 @@ static void read_sensors(uint8_t type)
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
ahrs.estimate_wind();
}
} else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
(type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER) ||
(type == LOG_ROVER_COMPASS_MSG && LogReader.vehicle == LogReader::VEHICLE_ROVER)) {
} else if ((type == LOG_PLANE_COMPASS_MSG && LogReader.vehicle == VehicleType::VEHICLE_PLANE) ||
(type == LOG_COPTER_COMPASS_MSG && LogReader.vehicle == VehicleType::VEHICLE_COPTER) ||
(type == LOG_ROVER_COMPASS_MSG && LogReader.vehicle == VehicleType::VEHICLE_ROVER)) {
compass.read();
} else if (type == LOG_PLANE_AIRSPEED_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) {
} else if (type == LOG_PLANE_AIRSPEED_MSG && LogReader.vehicle == VehicleType::VEHICLE_PLANE) {
ahrs.set_airspeed(&airspeed);
} else if (type == LOG_BARO_MSG) {
barometer.update();
@ -380,9 +381,9 @@ void loop()
read_sensors(type);
if ((type == LOG_ATTITUDE_MSG) ||
(type == LOG_PLANE_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
(type == LOG_COPTER_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER) ||
(type == LOG_ROVER_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_ROVER)) {
(type == LOG_PLANE_ATTITUDE_MSG && LogReader.vehicle == VehicleType::VEHICLE_PLANE) ||
(type == LOG_COPTER_ATTITUDE_MSG && LogReader.vehicle == VehicleType::VEHICLE_COPTER) ||
(type == LOG_ROVER_ATTITUDE_MSG && LogReader.vehicle == VehicleType::VEHICLE_ROVER)) {
Vector3f ekf_euler;
Vector3f velNED;

14
Tools/Replay/VehicleType.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef AP_VEHICLETYPE_H
#define AP_VEHICLETYPE_H
class VehicleType {
public:
enum vehicle_type {
VEHICLE_UNKNOWN,
VEHICLE_COPTER,
VEHICLE_PLANE,
VEHICLE_ROVER
};
};
#endif