AP_DDS: Enable offboard attitude control via ros2

Astik-2002 · Astik-2002 · commit f3c5599100eb · 2024-04-09T14:21:14.000+05:30
diff --git a/ArduCopter/AP_ExternalControl_Copter.cpp b/ArduCopter/AP_ExternalControl_Copter.cpp
@@ -29,6 +29,19 @@ bool AP_ExternalControl_Copter::set_linear_velocity_and_yaw_rate(const Vector3f
     return true;
 }
 
+/*
+  Sets the target orientation, angular velocity, angular acceleration and desired thrust
+  in body frame
+*/
+bool AP_ExternalControl_Copter::set_angular_goals(const Quaternion &orientation, const Vector3f &angular_velocity, const Vector3f &angular_acceleration, const float &thrust)
+{
+    if (!ready_for_external_control()) {
+        return false;
+    }
+    copter.mode_guided.set_angle(orientation, angular_velocity, angular_acceleration, thrust, true);
+    return true;
+}
+
 bool AP_ExternalControl_Copter::ready_for_external_control()
 {
     return copter.flightmode->in_guided_mode() && copter.motors->armed();
diff --git a/ArduCopter/AP_ExternalControl_Copter.h b/ArduCopter/AP_ExternalControl_Copter.h
@@ -15,6 +15,13 @@ class AP_ExternalControl_Copter : public AP_ExternalControl {
       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;
+
+    /*
+        Sets the target orientation, angular velocity, angular acceleration and desired thrust
+        in body frame
+    */
+    bool set_angular_goals(const Quaternion &orientation, const Vector3f &angular_velocity, const Vector3f &angular_acceleration, const float &thrust) override WARN_IF_UNUSED;
+
 private:
     /*
       Return true if Copter is ready to handle external control data.
diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
@@ -382,7 +382,7 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
     Quaternion q;
     q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
 
-    mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
+    mode_guided.set_angle(q, Vector3f{}, Vector3f{},climb_rate_ms*100, false);
     return true;
 }
 #endif
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
@@ -1261,7 +1261,8 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
             ang_vel.z = packet.body_yaw_rate;
         }
 
-        copter.mode_guided.set_angle(attitude_quat, ang_vel,
+        Vector3f angular_acceleration{0.0, 0.0, 0.0}; // only for use with custom controllers, hence 0 by default
+        copter.mode_guided.set_angle(attitude_quat, ang_vel, angular_acceleration,
                 climb_rate_or_thrust, use_thrust);
 
         break;
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
@@ -1037,7 +1037,7 @@ class ModeGuided : public Mode {
     // climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
     // use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
     //             IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
-    void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust);
+    void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, const Vector3f &ang_acceleration ,float climb_rate_cms_or_thrust, bool use_thrust);
 
     bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false);
     bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
@@ -16,6 +16,7 @@ struct {
     uint32_t update_time_ms;
     Quaternion attitude_quat;
     Vector3f ang_vel;
+    Vector3f ang_acceleration; // angular acceleration, not used by main PID controller but can be used in custom controllers
     float yaw_rate_cds;
     float climb_rate_cms;   // climb rate in cms.  Used if use_thrust is false
     float thrust;           // thrust from -1 to 1.  Used if use_thrust is true
@@ -315,6 +316,7 @@ void ModeGuided::angle_control_start()
     guided_angle_state.update_time_ms = millis();
     guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
     guided_angle_state.ang_vel.zero();
+    guided_angle_state.ang_acceleration.zero();
     guided_angle_state.climb_rate_cms = 0.0f;
     guided_angle_state.yaw_rate_cds = 0.0f;
     guided_angle_state.use_yaw_rate = false;
@@ -635,7 +637,7 @@ bool ModeGuided::use_wpnav_for_position_control() const
 // climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
 // use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
 //             IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
-void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust)
+void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, const Vector3f &ang_acceleration,float climb_rate_cms_or_thrust, bool use_thrust)
 {
     // check we are in velocity control mode
     if (guided_mode != SubMode::Angle) {
@@ -644,6 +646,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
 
     guided_angle_state.attitude_quat = attitude_quat;
     guided_angle_state.ang_vel = ang_vel;
+    guided_angle_state.ang_acceleration = ang_acceleration;
 
     guided_angle_state.use_thrust = use_thrust;
     if (use_thrust) {
@@ -978,7 +981,7 @@ void ModeGuided::angle_control_run()
     if (guided_angle_state.attitude_quat.is_zero()) {
         attitude_control->input_rate_bf_roll_pitch_yaw(ToDeg(guided_angle_state.ang_vel.x) * 100.0f, ToDeg(guided_angle_state.ang_vel.y) * 100.0f, ToDeg(guided_angle_state.ang_vel.z) * 100.0f);
     } else {
-        attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel);
+        attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel, guided_angle_state.ang_acceleration);
     }
 
     // call position controller
diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/GlobalPosition.msg"
+  "msg/AngularVelandAccn.msg"
   "srv/ArmMotors.srv"
   "srv/ModeSwitch.srv"
   DEPENDENCIES geometry_msgs std_msgs
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -228,12 +228,11 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
 
 // Command a Quaternion attitude with feedforward and smoothing
 // attitude_desired_quat: is updated on each time_step by the integral of the angular velocity
-void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target)
+void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target, Vector3f ang_acceleration_target)
 {
     Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat;
     Vector3f attitude_error_angle;
     attitude_error_quat.to_axis_angle(attitude_error_angle);
-
     // Limit the angular velocity
     ang_vel_limit(ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
 
@@ -260,7 +259,7 @@ void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vec
     attitude_desired_update.from_axis_angle(ang_vel_target * _dt);
     attitude_desired_quat = attitude_desired_quat * attitude_desired_update;
     attitude_desired_quat.normalize();
-
+    _ang_acceleration_target = ang_acceleration_target;
     // Call quaternion attitude controller
     attitude_controller_run_quat();
 }
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -161,7 +161,8 @@ class AC_AttitudeControl {
 
     // Command a Quaternion attitude with feedforward and smoothing
     // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
-    virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target);
+    // Angular acceleration target will be ignored, only for use with custom controllers
+    virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target, Vector3f ang_acceleration_target);
 
     // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
     virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
@@ -491,6 +492,10 @@ class AC_AttitudeControl {
     // It is reset to zero immediately after it is used.
     Vector3f            _sysid_ang_vel_body;
 
+    // This is the angular acceleration in radians per second^2 in the body frame. This is not used in 
+    // main attitude controller but can be used in custom attitude controllers.
+    Vector3f            _ang_acceleration_target;
+
     // This is the unitless value added to the output of the PID by the System Identification Mode.
     // It is reset to zero immediately after it is used.
     Vector3f            _actuator_sysid;
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp
@@ -149,15 +149,15 @@ void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float rol
 // Command a Quaternion attitude with feedforward and smoothing
 // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
 // not used anywhere in current code, panic in SITL so this implementation is not overlooked
-void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) {
+void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target, Vector3f ang_acceleration_target) {
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
 #endif
 
     _motors.set_lateral(0.0f);
     _motors.set_forward(0.0f);
 
-    AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_target);
+    AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_target, ang_acceleration_target);
 }
 
 
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h
@@ -21,7 +21,7 @@ class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi {
     // Command a Quaternion attitude with feedforward and smoothing
     // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
     // not used anywhere in current code, panic so this implementation is not overlooked
-    void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) override;
+    void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target, Vector3f ang_acceleration_target) override;
     /*
         override input functions to attitude controller and convert desired angles into thrust angles and substitute for offset angles
     */
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -15,6 +15,7 @@
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
 
+#include "ardupilot_msgs/msg/AngularVelandAccn.h"
 #include "ardupilot_msgs/srv/ArmMotors.h"
 #include "ardupilot_msgs/srv/ModeSwitch.h"
 
@@ -46,6 +47,7 @@ static constexpr uint16_t DELAY_PING_MS = 500;
 sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {};
 tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {};
 geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {};
+custom_msgs_msg_AngularVelandAccn AP_DDS_Client::rx_angular_control_topic {};
 ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {};
 
 
@@ -564,6 +566,21 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
 #endif // AP_EXTERNAL_CONTROL_ENABLED
         break;
     }
+    
+    case topics[to_underlying(TopicIndex::ANGULAR_CONTROL_SUB)].dr_id.id: {
+        const bool success = custom_msgs_msg_AngularVelandAccn_deserialize_topic(ub, &rx_angular_control_topic);
+        if (success == false) {
+            break;
+        }
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+        if (!AP_DDS_External_Control::handle_angular_control(rx_angular_control_topic)) {
+            // TODO #23430 handle velocity control failure through rosout, throttled.
+        }
+#endif // AP_EXTERNAL_CONTROL_ENABLED
+        break;
+    }
+
     case topics[to_underlying(TopicIndex::GLOBAL_POSITION_SUB)].dr_id.id: {
         const bool success = ardupilot_msgs_msg_GlobalPosition_deserialize_topic(ub, &rx_global_position_control_topic);
         if (success == false) {
diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h
@@ -17,6 +17,7 @@
 #include "sensor_msgs/msg/Joy.h"
 #include "geometry_msgs/msg/PoseStamped.h"
 #include "geometry_msgs/msg/TwistStamped.h"
+#include "ardupilot_msgs/msg/AngularVelandAccn.h"
 #include "geographic_msgs/msg/GeoPointStamped.h"
 #include "geographic_msgs/msg/GeoPoseStamped.h"
 #include "rosgraph_msgs/msg/Clock.h"
@@ -72,6 +73,8 @@ class AP_DDS_Client
     static sensor_msgs_msg_Joy rx_joy_topic;
     // incoming REP147 velocity control
     static geometry_msgs_msg_TwistStamped rx_velocity_control_topic;
+    // incoming orientation, angular velocity, angular acceleration and thrust goals
+    static custom_msgs_msg_AngularVelandAccn rx_angular_control_topic;
     // incoming REP147 goal interface global position
     static ardupilot_msgs_msg_GlobalPosition rx_global_position_control_topic;
     // outgoing transforms
diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp
@@ -85,6 +85,33 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
     return false;
 }
 
+bool AP_DDS_External_Control::handle_angular_control(custom_msgs_msg_AngularVelandAccn& cmd_angular_goals)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+
+    // commands in body frame
+    Quaternion orientation;
+    orientation[0] = cmd_angular_goals.orientation.x;
+    orientation[1] = cmd_angular_goals.orientation.y;
+    orientation[2] = cmd_angular_goals.orientation.z;
+    orientation[3] = cmd_angular_goals.orientation.w;
+
+    Vector3f angular_velocity;
+    angular_velocity[0] = cmd_angular_goals.angular_rate.x;
+    angular_velocity[1] = cmd_angular_goals.angular_rate.y;
+    angular_velocity[2] = cmd_angular_goals.angular_rate.z;
+
+    Vector3f angular_acceleration;
+    angular_acceleration[0] = cmd_angular_goals.angular_acceleration.x;
+    angular_acceleration[1] = cmd_angular_goals.angular_acceleration.y;
+    angular_acceleration[2] = cmd_angular_goals.angular_acceleration.z;
+    float Thrust = cmd_angular_goals.thrust;
+    return external_control->set_angular_goals(orientation, angular_velocity, angular_acceleration, Thrust);
+}
+
 bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out)
 {
 
diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h
@@ -3,6 +3,7 @@
 #if AP_DDS_ENABLED
 #include "ardupilot_msgs/msg/GlobalPosition.h"
 #include "geometry_msgs/msg/TwistStamped.h"
+#include "ardupilot_msgs/msg/AngularVelandAccn.h"
 
 #include <AP_Common/Location.h>
 
@@ -13,6 +14,7 @@ class AP_DDS_External_Control
     // https://ros.org/reps/rep-0147.html#goal-interface
     static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos);
     static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel);
