Dds yaw control plane

ArduCopter/AP_ExternalControl_Copter.cpp

@@ -12,7 +12,7 @@
   set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw
   velocity is in earth frame, NED, m/s
 */
-bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)
+bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, const float yaw_rate_rads)
 {
     if (!ready_for_external_control()) {
         return false;

ArduCopter/AP_ExternalControl_Copter.h

@@ -15,7 +15,7 @@ class AP_ExternalControl_Copter : public AP_ExternalControl
       Velocity is in earth frame, NED [m/s].
       Yaw is in earth frame, NED [rad/s].
      */
-    bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) override WARN_IF_UNUSED;
+    bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, const float yaw_rate_rads) override WARN_IF_UNUSED;
 
     /*
       Sets the target global position for a loiter point.

ArduPlane/AP_ExternalControl_Plane.cpp

@@ -19,4 +19,14 @@ bool AP_ExternalControl_Plane::set_global_position(const Location& loc)
     return plane.set_target_location(loc);
 }
 
+/*
+    Sets only the target yaw rate
+    Yaw is in earth frame, NED [rad/s]
+*/
+bool AP_ExternalControl_Plane::set_yaw_rate(const float yaw_rate_rads)
+{
+    // TODO validate yaw rate is feasible for current dynamics
+    return plane.set_target_yaw_rate(yaw_rate_rads);
+}
+
 #endif // AP_EXTERNAL_CONTROL_ENABLED

ArduPlane/AP_ExternalControl_Plane.h

@@ -16,6 +16,12 @@ class AP_ExternalControl_Plane : public AP_ExternalControl {
         Sets the target global position for a loiter point.
     */
     bool set_global_position(const Location& loc) override WARN_IF_UNUSED;
+
+    /*
+        Sets only the target yaw rate
+        Yaw is in earth frame, NED [rad/s]
+    */
+    bool set_yaw_rate(const float yaw_rate_rads) override WARN_IF_UNUSED;
 };
 
 #endif // AP_EXTERNAL_CONTROL_ENABLED

ArduPlane/Plane.cpp

@@ -884,6 +884,24 @@ bool Plane::set_target_location(const Location &target_loc)
     plane.set_guided_WP(loc);
     return true;
 }
+
+bool Plane::set_target_yaw_rate(const float yaw_rate)
+{
+    if (plane.control_mode != &plane.mode_guided) {
+        // only accept yaw rate updates when in GUIDED mode
+        return false;
+    }
+
+    const auto direction_is_ccw = yaw_rate < 0;
+    if (!is_zero(yaw_rate)) {
+        auto const speed = plane.ahrs.groundspeed();
+        auto const radius = speed / yaw_rate;
+        plane.mode_guided.set_radius_and_direction(abs(radius), direction_is_ccw);
+    } else {
+        plane.mode_guided.set_radius_and_direction(0.0, true);
+    }
+    return true;
+}
 #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 
 #if AP_SCRIPTING_ENABLED

ArduPlane/Plane.h

@@ -1306,6 +1306,7 @@ class Plane : public AP_Vehicle {
     bool is_taking_off() const override;
 #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
     bool set_target_location(const Location& target_loc) override;
+    bool set_target_yaw_rate(const float yaw_rate_rads) override;
 #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 #if AP_SCRIPTING_ENABLED
     bool get_target_location(Location& target_loc) override;

Rover/AP_ExternalControl_Rover.cpp

@@ -12,7 +12,7 @@
   set linear velocity and yaw rate. Pass NaN for yaw_rate_rads to not control yaw
   velocity is in earth frame, NED, m/s
 */
-bool AP_ExternalControl_Rover::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads)
+bool AP_ExternalControl_Rover::set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, const float yaw_rate_rads)
 {
     if (!ready_for_external_control()) {
         return false;

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -58,6 +58,8 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
         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.
         Vector3f linear_velocity;
@@ -67,8 +69,13 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
             float(-cmd_vel.twist.linear.z) };
         const float yaw_rate = -cmd_vel.twist.angular.z;
 
+        if (linear_velocity_base_link.is_all_nan()) {
+            return external_control->set_yaw_rate(yaw_rate);
+        }
+
         auto &ahrs = AP::ahrs();
         linear_velocity = ahrs.body_to_earth(linear_velocity_base_link);
+
         return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);
     }
 

libraries/AP_ExternalControl/AP_ExternalControl.h

@@ -22,7 +22,7 @@ class AP_ExternalControl
       Velocity is in earth frame, NED [m/s].
       Yaw is in earth frame, NED [rad/s].
      */
-    virtual bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, float yaw_rate_rads) WARN_IF_UNUSED {
+    virtual bool set_linear_velocity_and_yaw_rate(const Vector3f &linear_velocity, const float yaw_rate_rads) WARN_IF_UNUSED {
         return false;
     }
 
@@ -43,6 +43,14 @@ class AP_ExternalControl
     */
     virtual bool disarm(AP_Arming::Method method, bool do_disarm_checks) WARN_IF_UNUSED;
 
+    /*
+        Sets only the target yaw rate
+        Yaw is in earth frame, NED [rad/s]
+    */
+    virtual bool set_yaw_rate(const float yaw_rate_rads) WARN_IF_UNUSED {
+        return false;
+    }
+
     static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED {
         return singleton;
     }

libraries/AP_Math/vector3.cpp

@@ -366,6 +366,12 @@ bool Vector3<T>::is_nan(void) const
     return isnan(x) || isnan(y) || isnan(z);
 }
 
+template <typename T>
+bool Vector3<T>::is_all_nan(void) const
+{
+    return isnan(x) && isnan(y) && isnan(z);
+}
+
 template <typename T>
 bool Vector3<T>::is_inf(void) const
 {

libraries/AP_Math/vector3.h

@@ -182,6 +182,9 @@ class Vector3
     // check if any elements are NAN
     bool is_nan(void) const WARN_IF_UNUSED;
 
+    // check if all elements are NAN
+    bool is_all_nan(void) const WARN_IF_UNUSED;
+
     // check if any elements are infinity
     bool is_inf(void) const WARN_IF_UNUSED;
 

libraries/AP_Vehicle/AP_Vehicle.h

@@ -176,7 +176,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     virtual bool start_takeoff(const float alt) { return false; }
     // Method to control vehicle position for use by external control
     virtual bool set_target_location(const Location& target_loc) { return false; }
-#endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
+    // Method to control vehicle yaw rate for use by external control
+    // Yaw is in earth frame, NED [rad/s]
+    virtual bool set_target_yaw_rate(const float yaw_rate_rads) { return false; }
+#endif // AP_EXTERNAL_CONTROL_ENABLED  || AP_EXTERNAL_CONTROL_ENABLED
 #if AP_SCRIPTING_ENABLED
     /*
       methods to control vehicle for use by scripting