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

Replay: handle multiple GPS sensors

This commit is contained in:
Andrew Tridgell 2014-04-02 07:49:58 +11:00
parent 067bbf5b5c
commit ec04eba61c
1 changed files with 34 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
Tools/Replay/LogReader.cpp
View File

@ -382,11 +382,13 @@ bool LogReader::update(uint8_t &type)
Vector3f vel(msg.ground_speed*0.01f*cosf(msg.ground_course*0.01f),
msg.ground_speed*0.01f*sinf(msg.ground_course*0.01f),
msg.vel_z);
gps.setHIL((AP_GPS::GPS_Status)msg.status,
gps.setHIL(0, (AP_GPS::GPS_Status)msg.status,
msg.apm_time,
loc,
vel,
msg.num_sats);
msg.num_sats,
msg.hdop,
msg.vel_z != 0);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
@ -394,6 +396,36 @@ bool LogReader::update(uint8_t &type)
break;
}
case LOG_GPS2_MSG: {
struct log_GPS2 msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
Location loc;
loc.lat = msg.latitude;
loc.lng = msg.longitude;
loc.alt = msg.altitude;
loc.options = 0;
Vector3f vel(msg.ground_speed*0.01f*cosf(msg.ground_course*0.01f),
msg.ground_speed*0.01f*sinf(msg.ground_course*0.01f),
msg.vel_z);
gps.setHIL(1, (AP_GPS::GPS_Status)msg.status,
msg.apm_time,
loc,
vel,
msg.num_sats,
msg.hdop,
msg.vel_z != 0);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
break;
}
case LOG_SIMSTATE_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {