AC_AttitudeControl: add 6DoF support

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -133,7 +133,7 @@ public:
     void inertial_frame_reset();
 
     // Command a Quaternion attitude with feedforward and smoothing
-    void input_quaternion(Quaternion attitude_desired_quat);
+    virtual void input_quaternion(Quaternion attitude_desired_quat);
 
     // 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);
@@ -147,16 +147,16 @@ public:
         float euler_pitch_angle_cd, float euler_yaw_rate_cds) {}
 
     // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
-    void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
+    virtual void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
 
     // Command an angular velocity with angular velocity feedforward and smoothing
     virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
 
     // Command an angular velocity with angular velocity feedforward and smoothing
-    void input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
+    virtual void input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
 
     // Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
-    void input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
+    virtual void input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
 
     // Command an angular step (i.e change) in body frame angle
     virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd);
@@ -260,7 +260,7 @@ public:
     float angle_boost() const { return _angle_boost; }
 
     // Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
-    float get_althold_lean_angle_max() const;
+    virtual float get_althold_lean_angle_max() const;
 
     // Return configured tilt angle limit in centidegrees
     float lean_angle_max() const { return _aparm.angle_max; }

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp

@@ -0,0 +1,129 @@
+#ifdef ENABLE_SCRIPTING
+
+#include "AC_AttitudeControl_Multi_6DoF.h"
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+
+// 6DoF control is extracted from the existing copter code by treating desired angles as thrust angles rather than vehicle attitude.
+// Vehicle attitude is then set separately, typically the vehicle would matain 0 roll and pitch.
+// rate commands result in the vehicle behaving as a ordinary copter.
+
+// run lowest level body-frame rate controller and send outputs to the motors
+void AC_AttitudeControl_Multi_6DoF::rate_controller_run() {
+
+    // pass current offsets to motors and run baseclass controller
+    // motors require the offsets to know which way is up
+    float roll_deg = roll_offset_deg;
+    float pitch_deg = pitch_offset_deg;
+    // if 6DoF control, always point directly up
+    // this stops horizontal drift due to error between target and true attitude
+    if (lateral_enable) {
+        roll_deg = degrees(AP::ahrs().get_roll());
+    }
+    if (forward_enable) {
+        pitch_deg = degrees(AP::ahrs().get_pitch());
+    }
+    _motors.set_roll_pitch(roll_deg,pitch_deg);
+
+    AC_AttitudeControl_Multi::rate_controller_run();
+}
+
+/*
+    override all input to the attitude controller and convert desired angles into thrust angles and substitute
+*/
+
+// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
+void AC_AttitudeControl_Multi_6DoF::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds) {
+
+    set_forward_lateral(euler_pitch_angle_cd, euler_roll_angle_cd);
+
+    AC_AttitudeControl_Multi::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds);
+}
+
+// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
+void AC_AttitudeControl_Multi_6DoF::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) {
+
+    set_forward_lateral(euler_pitch_angle_cd, euler_roll_angle_cd);
+
+    AC_AttitudeControl_Multi::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw);
+}
+
+void AC_AttitudeControl_Multi_6DoF::set_forward_lateral(float &euler_pitch_angle_cd, float &euler_roll_angle_cd)
+{
+    // pitch/forward
+    if (forward_enable) {
+        _motors.set_forward(-sinf(radians(euler_pitch_angle_cd * 0.01f)));
+        euler_pitch_angle_cd = pitch_offset_deg * 100.0f;
+    } else {
+        _motors.set_forward(0.0f);
+        euler_pitch_angle_cd += pitch_offset_deg * 100.0f;
+    }
+    euler_pitch_angle_cd = wrap_180_cd(euler_pitch_angle_cd);
+
+    // roll/lateral
+    if (lateral_enable) {
+        _motors.set_lateral(sinf(radians(euler_roll_angle_cd * 0.01f)));
+        euler_roll_angle_cd = roll_offset_deg * 100.0f;
+    } else {
+        _motors.set_lateral(0.0f);
+        euler_roll_angle_cd += roll_offset_deg * 100.0f;
+    }
+    euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd);
+}
+
+/*
+    all other input functions should zero thrust vectoring
+*/
+
+// Command euler yaw rate and pitch angle with roll angle specified in body frame
+// (used only by tailsitter quadplanes)
+void AC_AttitudeControl_Multi_6DoF::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds) {
+    _motors.set_lateral(0.0f);
+    _motors.set_forward(0.0f);
+
+    AC_AttitudeControl_Multi::input_euler_rate_yaw_euler_angle_pitch_bf_roll(plane_controls, euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds);
+}
+
+// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
+void AC_AttitudeControl_Multi_6DoF::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds) {
+    _motors.set_lateral(0.0f);
+    _motors.set_forward(0.0f);
+
+    AC_AttitudeControl_Multi::input_euler_rate_roll_pitch_yaw(euler_roll_rate_cds, euler_pitch_rate_cds, euler_yaw_rate_cds);
+}
+
+// Command an angular velocity with angular velocity feedforward and smoothing
+void AC_AttitudeControl_Multi_6DoF::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) {
+    _motors.set_lateral(0.0f);
+    _motors.set_forward(0.0f);
+
+    AC_AttitudeControl_Multi::input_rate_bf_roll_pitch_yaw(roll_rate_bf_cds, pitch_rate_bf_cds, yaw_rate_bf_cds);
+}
+
+// Command an angular velocity with angular velocity feedforward and smoothing
+void AC_AttitudeControl_Multi_6DoF::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) {
+    _motors.set_lateral(0.0f);
+    _motors.set_forward(0.0f);
+
+    AC_AttitudeControl_Multi::input_rate_bf_roll_pitch_yaw_2(roll_rate_bf_cds, pitch_rate_bf_cds, yaw_rate_bf_cds);
+}
+
+// Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
+void AC_AttitudeControl_Multi_6DoF::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) {
+    _motors.set_lateral(0.0f);
+    _motors.set_forward(0.0f);
+
+    AC_AttitudeControl_Multi::input_rate_bf_roll_pitch_yaw_3(roll_rate_bf_cds, pitch_rate_bf_cds, yaw_rate_bf_cds);
+}
+
+// Command an angular step (i.e change) in body frame angle
+void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd) {
+    _motors.set_lateral(0.0f);
+    _motors.set_forward(0.0f);
+
+    AC_AttitudeControl_Multi::input_angle_step_bf_roll_pitch_yaw(roll_angle_step_bf_cd, pitch_angle_step_bf_cd, yaw_angle_step_bf_cd);
+}
+
+AC_AttitudeControl_Multi_6DoF *AC_AttitudeControl_Multi_6DoF::_singleton = nullptr;
+
+#endif // ENABLE_SCRIPTING

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h

