addition for handling angular goals (not compiling)

ArduCopter/AP_ExternalControl_Copter.cpp

@@ -29,6 +29,18 @@ 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 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)
+}
+
 bool AP_ExternalControl_Copter::ready_for_external_control()
 {
     return copter.flightmode->in_guided_mode() && copter.motors->armed();

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.

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

ArduCopter/GCS_Mavlink.cpp

@@ -1268,7 +1268,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;

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);

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

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();
 }

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;

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);
 }
 
 

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
     */

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -92,15 +92,30 @@ bool AP_DDS_External_Control::handle_angular_control(custom_msgs_msg_AngularVela
         return false;
     }
 
-    // Convert commands from body frame (x-forward, y-left, z-up) to NED.
-    //Quaternion orientation;
-    //Vector3f angular_velocity;
-    //Vector3f angular_acceleration;
-    //double thrust;
-
-    GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "angular control");
-
-    return true;
+    if(strcmp(cmd_angular_goals.header.frame_id, BASE_LINK_FRAME_ID) == 0)
+    {
+        // 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;
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "angular control");
+        return external_control->set_angular_goals(orientation, angular_velocity, angular_acceleration, thrust);
+    }
+    return false;
 }
 
 bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out)

libraries/AP_DDS/Idl/ardupilot_msgs/msg/AngularVelandAccn.idl

@@ -28,7 +28,7 @@ module custom_msgs {
 
       @verbatim (language="comment", text="Thrust represented as a single floating-point number. \
                                            It describes the magnitude of the thrust force.")
-      double thrust;
+      floatgit thrust;
     };
   };
 };

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.
     */