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; }