From 0d6766d14d6f9ebeb62e81eb7c69e17fbf1f8b4d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Oct 2022 11:15:18 -0400 Subject: [PATCH] limit vehicle_command subscription updates per cycle - this is a precaution to eliminate the possibility of getting stuck in a loop trying to keep up with a high rate publication that could be coming from a higher priority task --- src/modules/mavlink/mavlink_main.cpp | 7 +++++-- src/modules/mavlink/streams/COMMAND_LONG.hpp | 6 +++++- src/modules/navigator/navigator_main.cpp | 5 ++++- .../TemperatureCompensationModule.cpp | 5 ++++- 4 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed54d899ad..83d51437f7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2313,9 +2313,12 @@ Mavlink::task_main(int argc, char *argv[]) } - // vehicle_command + // MAVLINK_MODE_IRIDIUM: handle VEHICLE_CMD_CONTROL_HIGH_LATENCY if (_mode == MAVLINK_MODE_IRIDIUM) { - while (_vehicle_command_sub.updated()) { + int vehicle_command_updates = 0; + + while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { + vehicle_command_updates++; const unsigned last_generation = _vehicle_command_sub.get_last_generation(); vehicle_command_s vehicle_cmd; diff --git a/src/modules/mavlink/streams/COMMAND_LONG.hpp b/src/modules/mavlink/streams/COMMAND_LONG.hpp index e4bc46688b..89f4a77dbe 100644 --- a/src/modules/mavlink/streams/COMMAND_LONG.hpp +++ b/src/modules/mavlink/streams/COMMAND_LONG.hpp @@ -62,8 +62,12 @@ private: bool sent = false; static constexpr size_t COMMAND_LONG_SIZE = MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + int vehicle_command_updates = 0; - while ((_mavlink->get_free_tx_buf() >= COMMAND_LONG_SIZE) && _vehicle_command_sub.updated()) { + while ((_mavlink->get_free_tx_buf() >= COMMAND_LONG_SIZE) + && _vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { + + vehicle_command_updates++; const unsigned last_generation = _vehicle_command_sub.get_last_generation(); vehicle_command_s cmd; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 37bdb83ac6..2d2cadf77e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -223,7 +223,10 @@ void Navigator::run() _home_pos_sub.update(&_home_pos); // Handle Vehicle commands - while (_vehicle_command_sub.updated()) { + int vehicle_command_updates = 0; + + while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { + vehicle_command_updates++; const unsigned last_generation = _vehicle_command_sub.get_last_generation(); vehicle_command_s cmd{}; diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index f11c2bba1f..ea34d654b6 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -194,7 +194,10 @@ void TemperatureCompensationModule::Run() perf_begin(_loop_perf); // Check if user has requested to run the calibration routine - while (_vehicle_command_sub.updated()) { + int vehicle_command_updates = 0; + + while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { + vehicle_command_updates++; vehicle_command_s cmd; if (_vehicle_command_sub.copy(&cmd)) {