From 0571f86c17518051b708b853a5253e2c7b7c0f1a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 May 2016 10:23:10 +1000 Subject: [PATCH] Replay: handle float ground speed in GPS --- Tools/Replay/MsgHandler.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index 3dccc1f8da..36e0d3cf7f 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -213,12 +213,16 @@ void MsgHandler::ground_vel_from_msg(uint8_t *msg, const char *label_course, const char *label_vz) { - uint32_t ground_speed; + float ground_speed; int32_t ground_course; - require_field(msg, label_speed, ground_speed); + 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; + } require_field(msg, label_course, ground_course); - vel[0] = ground_speed*0.01f*cosf(radians(ground_course*0.01f)); - vel[1] = ground_speed*0.01f*sinf(radians(ground_course*0.01f)); + vel[0] = ground_speed*cosf(radians(ground_course*0.01f)); + vel[1] = ground_speed*sinf(radians(ground_course*0.01f)); vel[2] = require_field_float(msg, label_vz); }