AP_DDS: Fix NaN handling

ArduPilot · Oct 24, 2024 · bdb9f43 · bdb9f43
1 parent 4a4f59e
commit bdb9f43
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp
@@ -57,6 +57,8 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
     if (external_control == nullptr) {
         return false;
     }
+
+    // TODO handle stale data from timestamp gracefully.
 
     if (strcmp(cmd_vel.header.frame_id, BASE_LINK_FRAME_ID) == 0) {
         // Convert commands from body frame (x-forward, y-left, z-up) to NED.
@@ -66,7 +68,8 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
             float(cmd_vel.twist.linear.y),
             float(-cmd_vel.twist.linear.z) };
         const float yaw_rate = -cmd_vel.twist.angular.z;
-        if (linear_velocity_base_link == Vector3f(NAN, NAN, NAN)) {
+
+        if (linear_velocity_base_link.is_all_nan()) {
             return external_control->set_yaw_rate(yaw_rate);
         }