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.
384 lines
14 KiB
384 lines
14 KiB
#include <AP_HAL/AP_HAL.h> |
|
#include <AP_Common/AP_Common.h> |
|
#include <AP_Math/AP_Math.h> |
|
#include <AP_Airspeed/AP_Airspeed.h> |
|
#include <AP_Compass/AP_Compass.h> |
|
#include <AP_GPS/AP_GPS.h> |
|
#include <AP_Compass/AP_Compass.h> |
|
#include <AP_Baro/AP_Baro.h> |
|
#include <AP_InertialSensor/AP_InertialSensor.h> |
|
#include <DataFlash/DataFlash.h> |
|
|
|
#include "LogReader.h" |
|
#include <stdio.h> |
|
#include <unistd.h> |
|
#include <sys/types.h> |
|
#include <sys/stat.h> |
|
|
|
#include "MsgHandler.h" |
|
#include "Replay.h" |
|
|
|
#define DEBUG 0 |
|
#if DEBUG |
|
# define debug(fmt, args...) printf(fmt "\n", ##args) |
|
#else |
|
# define debug(fmt, args...) |
|
#endif |
|
|
|
#define streq(x, y) (!strcmp(x, y)) |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
const struct LogStructure log_structure[] = { |
|
LOG_COMMON_STRUCTURES, |
|
{ LOG_CHEK_MSG, sizeof(log_Chek), |
|
"CHEK", |
|
"QccCLLffff", |
|
"TimeUS,Roll,Pitch,Yaw,Lat,Lng,Alt,VN,VE,VD", |
|
"sdddDUmnnn", |
|
"FBBBGGB000"} |
|
}; |
|
|
|
LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, |
|
AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&_nottypes): |
|
DataFlashFileReader(), |
|
vehicle(VehicleType::VEHICLE_UNKNOWN), |
|
ahrs(_ahrs), |
|
ins(_ins), |
|
baro(_baro), |
|
compass(_compass), |
|
gps(_gps), |
|
airspeed(_airspeed), |
|
dataflash(_dataflash), |
|
accel_mask(7), |
|
gyro_mask(7), |
|
last_timestamp_usec(0), |
|
installed_vehicle_specific_parsers(false), |
|
nottypes(_nottypes) |
|
{ |
|
} |
|
|
|
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 LR_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; |
|
} |
|
} |
|
|
|
/* |
|
messages which we will be generating, so should be discarded |
|
*/ |
|
static const char *generated_names[] = { "EKF1", "EKF2", "EKF3", "EKF4", "EKF5", |
|
"NKF1", "NKF2", "NKF3", "NKF4", "NKF5", |
|
"NKF6", "NKF7", "NKF8", "NKF9", "NKF10", |
|
"AHR2", "POS", "CHEK", |
|
"IMT", "IMT2", |
|
"MAG", "MAG2", |
|
"BARO", "BAR2", |
|
"GPS","GPA", |
|
"NKA", "NKV", NULL }; |
|
|
|
/* |
|
see if a type is in a list of types |
|
*/ |
|
bool LogReader::in_list(const char *type, const char *list[]) |
|
{ |
|
if (list == NULL) { |
|
return false; |
|
} |
|
for (uint8_t i=0; list[i] != NULL; i++) { |
|
if (strcmp(type, list[i]) == 0) { |
|
return true; |
|
} |
|
} |
|
return false; |
|
} |
|
|
|
/* |
|
map from an incoming format type to an outgoing format type |
|
*/ |
|
uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype) |
|
{ |
|
if (mapped_msgid[intype] != 0) { |
|
// already mapped |
|
return mapped_msgid[intype]; |
|
} |
|
// see if it is in our structure list |
|
for (uint8_t i=0; i<ARRAY_SIZE(log_structure); i++) { |
|
if (strcmp(name, log_structure[i].name) == 0) { |
|
mapped_msgid[intype] = log_structure[i].msg_type; |
|
return mapped_msgid[intype]; |
|
} |
|
} |
|
// it is a new one, allocate an ID |
|
mapped_msgid[intype] = next_msgid++; |
|
return mapped_msgid[intype]; |
|
} |
|
|
|
bool LogReader::save_message_type(const char *name) |
|
{ |
|
bool save_message = !in_list(name, generated_names); |
|
if (save_chek_messages && strcmp(name, "CHEK") == 0) { |
|
save_message = true; |
|
} |
|
return save_message; |
|
} |
|
|
|
bool LogReader::handle_log_format_msg(const struct log_Format &f) |
|
{ |
|
char name[5]; |
|
memset(name, '\0', 5); |
|
memcpy(name, f.name, 4); |
|
debug("Defining log format for type (%d) (%s)\n", f.type, name); |
|
|
|
if (save_message_type(name)) { |
|
/* |
|
any messages which we won't be generating internally in |
|
replay should get the original FMT header |
|
We need to remap the type in the FMT header to avoid |
|
conflicts with our current table |
|
*/ |
|
struct log_Format f_mapped = f; |
|
f_mapped.type = map_fmt_type(name, f.type); |
|
dataflash.WriteBlock(&f_mapped, sizeof(f_mapped)); |
|
} |
|
|
|
if (msgparser[f.type] != NULL) { |
|
return true; |
|
} |
|
|
|
// map from format name to a parser subclass: |
|
if (streq(name, "PARM")) { |
|
msgparser[f.type] = new LR_MsgHandler_PARM |
|
(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
[this](const char *xname, const float xvalue) { |
|
return set_parameter(xname, xvalue); |
|
}); |
|
} else if (streq(name, "GPS")) { |
|
msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type], |
|
dataflash, |
|
last_timestamp_usec, |
|
gps, ground_alt_cm); |
|
} else if (streq(name, "GPS2")) { |
|
msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
gps, ground_alt_cm); |
|
} else if (streq(name, "GPA")) { |
|
msgparser[f.type] = new LR_MsgHandler_GPA(formats[f.type], |
|
dataflash, |
|
last_timestamp_usec, |
|
gps); |
|
} else if (streq(name, "GPA2")) { |
|
msgparser[f.type] = new LR_MsgHandler_GPA2(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
gps); |
|
} else if (streq(name, "MSG")) { |
|
msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
vehicle, ahrs); |
|
} else if (streq(name, "IMU")) { |
|
msgparser[f.type] = new LR_MsgHandler_IMU(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
accel_mask, gyro_mask, ins); |
|
} else if (streq(name, "IMU2")) { |
|
msgparser[f.type] = new LR_MsgHandler_IMU2(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
accel_mask, gyro_mask, ins); |
|
} else if (streq(name, "IMU3")) { |
|
msgparser[f.type] = new LR_MsgHandler_IMU3(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
accel_mask, gyro_mask, ins); |
|
} else if (streq(name, "IMT")) { |
|
msgparser[f.type] = new LR_MsgHandler_IMT(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
accel_mask, gyro_mask, use_imt, ins); |
|
} else if (streq(name, "IMT2")) { |
|
msgparser[f.type] = new LR_MsgHandler_IMT2(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
accel_mask, gyro_mask, use_imt, ins); |
|
} else if (streq(name, "IMT3")) { |
|
msgparser[f.type] = new LR_MsgHandler_IMT3(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
accel_mask, gyro_mask, use_imt, ins); |
|
} else if (streq(name, "SIM")) { |
|
msgparser[f.type] = new LR_MsgHandler_SIM(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
sim_attitude); |
|
} else if (streq(name, "BARO")) { |
|
msgparser[f.type] = new LR_MsgHandler_BARO(formats[f.type], dataflash, |
|
last_timestamp_usec, baro); |
|
} else if (streq(name, "ARM")) { |
|
msgparser[f.type] = new LR_MsgHandler_ARM(formats[f.type], dataflash, |
|
last_timestamp_usec); |
|
} else if (streq(name, "EV")) { |
|
msgparser[f.type] = new LR_MsgHandler_Event(formats[f.type], dataflash, |
|
last_timestamp_usec); |
|
} else if (streq(name, "AHR2")) { |
|
msgparser[f.type] = new LR_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 LR_MsgHandler_ATT(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
attitude); |
|
} else if (streq(name, "MAG")) { |
|
msgparser[f.type] = new LR_MsgHandler_MAG(formats[f.type], dataflash, |
|
last_timestamp_usec, compass); |
|
} else if (streq(name, "MAG2")) { |
|
msgparser[f.type] = new LR_MsgHandler_MAG2(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 LR_MsgHandler_ARSP(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
airspeed); |
|
} else if (streq(name, "NKF1")) { |
|
msgparser[f.type] = new LR_MsgHandler_NKF1(formats[f.type], dataflash, |
|
last_timestamp_usec); |
|
} else if (streq(name, "CHEK")) { |
|
msgparser[f.type] = new LR_MsgHandler_CHEK(formats[f.type], dataflash, |
|
last_timestamp_usec, |
|
check_state); |
|
} else if (streq(name, "PM")) { |
|
msgparser[f.type] = new LR_MsgHandler_PM(formats[f.type], dataflash, |
|
last_timestamp_usec); |
|
} else { |
|
debug(" No parser for (%s)\n", name); |
|
} |
|
|
|
return true; |
|
} |
|
|
|
bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) { |
|
char name[5]; |
|
memset(name, '\0', 5); |
|
memcpy(name, f.name, 4); |
|
|
|
if (save_message_type(name)) { |
|
if (mapped_msgid[msg[2]] == 0) { |
|
printf("Unknown msgid %u\n", (unsigned)msg[2]); |
|
exit(1); |
|
} |
|
msg[2] = mapped_msgid[msg[2]]; |
|
if (!in_list(name, nottypes)) { |
|
dataflash.WriteBlock(msg, f.length); |
|
} |
|
// a MsgHandler would probably have found a timestamp and |
|
// caled stop_clock. This runs IO, clearing dataflash's |
|
// buffer. |
|
hal.scheduler->stop_clock(last_timestamp_usec); |
|
} |
|
|
|
LR_MsgHandler *p = msgparser[f.type]; |
|
if (p == NULL) { |
|
return true; |
|
} |
|
|
|
p->process_message(msg); |
|
|
|
maybe_install_vehicle_specific_parsers(); |
|
|
|
return true; |
|
} |
|
|
|
bool LogReader::wait_type(const char *wtype) |
|
{ |
|
while (true) { |
|
char type[5]; |
|
if (!update(type)) { |
|
return false; |
|
} |
|
if (streq(type,wtype)) { |
|
break; |
|
} |
|
} |
|
return true; |
|
} |
|
|
|
bool LogReader::set_parameter(const char *name, float value) |
|
{ |
|
enum ap_var_type var_type; |
|
AP_Param *vp = AP_Param::find(name, &var_type); |
|
if (vp == NULL) { |
|
return false; |
|
} |
|
float old_value = 0; |
|
if (var_type == AP_PARAM_FLOAT) { |
|
old_value = ((AP_Float *)vp)->cast_to_float(); |
|
((AP_Float *)vp)->set(value); |
|
} else if (var_type == AP_PARAM_INT32) { |
|
old_value = ((AP_Int32 *)vp)->cast_to_float(); |
|
((AP_Int32 *)vp)->set(value); |
|
} else if (var_type == AP_PARAM_INT16) { |
|
old_value = ((AP_Int16 *)vp)->cast_to_float(); |
|
((AP_Int16 *)vp)->set(value); |
|
} else if (var_type == AP_PARAM_INT8) { |
|
old_value = ((AP_Int8 *)vp)->cast_to_float(); |
|
((AP_Int8 *)vp)->set(value); |
|
} else { |
|
// we don't support mavlink set on this parameter |
|
return false; |
|
} |
|
if (fabsf(old_value - value) > 1.0e-12) { |
|
::printf("Changed %s to %.8f from %.8f\n", name, value, old_value); |
|
} |
|
return true; |
|
} |
|
|
|
/* |
|
called when the last FMT message has been processed |
|
*/ |
|
void LogReader::end_format_msgs(void) |
|
{ |
|
// write out any formats we will be producing |
|
for (uint8_t i=0; generated_names[i]; i++) { |
|
for (uint8_t n=0; n<ARRAY_SIZE(log_structure); n++) { |
|
if (strcmp(generated_names[i], log_structure[n].name) == 0) { |
|
const struct LogStructure *s = &log_structure[n]; |
|
struct log_Format pkt {}; |
|
pkt.head1 = HEAD_BYTE1; |
|
pkt.head2 = HEAD_BYTE2; |
|
pkt.msgid = LOG_FORMAT_MSG; |
|
pkt.type = s->msg_type; |
|
pkt.length = s->msg_len; |
|
strncpy(pkt.name, s->name, sizeof(pkt.name)); |
|
strncpy(pkt.format, s->format, sizeof(pkt.format)); |
|
strncpy(pkt.labels, s->labels, sizeof(pkt.labels)); |
|
dataflash.WriteCriticalBlock(&pkt, sizeof(pkt)); |
|
} |
|
} |
|
} |
|
}
|
|
|