From 200124f0944508dfc76e340c0f5b657739a7430a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 9 May 2022 18:17:57 +0200 Subject: [PATCH] mavlink_receiver: check entire Vectors for NAN Note: Behavioral change Inf also results in the flag being true. --- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 64fb4f96a8..c689e7a0b3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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");