AC_AttitudeControl: added support for AP_AHRS_View

this allows for tailsitters with a different attitude view

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -7,7 +7,7 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
-#include <AP_AHRS/AP_AHRS.h>
+#include <AP_AHRS/AP_AHRS_View.h>
 #include <AP_Motors/AP_Motors.h>
 #include <AC_PID/AC_PID.h>
 #include <AC_PID/AC_P.h>
@@ -44,7 +44,7 @@
 
 class AC_AttitudeControl {
 public:
-    AC_AttitudeControl( AP_AHRS &ahrs,
+    AC_AttitudeControl( AP_AHRS_View &ahrs,
                         const AP_Vehicle::MultiCopter &aparm,
                         AP_Motors& motors,
                         float dt) :
@@ -369,7 +369,7 @@ protected:
     float               _throttle_rpy_mix;
 
     // References to external libraries
-    const AP_AHRS&      _ahrs;
+    const AP_AHRS_View&  _ahrs;
     const AP_Vehicle::MultiCopter &_aparm;
     AP_Motors&          _motors;
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

@@ -30,7 +30,7 @@
 
 class AC_AttitudeControl_Heli : public AC_AttitudeControl {
 public:
-    AC_AttitudeControl_Heli( AP_AHRS &ahrs,
+    AC_AttitudeControl_Heli( AP_AHRS_View &ahrs,
                         const AP_Vehicle::MultiCopter &aparm,
                         AP_MotorsHeli& motors,
                         float dt) :

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@@ -145,7 +145,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
     AP_GROUPEND
 };
 
-AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) :
+AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) :
     AC_AttitudeControl(ahrs, aparm, motors, dt),
     _motors_multi(motors),
     _pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h

@@ -41,7 +41,7 @@
 
 class AC_AttitudeControl_Multi : public AC_AttitudeControl {
 public:
-	AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt);
+	AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_Vehicle::MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt);
 
 	// empty destructor to suppress compiler warning
 	virtual ~AC_AttitudeControl_Multi() {}

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -23,7 +23,7 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
 // Note that the Vector/Matrix constructors already implicitly zero
 // their values.
 //
-AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
+AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav,
                              const AP_Motors& motors, AC_AttitudeControl& attitude_control,
                              AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z,
                              AC_P& p_pos_xy, AC_PI_2D& pi_vel_xy) :

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -47,7 +47,7 @@ class AC_PosControl
 public:
 
     /// Constructor
-    AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
+    AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& inav,
                   const AP_Motors& motors, AC_AttitudeControl& attitude_control,
                   AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z,
                   AC_P& p_pos_xy, AC_PI_2D& pi_vel_xy);
@@ -366,7 +366,7 @@ private:
     void check_for_ekf_z_reset();
 
     // references to inertial nav and ahrs libraries
-    const AP_AHRS&              _ahrs;
+    const AP_AHRS_View &        _ahrs;
     const AP_InertialNav&       _inav;
     const AP_Motors&            _motors;
     AC_AttitudeControl&         _attitude_control;

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -0,0 +1,45 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *  AHRS View class - for creating a 2nd view of the vehicle attitude
+ *
+ */
+
+#include "AP_AHRS_View.h"
+
+AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) :
+    rotation(_rotation),
+    ahrs(_ahrs)
+{
+
+}
+
+// update state
+void AP_AHRS_View::update(void)
+{
+    rot_body_to_ned = ahrs.get_rotation_body_to_ned();
+    gyro = ahrs.get_gyro();
+
+    rot_body_to_ned.to_euler(&roll, &pitch, &yaw);
+
+    roll_sensor  = degrees(roll) * 100;
+    pitch_sensor = degrees(pitch) * 100;
+    yaw_sensor   = degrees(yaw) * 100;
+
+    ahrs.calc_trig(rot_body_to_ned,
+                   trig.cos_roll, trig.cos_pitch, trig.cos_yaw,
+                   trig.sin_roll, trig.sin_pitch, trig.sin_yaw);
+}

