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

Replay: cope with GPS ground couse in degrees

This commit is contained in:
Andrew Tridgell 2016-05-05 11:28:59 +10:00
parent b424c49bc7
commit 468f020b51
2 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Tools/Replay/LR_MsgHandler.cpp
View File

@ -175,8 +175,7 @@ void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *ms
loc,
vel,
nsats,
hdop,
require_field_float(msg, "VZ") != 0);
hdop);
if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
ground_alt_cm = require_field_int32_t(msg, "Alt");
}
@ -201,8 +200,12 @@ void LR_MsgHandler_GPA_Base::update_from_msg_gpa(uint8_t gps_offset, uint8_t *ms
require_field(msg, "HAcc", hacc);
require_field(msg, "VAcc", vacc);
require_field(msg, "SAcc", sacc);
uint8_t have_vertical_velocity;
if (! field_value(msg, "VV", have_vertical_velocity)) {
have_vertical_velocity = !is_zero(gps.velocity(gps_offset).z);
}
gps.setHIL_Accuracy(gps_offset, vdop*0.01f, hacc*0.01f, vacc*0.01f, sacc*0.01f);
gps.setHIL_Accuracy(gps_offset, vdop*0.01f, hacc*0.01f, vacc*0.01f, sacc*0.01f, have_vertical_velocity);
}
void LR_MsgHandler_GPA::process_message(uint8_t *msg)

12
Tools/Replay/MsgHandler.cpp
View File

@ -214,15 +214,21 @@ void MsgHandler::ground_vel_from_msg(uint8_t *msg,
const char *label_vz)
{
float ground_speed;
int32_t ground_course;
float ground_course;
// in older logs speed and course are integers
if (!field_value(msg, label_speed, ground_speed)) {
uint32_t speed_cms;
require_field(msg, label_speed, speed_cms);
ground_speed = speed_cms * 0.01f;
}
if (!field_value(msg, label_course, ground_course)) {
uint32_t course_cd;
require_field(msg, label_course, course_cd);
ground_course = course_cd * 0.01f;
}
require_field(msg, label_course, ground_course);
vel[0] = ground_speed*cosf(radians(ground_course*0.01f));
vel[1] = ground_speed*sinf(radians(ground_course*0.01f));
vel[0] = ground_speed*cosf(radians(ground_course));
vel[1] = ground_speed*sinf(radians(ground_course));
vel[2] = require_field_float(msg, label_vz);
}