Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Replay: fixed for rover logs

This commit is contained in:
Andrew Tridgell 2014-03-06 17:50:50 +11:00
parent 4cfe50b70c
commit 5dfd9b074e
3 changed files with 62 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
Tools/Replay/LogReader.cpp
View File

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

5
Tools/Replay/LogReader.h
View File

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

3
Tools/Replay/Replay.pde
View File

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