From 73d6bdbbf43f2c2ac5735aea899e040892079ccd 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 1/2] 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 c9369addd526..7301776113b6 100644 --- a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp +++ b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp @@ -61,12 +61,17 @@ class FunctionActuatorSet : public FunctionProviderBase 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; } } } } From eb464d592048afcf47798c9e5d1eb0171f2384a5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 1 Feb 2024 16:25:02 +0100 Subject: [PATCH 2/2] commander: send ack for VEHICLE_CMD_DO_SET_ACTUATOR --- src/modules/commander/Commander.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a9061ad33ba1..8256986a6c21 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1367,6 +1367,10 @@ Commander::handle_command(const vehicle_command_s &cmd) answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); break; + case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + break; + case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: @@ -1403,7 +1407,6 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: - case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE: case vehicle_command_s::VEHICLE_CMD_DO_WINCH: case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER: