From 47c291f05047abc49e2a1fe90b90d05fa4919a41 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Wed, 18 May 2022 13:29:27 -0300 Subject: [PATCH] Vehicle: create '_commandCanBeDuplicated()', use for MOTOR_TEST --- src/Vehicle/Vehicle.cc | 15 ++++++++++++++- src/Vehicle/Vehicle.h | 1 + 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 1dc24ef..1bb98b6 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2876,9 +2876,22 @@ bool Vehicle::_sendMavCommandShouldRetry(MAV_CMD command) } } +bool Vehicle::_commandCanBeDuplicated(MAV_CMD command) +{ + // For some commands we don't care about response as much as we care about sending them regularly. + // This test avoids commands not being sent due to an ACK not being received yet. + // MOTOR_TEST in ardusub is a case where we need a constant stream of commands so it doesn't time out. + switch (command) { + case MAV_CMD_DO_MOTOR_TEST: + return true; + default: + return false; + } +} + void Vehicle::_sendMavCommandWorker(bool commandInt, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int targetCompId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7) { - if ((targetCompId == MAV_COMP_ID_ALL) || isMavCommandPending(targetCompId, command)) { + if ((targetCompId == MAV_COMP_ID_ALL) || (isMavCommandPending(targetCompId, command) && !_commandCanBeDuplicated(command))) { bool compIdAll = targetCompId == MAV_COMP_ID_ALL; QString rawCommandName = _toolbox->missionCommandTree()->rawName(command); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index e226880..b4a0a99 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1296,6 +1296,7 @@ private: void _sendMavCommandFromList(int index); int _findMavCommandListEntryIndex(int targetCompId, MAV_CMD command); bool _sendMavCommandShouldRetry(MAV_CMD command); + bool _commandCanBeDuplicated(MAV_CMD command); QMap _lowestBatteryChargeStateAnnouncedMap;