fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 10:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: check entire Vectors for NAN

Browse Source 
Note: Behavioral change Inf also results in the flag being true.
This commit is contained in:
Matthias Grob
2022-05-09 18:17:57 +02:00
parent 8ca28f3796
commit 200124f094
1 changed files with 6 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+6 -12
View File
@@ -1087,12 +1087,9 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
offboard_control_mode_s ocm{};
ocm.position = PX4_ISFINITE(setpoint.position[0]) || PX4_ISFINITE(setpoint.position[1])
|| PX4_ISFINITE(setpoint.position[2]);
ocm.velocity = PX4_ISFINITE(setpoint.velocity[0]) || PX4_ISFINITE(setpoint.velocity[1])
|| PX4_ISFINITE(setpoint.velocity[2]);
ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
|| PX4_ISFINITE(setpoint.acceleration[2]);
ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported\t");
@@ -1212,12 +1209,9 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
offboard_control_mode_s ocm{};
ocm.position = PX4_ISFINITE(setpoint.position[0]) || PX4_ISFINITE(setpoint.position[1])
|| PX4_ISFINITE(setpoint.position[2]);
ocm.velocity = PX4_ISFINITE(setpoint.velocity[0]) || PX4_ISFINITE(setpoint.velocity[1])
|| PX4_ISFINITE(setpoint.velocity[2]);
ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
|| PX4_ISFINITE(setpoint.acceleration[2]);
ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT force not supported\t");