From 8b422c5ed6389211523ce59bffc5d2acb3100775 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 1 Feb 2024 16:01:05 +0100 Subject: [PATCH] fix FunctionActuatorSet: if a param is set to NaN, it should be ignored MAVLink spec: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ACTUATOR Previously, a command was overwriting all other indexes. --- .../functions/FunctionActuatorSet.hpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp index a132304bf0..8c34767eb5 100644 --- a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp +++ b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp @@ -61,12 +61,17 @@ public: int index = (int)(vehicle_command.param7 + 0.5f); if (index == 0) { - _data[0] = vehicle_command.param1; - _data[1] = vehicle_command.param2; - _data[2] = vehicle_command.param3; - _data[3] = vehicle_command.param4; - _data[4] = vehicle_command.param5; - _data[5] = vehicle_command.param6; + if (PX4_ISFINITE(vehicle_command.param1)) { _data[0] = vehicle_command.param1; } + + if (PX4_ISFINITE(vehicle_command.param2)) {_data[1] = vehicle_command.param2; } + + if (PX4_ISFINITE(vehicle_command.param3)) {_data[2] = vehicle_command.param3; } + + if (PX4_ISFINITE(vehicle_command.param4)) {_data[3] = vehicle_command.param4; } + + if (PX4_ISFINITE(vehicle_command.param5)) {_data[4] = vehicle_command.param5; } + + if (PX4_ISFINITE(vehicle_command.param6)) {_data[5] = vehicle_command.param6; } } } }