+    static bool handle_angular_control(custom_msgs_msg_AngularVelandAccn& cmd_angular_goals);
 private:
     static bool convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out);
 };
diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h
@@ -25,6 +25,7 @@ enum class TopicIndex: uint8_t {
     JOY_SUB,
     DYNAMIC_TRANSFORMS_SUB,
     VELOCITY_CONTROL_SUB,
+    ANGULAR_CONTROL_SUB,
     GLOBAL_POSITION_SUB,
 };
 
@@ -166,6 +167,16 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .dw_profile_label = "",
         .dr_profile_label = "velocitycontrol__dr",
     },
+    {
+        .topic_id = to_underlying(TopicIndex::ANGULAR_CONTROL_SUB),
+        .pub_id = to_underlying(TopicIndex::ANGULAR_CONTROL_SUB),
+        .sub_id = to_underlying(TopicIndex::ANGULAR_CONTROL_SUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::ANGULAR_CONTROL_SUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::ANGULAR_CONTROL_SUB), .type=UXR_DATAREADER_ID},
+        .topic_profile_label = "angularcontrol__t",
+        .dw_profile_label = "",
+        .dr_profile_label = "angularcontrol__dr",
+    },
     {
         .topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
         .pub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/AngularVelandAccn.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/AngularVelandAccn.idl
@@ -0,0 +1,34 @@
+// This is an example and does not exist in the default ROS 2 message sets.
+
+#include "geometry_msgs/msg/Vector3.idl"
+#include "geometry_msgs/msg/Quaternion.idl"
+#include "std_msgs/msg/Header.idl"
+
+module custom_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "This message contains angular rate, angular acceleration, orientation as a quaternion, and thrust, each with a reference frame and timestamp. \
+       The angular rate and angular acceleration are represented in free space, anchored at the origin, and only the rotational component applies when transformed. \
+       The orientation is represented as a quaternion. \
+       The thrust is represented as a single floating-point number.")
+    struct AngularVelandAccn {
+      std_msgs::msg::Header header;
+
+      @verbatim (language="comment", text="Angular rate represented by a Vector3. \
+                                           It describes the angular velocity around each of the three principal axes.")
+      geometry_msgs::msg::Vector3 angular_rate;
+
+      @verbatim (language="comment", text="Angular acceleration represented by a Vector3. \
+                                           It describes the rate of change of angular velocity around each of the three principal axes.")
+      geometry_msgs::msg::Vector3 angular_acceleration;
+
+      @verbatim (language="comment", text="Orientation represented by a quaternion. \
+                                           It describes the orientation in space as a rotation from a reference orientation.")
+      geometry_msgs::msg::Quaternion orientation;
+
+      @verbatim (language="comment", text="Thrust represented as a single floating-point number. \
+                                           It describes the magnitude of the thrust force.")
+      double thrust;
+    };
+  };
+};
diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml
@@ -319,6 +319,21 @@
       <dataType>geometry_msgs::msg::dds_::TwistStamped_</dataType>
     </topic>
   </data_reader>
