From 468f020b51fd0625b9dc897ce12add2bca3c5632 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 May 2016 11:28:59 +1000 Subject: [PATCH] Replay: cope with GPS ground couse in degrees --- Tools/Replay/LR_MsgHandler.cpp | 9 ++++++--- Tools/Replay/MsgHandler.cpp | 12 +++++++++--- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index b0e2a3b45f..07b349c824 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -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) diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index 36e0d3cf7f..7059562c12 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -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); }