From cad7f9d9c0e965e2e7cb4f984fecb7e156ec21c9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 May 2018 21:53:36 +1000 Subject: [PATCH] GCS_MAVLink: tidy waypoint receiving part of update This is NFC. The early-return here is confusing, and there were redundant checks going on. --- libraries/GCS_MAVLink/GCS_Common.cpp | 22 +++++++++------------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 78d67a9bfc..d9fbdce7b4 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -951,20 +951,16 @@ GCS_MAVLINK::update(uint32_t max_time_us) } } - if (!waypoint_receiving) { - hal.util->perf_end(_perf_update); - return; - } + if (waypoint_receiving) { + const uint32_t wp_recv_time = 1000U + (stream_slowdown*20); - uint32_t wp_recv_time = 1000U + (stream_slowdown*20); - - // stop waypoint receiving if timeout - if (waypoint_receiving && (tnow - waypoint_timelast_receive) > wp_recv_time+waypoint_receive_timeout) { - waypoint_receiving = false; - } else if (waypoint_receiving && - (tnow - waypoint_timelast_request) > wp_recv_time) { - waypoint_timelast_request = tnow; - send_message(MSG_NEXT_WAYPOINT); + // stop waypoint receiving if timeout + if (tnow - waypoint_timelast_receive > wp_recv_time+waypoint_receive_timeout) { + waypoint_receiving = false; + } else if (tnow - waypoint_timelast_request > wp_recv_time) { + waypoint_timelast_request = tnow; + send_message(MSG_NEXT_WAYPOINT); + } } hal.util->perf_end(_perf_update);