@@ -0,0 +1,96 @@
+#pragma once
+#ifdef ENABLE_SCRIPTING
+
+#include "AC_AttitudeControl_Multi.h"
+
+class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi {
+public:
+    AC_AttitudeControl_Multi_6DoF(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt):
+        AC_AttitudeControl_Multi(ahrs,aparm,motors,dt) {
+
+        if (_singleton != nullptr) {
+            AP_HAL::panic("Can only be one AC_AttitudeControl_Multi_6DoF");
+        }
+        _singleton = this;
+    }
+
+    static AC_AttitudeControl_Multi_6DoF *get_singleton() {
+        return _singleton;
+    }
+
+    // Command a Quaternion attitude with feedforward and smoothing
+    // not used anywhere in current code, panic so this implementaiton is not overlooked
+    void input_quaternion(Quaternion attitude_desired_quat) override {
+        AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
+    }
+
+    /*
+        override input functions to attitude controller and convert desired angles into thrust angles and substitute for osset angles
+    */
+
+    // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
+    void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)  override;
+
+    // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
+    void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override;
+
+    /*
+        all other input functions should zero thrust vectoring and behave as a normal copter
+    */
+
+    // Command euler yaw rate and pitch angle with roll angle specified in body frame
+    // (used only by tailsitter quadplanes)
+    void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds) override;
+
+    // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
+    void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds) override;
+
+     // Command an angular velocity with angular velocity feedforward and smoothing
+    void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) override;
+
+    // Command an angular velocity with angular velocity feedforward and smoothing
+    void input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) override;
+
+    // Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
+    void input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) override;
+
+    // Command an angular step (i.e change) in body frame angle
+    void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd) override;
+
+    // run lowest level body-frame rate controller and send outputs to the motors
+    void rate_controller_run() override;
+
+    // limiting lean angle based on throttle makes no sense for 6DoF, always allow 90 deg, return in centi-degrees
+    float get_althold_lean_angle_max() const override { return 9000.0f; }
+
+    // set the attitude that will be used in 6DoF flight
+    void set_offset_roll_pitch(float roll_deg, float pitch_deg) {
+        roll_offset_deg = roll_deg;
+        pitch_offset_deg = pitch_deg;
+    }
+
+    // these flags enable or disable lateral or forward thrust, with both disabled the vehicle acts like a traditional copter
+    // it will roll and pitch to move, its also possible to enable only forward or lateral to suit the vehicle configuration.
+    // by default the vehicle is full 6DoF, these can be set in flight via scripting
+    void set_forward_enable(bool b) {
+        forward_enable = b;
+    }
+    void set_lateral_enable(bool b) {
+        lateral_enable = b;
+    }
+
+private:
+
+    void set_forward_lateral(float &euler_pitch_angle_cd, float &euler_roll_angle_cd);
+
+    float roll_offset_deg;
+    float pitch_offset_deg;
+
+    bool forward_enable = true;
+    bool lateral_enable = true;
+
+    static AC_AttitudeControl_Multi_6DoF *_singleton;
+
+};
+
+#endif // ENABLE_SCRIPTING