From 0eded27a9f9123c28eadf2e00bd467fe4b1b4e2c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 22 Apr 2024 08:31:01 +1000 Subject: [PATCH] AP_Periph: check return of get_RelPosHeading --- Tools/AP_Periph/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/gps.cpp b/Tools/AP_Periph/gps.cpp index f185f709a0..87b1976eb9 100644 --- a/Tools/AP_Periph/gps.cpp +++ b/Tools/AP_Periph/gps.cpp @@ -295,8 +295,8 @@ void AP_Periph_FW::send_relposheading_msg() { float relative_down_pos; float reported_heading_acc; uint32_t curr_timestamp = 0; - gps.get_RelPosHeading(curr_timestamp, reported_heading, relative_distance, relative_down_pos, reported_heading_acc); - if (last_relposheading_ms == curr_timestamp) { + if (!gps.get_RelPosHeading(curr_timestamp, reported_heading, relative_distance, relative_down_pos, reported_heading_acc) || + last_relposheading_ms == curr_timestamp) { return; } last_relposheading_ms = curr_timestamp;