Skip to content

Commit

Permalink
fix FunctionActuatorSet: if a param is set to NaN, it should be ignored
Browse files Browse the repository at this point in the history
MAVLink spec: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ACTUATOR
Previously, a command was overwriting all other indexes.
  • Loading branch information
bkueng authored and dagar committed Feb 2, 2024
1 parent 454a987 commit b0e86ba
Showing 1 changed file with 11 additions and 6 deletions.
17 changes: 11 additions & 6 deletions src/lib/mixer_module/functions/FunctionActuatorSet.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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; }
}
}
}
Expand Down

0 comments on commit b0e86ba

Please sign in to comment.