libraries/AP_AHRS/AP_AHRS_View.h

@@ -0,0 +1,156 @@
+#pragma once
+
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *  AHRS View class - for creating a 2nd view of the vehicle attitude
+ *
+ */
+
+#include "AP_AHRS.h"
+
+class AP_AHRS_View
+{
+public:
+    // Constructor
+    AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation);
+
+    // update state
+    void update(void);
+    
+    // empty virtual destructor
+    virtual ~AP_AHRS_View() {}
+
+    // return a smoothed and corrected gyro vector
+    const Vector3f &get_gyro(void) const {
+        return gyro;
+    }
+
+    // return a DCM rotation matrix representing our current
+    // attitude in this view
+    const Matrix3f &get_rotation_body_to_ned(void) const {
+        return rot_body_to_ned;
+    }
+
+    // helper trig value accessors
+    float cos_roll() const {
+        return trig.cos_roll;
+    }
+    float cos_pitch() const {
+        return trig.cos_pitch;
+    }
+    float cos_yaw() const   {
+        return trig.cos_yaw;
+    }
+    float sin_roll() const  {
+        return trig.sin_roll;
+    }
+    float sin_pitch() const {
+        return trig.sin_pitch;
+    }
+    float sin_yaw() const   {
+        return trig.sin_yaw;
+    }
+
+    
+    /*
+      wrappers around ahrs functions which pass-thru directly. See
+      AP_AHRS.h for description of each function
+     */
+    bool get_position(struct Location &loc) const {
+        return ahrs.get_position(loc);
+    }
+    
+    Vector3f wind_estimate(void) {
+        return ahrs.wind_estimate();
+    }
+    
+    bool airspeed_estimate(float *airspeed_ret) const {
+        return ahrs.airspeed_estimate(airspeed_ret);
+    }
+    
+    bool airspeed_estimate_true(float *airspeed_ret) const {
+        return ahrs.airspeed_estimate_true(airspeed_ret);
+    }
+
+    float get_EAS2TAS(void) const {
+        return ahrs.get_EAS2TAS();
+    }
+
+    Vector2f groundspeed_vector(void) {
+        return ahrs.groundspeed_vector();
+    }
+
+    bool get_velocity_NED(Vector3f &vec) const {
+        return ahrs.get_velocity_NED(vec);
+    }
+
+    bool get_expected_mag_field_NED(Vector3f &ret) const {
+        return ahrs.get_expected_mag_field_NED(ret);
+    }
+
+    bool get_relative_position_NED(Vector3f &vec) const {
+        return ahrs.get_relative_position_NED(vec);
+    }
+
+    bool get_relative_position_NE(Vector2f &vecNE) const {
+        return ahrs.get_relative_position_NE(vecNE);
+    }
+
+    bool get_relative_position_D(float &posD) const {
+        return get_relative_position_D(posD);
+    }
+
+    float groundspeed(void) {
+        return ahrs.groundspeed();
+    }
+
+    const Vector3f &get_accel_ef_blended(void) const {
+        return ahrs.get_accel_ef_blended();
+    }
+
+    uint32_t getLastPosNorthEastReset(Vector2f &pos) const {
+        return ahrs.getLastPosNorthEastReset(pos);
+    }
+
+    uint32_t getLastPosDownReset(float &posDelta) const {
+        return ahrs.getLastPosDownReset(posDelta);
+    }
+    
+    float roll;
+    float pitch;
+    float yaw;
+    int32_t roll_sensor;
+    int32_t pitch_sensor;
+    int32_t yaw_sensor;
+    
+private:
+    const enum Rotation rotation;
+    AP_AHRS &ahrs;
+    
+    Matrix3f rot_body_to_ned;
+    Vector3f gyro;
+    
+    struct {
+        float cos_roll;
+        float cos_pitch;
+        float cos_yaw;
+        float sin_roll;
+        float sin_pitch;
+        float sin_yaw;
+    } trig;
+};
+