+  <topic profile_name="angularcontrol__t">
+    <name>rt/ap/cmd_ang_goals</name>
+    <dataType>ardupilot_msgs::msg::dds_::AngularVelandAccn_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>5</depth>
+    </historyQos>
+  </topic>
+  <data_reader profile_name="angularcontrol__dr">
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/cmd_ang_goals</name>
+      <dataType>ardupilot_msgs::msg::dds_::AngularVelandAccn_</dataType>
+    </topic>
+  </data_reader>
   <replier profile_name="arm_motors__replier" service_name="rs/ap/arm_motorsService" request_type="ardupilot_msgs::srv::dds_::ArmMotors_Request_" reply_type="ardupilot_msgs::srv::dds_::ArmMotors_Response_">
     <request_topic_name>rq/ap/arm_motorsRequest</request_topic_name>
     <reply_topic_name>rr/ap/arm_motorsReply</reply_topic_name>
diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h
@@ -25,6 +25,14 @@ class AP_ExternalControl
         return false;
     }
 
+    /*
+        Sets the target orientation, angular velocity, angular acceleration and desired thrust
+        in body frame
+    */
+    virtual bool set_angular_goals(const Quaternion &orientation, const Vector3f &angular_velocity, const Vector3f &angular_acceleration, const float &thrust) WARN_IF_UNUSED {
+        return true;
+    }
+
     /*
         Sets the target global position with standard guided mode behavior.
     */

Original file line number	Diff line number	Diff line change
`@@ -382,7 +382,7 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo`
`382`	`382`	`Quaternion q;`
`383`	`383`	`q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));`
`384`	`384`
`385`		`- mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);`
	`385`	`+ mode_guided.set_angle(q, Vector3f{}, Vector3f{},climb_rate_ms*100, false);`
`386`	`386`	`return true;`
`387`	`387`	`}`
`388`	`388`	`#endif`
Original file line number	Diff line number	Diff line change
`@@ -1261,7 +1261,8 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)`
`1261`	`1261`	`ang_vel.z = packet.body_yaw_rate;`
`1262`	`1262`	`}`
`1263`	`1263`
`1264`		`- copter.mode_guided.set_angle(attitude_quat, ang_vel,`
	`1264`	`+ Vector3f angular_acceleration{0.0, 0.0, 0.0}; // only for use with custom controllers, hence 0 by default`
	`1265`	`+ copter.mode_guided.set_angle(attitude_quat, ang_vel, angular_acceleration,`
`1265`	`1266`	`climb_rate_or_thrust, use_thrust);`
`1266`	`1267`
`1267`	`1268`	`break;`