AC_AttitudeControl: Support MAVLINK_MSG_ID_SET_ATTITUDE_TARGET

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -210,40 +210,40 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
 //    trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
 
 // Command a Quaternion attitude with feedforward and smoothing
-void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
+// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
+void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target)
 {
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    // this function is not currently used, this is a reminder that we need to add a 6DoF implementation
-    AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
-#endif
-
-    // calculate the attitude target euler angles
-    _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
-
     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));
+
     if (_rate_bf_ff_enabled) {
         // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
         // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
         // and an exponential decay specified by _input_tc at the end.
-        _ang_vel_target.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
-        _ang_vel_target.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
-        _ang_vel_target.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
-
-        // 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));
-        // Convert body-frame angular velocity into euler angle derivative of desired attitude
-        ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
+        _ang_vel_target.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, ang_vel_target.x, radians(_ang_vel_roll_max), _dt);
+        _ang_vel_target.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, ang_vel_target.y, radians(_ang_vel_pitch_max), _dt);
+        _ang_vel_target.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, ang_vel_target.z, radians(_ang_vel_yaw_max), _dt);
     } else {
         _attitude_target = attitude_desired_quat;
-
-        // Set rate feedforward requests to zero
-        _euler_rate_target.zero();
-        _ang_vel_target.zero();
+        _ang_vel_target = ang_vel_target;
     }
 
+    // calculate the attitude target euler angles
+    _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
+
+    // Convert body-frame angular velocity into euler angle derivative of desired attitude
+    ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
+
+    // rotate target and normalize
+    Quaternion attitude_desired_update;
+    attitude_desired_update.from_axis_angle(ang_vel_target * _dt);
+    attitude_desired_quat = attitude_desired_quat * attitude_desired_update;
+    attitude_desired_quat.normalize();
+
     // Call quaternion attitude controller
     attitude_controller_run_quat();
 }

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -149,7 +149,8 @@ public:
     void inertial_frame_reset();
 
     // Command a Quaternion attitude with feedforward and smoothing
-    virtual void input_quaternion(Quaternion attitude_desired_quat);
+    // 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);
 
     // 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);
@@ -295,7 +296,7 @@ public:
     // calculates the velocity correction from an angle error. The angular velocity has acceleration and
     // deceleration limits including basic jerk limiting using smoothing_gain
     static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt);
-    static float input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt){ return input_shaping_angle(error_angle,  smoothing_gain,  accel_max,  target_ang_vel,  0.0f,  0.0f,  dt); }
+    static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt){ return input_shaping_angle(error_angle,  input_tc,  accel_max,  target_ang_vel,  0.0f,  0.0f,  dt); }
 
     // limits the acceleration and deceleration of a velocity request
     static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt);

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp

@@ -145,8 +145,9 @@ void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float rol
 }
 
 // Command a Quaternion attitude with feedforward and smoothing
-// not used anywhere in current code, panic in SITL so this implementaiton is not overlooked
-void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion attitude_desired_quat) {
+// 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) {
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
 #endif
@@ -154,7 +155,7 @@ void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion attitude_desired
     _motors.set_lateral(0.0f);
     _motors.set_forward(0.0f);
 
-    AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat);
+    AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_target);
 }
 
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h

@@ -19,8 +19,9 @@ public:
     }
 
     // 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 implementaiton is not overlooked
-    void input_quaternion(Quaternion attitude_desired_quat) override;
+    void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) override;
     /*
         override input functions to attitude controller and convert desired angles into thrust angles and substitute for osset angles
     */