You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
601 lines
16 KiB
601 lines
16 KiB
#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])); |
|
add_field_type('q', sizeof(int64_t)); |
|
add_field_type('Q', sizeof(uint64_t)); |
|
} |
|
|
|
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); |
|
} |
|
} |
|
} |
|
|
|
void MsgHandler::wait_timestamp_usec(uint64_t timestamp) |
|
{ |
|
last_timestamp_usec = timestamp; |
|
hal.scheduler->stop_clock(timestamp); |
|
} |
|
|
|
void MsgHandler::wait_timestamp(uint32_t timestamp) |
|
{ |
|
uint64_t usecs = timestamp*1000UL; |
|
wait_timestamp_usec(usecs); |
|
} |
|
|
|
void MsgHandler::location_from_msg(uint8_t *msg, |
|
Location &loc, |
|
const char *label_lat, |
|
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::field_not_found(uint8_t *msg, const char *label) |
|
{ |
|
char all_labels[256]; |
|
uint8_t type = msg[2]; |
|
string_for_labels(all_labels, 256); |
|
::printf("Field (%s) not found for id=%d; options are (%s)\n", |
|
label, type, all_labels); |
|
abort(); |
|
} |
|
|
|
void MsgHandler::require_field(uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen) |
|
{ |
|
if (! field_value(msg, label, buffer, bufferlen)) { |
|
field_not_found(msg,label); |
|
} |
|
} |
|
|
|
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) |
|
{ |
|
uint64_t time_us; |
|
uint64_t time_ms; |
|
|
|
if (field_value(msg, "TimeUS", time_us)) { |
|
// 64-bit timestamp present - great! |
|
wait_timestamp_usec(time_us); |
|
} else if (field_value(msg, "TimeMS", time_ms)) { |
|
// there is special rounding code that needs to be crossed in |
|
// wait_timestamp: |
|
wait_timestamp(time_ms); |
|
} else { |
|
::printf("No timestamp on message"); |
|
} |
|
} |
|
|
|
|
|
|
|
/* |
|
* subclasses to handle specific messages below here |
|
*/ |
|
|
|
void 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) |
|
{ |
|
uint64_t time_us; |
|
if (! field_value(msg, "TimeUS", time_us)) { |
|
uint32_t timestamp; |
|
require_field(msg, "T", timestamp); |
|
time_us = timestamp * 1000; |
|
} |
|
wait_timestamp_usec(time_us); |
|
|
|
Location loc; |
|
location_from_msg(msg, loc, "Lat", "Lng", "Alt"); |
|
Vector3f vel; |
|
ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ"); |
|
|
|
uint8_t status = require_field_uint8_t(msg, "Status"); |
|
gps.setHIL(gps_offset, |
|
(AP_GPS::GPS_Status)status, |
|
uint32_t(time_us/1000), |
|
loc, |
|
vel, |
|
require_field_uint8_t(msg, "NSats"), |
|
require_field_uint8_t(msg, "HDop"), |
|
require_field_float(msg, "VZ") != 0); |
|
if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) { |
|
ground_alt_cm = require_field_int32_t(msg, "Alt"); |
|
} |
|
|
|
if (responsible_for_relalt) { |
|
// this could possibly check for the presence of "RelAlt" label? |
|
int32_t tmp; |
|
if (! field_value(msg, "RAlt", tmp)) { |
|
tmp = require_field_int32_t(msg, "RelAlt"); |
|
} |
|
rel_altitude = 0.01f * tmp; |
|
} |
|
|
|
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", |
|
"COMPASS_ORIENT", "COMPASS_ORIENT2", |
|
"COMPASS_ORIENT3"}; |
|
for (uint8_t i=0; i<sizeof(ignore_parms)/sizeof(ignore_parms[0]); i++) { |
|
if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) { |
|
::printf("Ignoring set of %s to %f\n", name, value); |
|
return true; |
|
} |
|
} |
|
enum ap_var_type var_type; |
|
AP_Param *vp = AP_Param::find(name, &var_type); |
|
if (vp == NULL) { |
|
return false; |
|
} |
|
if (var_type == AP_PARAM_FLOAT) { |
|
((AP_Float *)vp)->set(value); |
|
::printf("Set %s to %f\n", name, value); |
|
} else if (var_type == AP_PARAM_INT32) { |
|
((AP_Int32 *)vp)->set(value); |
|
::printf("Set %s to %d\n", name, (int)value); |
|
} else if (var_type == AP_PARAM_INT16) { |
|
((AP_Int16 *)vp)->set(value); |
|
::printf("Set %s to %d\n", name, (int)value); |
|
} else if (var_type == AP_PARAM_INT8) { |
|
((AP_Int8 *)vp)->set(value); |
|
::printf("Set %s to %d\n", name, (int)value); |
|
} else { |
|
// we don't support mavlink set on this parameter |
|
return false; |
|
} |
|
return true; |
|
} |
|
|
|
void 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"); |
|
} |
|
|
|
|