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::handle_msg(const struct log_Format &f, uint8_t *msg) {
char name[5];
memset(name, '\0', 5);
memcpy(name, f.name, 4);
if (!in_list(name, generated_names)) {
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::update(uint8_t &type)
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format f;
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
memcpy(&formats[f.type], &f, sizeof(formats[f.type]));
type = 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;
}
const struct log_Format &f = formats[hdr[2]];
if (f.length == 0) {
// can't just throw these away as the format specifies the
// number of bytes in the message
::printf("No format defined for type (%d)\n", hdr[2]);
exit(1);
}
uint8_t msg[f.length];
memcpy(msg, hdr, 3);
if (::read(fd, &msg[3], f.length-3) != f.length-3) {
return false;
}
type = f.type;
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;
}
p->process_message(msg);
maybe_install_vehicle_specific_parsers();
return true;
}
