fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 13:58:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: don't publish out of range joystick input

Browse Source 
Note that the MAVLink definition explicitly writes
"A value of INT16_MAX indicates that this axis is invalid."
which before this change was happily executed.
This commit is contained in:
Matthias Grob
2024-12-19 17:59:41 +01:00
parent 5cf85e320a
commit 61961350f9
1 changed files with 21 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+21 -10
View File
@@ -2145,25 +2145,36 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
} }
manual_control_setpoint_s manual_control_setpoint{}; manual_control_setpoint_s manual_control_setpoint{};
manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f;
manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f; if (math::isInRange((int)mavlink_manual_control.x, -1000, 1000)) { manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f; }
if (math::isInRange((int)mavlink_manual_control.y, -1000, 1000)) { manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f; }
// For backwards compatibility at the moment interpret throttle in range [0,1000] // For backwards compatibility at the moment interpret throttle in range [0,1000]
manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; if (math::isInRange((int)mavlink_manual_control.z, 0, 1000)) { manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; }
manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
if (math::isInRange((int)mavlink_manual_control.r, -1000, 1000)) { manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; }
// Pass along the button states // Pass along the button states
manual_control_setpoint.buttons = mavlink_manual_control.buttons; manual_control_setpoint.buttons = mavlink_manual_control.buttons;
if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; } if (mavlink_manual_control.enabled_extensions & (1u << 2)
&& math::isInRange((int)mavlink_manual_control.aux1, -1000, 1000)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; } if (mavlink_manual_control.enabled_extensions & (1u << 3)
&& math::isInRange((int)mavlink_manual_control.aux2, -1000, 1000)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; } if (mavlink_manual_control.enabled_extensions & (1u << 4)
&& math::isInRange((int)mavlink_manual_control.aux3, -1000, 1000)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; } if (mavlink_manual_control.enabled_extensions & (1u << 5)
&& math::isInRange((int)mavlink_manual_control.aux4, -1000, 1000)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; } if (mavlink_manual_control.enabled_extensions & (1u << 6)
&& math::isInRange((int)mavlink_manual_control.aux5, -1000, 1000)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; }
if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } if (mavlink_manual_control.enabled_extensions & (1u << 7)
&& math::isInRange((int)mavlink_manual_control.aux6, -1000, 1000)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; }
manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id(); manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id();
manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time();