|
|
|
|
@ -16,12 +16,28 @@
@@ -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
@@ -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)
@@ -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]; |
|
|
|
|
|
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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"); |
|
|
|
|
struct log_Format deferred_formats[LOGREADER_MAX_FORMATS]; |
|
|
|
|
|
|
|
|
|
// 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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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)
@@ -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); |
|
|
|
|
|
|
|
|
|
// ::printf("Defining log format for type (%d)\n", f.type);
|
|
|
|
|
char name[5]; |
|
|
|
|
memset(name, '\0', 5); |
|
|
|
|
memcpy(name, f.name, 4); |
|
|
|
|
::printf("Defining log format for type (%d) (%s)\n", f.type, name); |
|
|
|
|
|
|
|
|
|
// 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,129 +202,23 @@ bool LogReader::update(uint8_t &type)
@@ -649,129 +202,23 @@ 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; |
|
|
|
|
} |
|
|
|
|
type = f.type; |
|
|
|
|
|
|
|
|
|
case LOG_COMPASS_MSG: { |
|
|
|
|
update_from_msg_compass(0, p, msg); |
|
|
|
|
break; |
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
|
p->process_message(msg); |
|
|
|
|
|
|
|
|
|
type = f.type; |
|
|
|
|
maybe_install_vehicle_specific_parsers(); |
|
|
|
|
|
|
|
|
|
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); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool LogReader::wait_type(uint8_t wtype) |
|
|
|
|
{ |
|
|
|
|
while (true) { |
|
|
|
|
@ -786,3 +233,12 @@ 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); |
|
|
|
|
} |
|
|
|
|
|
Peter Barker
10 years ago
committed by
Andrew Tridgell