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
This commit is contained in:
Daniel Agar 2022-10-17 11:15:18 -04:00
parent 677f3e9294
commit 0d6766d14d
4 changed files with 18 additions and 5 deletions

View File

@ -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;

View File

@ -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;

View File

@ -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{};

View File

@ -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)) {