dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Beat Küng 2024-02-01 16:01:05 +01:00 committed by Daniel Agar
parent 75d6e523b5
commit 8b422c5ed6
1 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/lib/mixer_module/functions/FunctionActuatorSet.hpp
View File

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