diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 3915ffb3ae..de96a684b0 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -109,6 +109,28 @@ struct PACKED log_Copter_Nav_Tuning { float desired_accel_y; }; +struct PACKED log_Rover_Attitude { + LOG_PACKET_HEADER; + uint32_t time_ms; + int16_t roll; + int16_t pitch; + uint16_t yaw; +}; + +struct PACKED log_Rover_Compass { + LOG_PACKET_HEADER; + uint32_t time_ms; + int16_t mag_x; + int16_t mag_y; + int16_t mag_z; + int16_t offset_x; + int16_t offset_y; + int16_t offset_z; + int16_t motor_offset_x; + int16_t motor_offset_y; + int16_t motor_offset_z; +}; + void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length) { switch (type) { @@ -151,6 +173,36 @@ void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length) } } +void LogReader::process_rover(uint8_t type, uint8_t *data, uint16_t length) +{ + switch (type) { + case LOG_ROVER_COMPASS_MSG: { + struct log_Rover_Compass msg; + if(sizeof(msg) != length) { + printf("Bad rover COMPASS length\n"); + exit(1); + } + memcpy(&msg, data, sizeof(msg)); + wait_timestamp(msg.time_ms); + compass.setHIL(Vector3i(msg.mag_x - msg.offset_x, msg.mag_y - msg.offset_y, msg.mag_z - msg.offset_z)); + compass.set_offsets(msg.offset_x, msg.offset_y, msg.offset_z); + break; + } + + case LOG_ROVER_ATTITUDE_MSG: { + struct log_Rover_Attitude msg; + if(sizeof(msg) != length) { + printf("Bad ATTITUDE length\n"); + exit(1); + } + memcpy(&msg, data, sizeof(msg)); + wait_timestamp(msg.time_ms); + attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f); + break; + } + } +} + void LogReader::process_copter(uint8_t type, uint8_t *data, uint16_t length) { switch (type) { @@ -272,7 +324,7 @@ bool LogReader::update(uint8_t &type) } else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) { vehicle = VEHICLE_COPTER; ::printf("Detected Copter\n"); - } else if (strncmp(msg.msg, "APMRover2", strlen("APMRover2")) == 0) { + } else if (strncmp(msg.msg, "ArduRover", strlen("ArduRover")) == 0) { vehicle = VEHICLE_ROVER; ::printf("Detected Rover\n"); } @@ -378,6 +430,8 @@ bool LogReader::update(uint8_t &type) process_plane(f.type, data, f.length); } else if (vehicle == VEHICLE_COPTER) { process_copter(f.type, data, f.length); + } else if (vehicle == VEHICLE_ROVER) { + process_rover(f.type, data, f.length); } break; } diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 7b97ce24d5..8df966ddf5 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -9,6 +9,10 @@ enum log_messages { LOG_COPTER_ATTITUDE_MSG = 1, LOG_COPTER_COMPASS_MSG = 15, LOG_COPTER_NAV_TUNING_MSG = 5, + + // rover specific messages + LOG_ROVER_ATTITUDE_MSG = 8, + LOG_ROVER_COMPASS_MSG = 10, }; @@ -59,4 +63,5 @@ private: void process_plane(uint8_t type, uint8_t *data, uint16_t length); void process_copter(uint8_t type, uint8_t *data, uint16_t length); + void process_rover(uint8_t type, uint8_t *data, uint16_t length); }; diff --git a/Tools/Replay/Replay.pde b/Tools/Replay/Replay.pde index cca4c555b8..b49ba1f714 100644 --- a/Tools/Replay/Replay.pde +++ b/Tools/Replay/Replay.pde @@ -305,7 +305,8 @@ void loop() read_sensors(type); if ((type == LOG_PLANE_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) || - (type == LOG_COPTER_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) { + (type == LOG_COPTER_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER) || + (type == LOG_ROVER_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_ROVER)) { Vector3f ekf_euler; Vector3f velNED;