AP_AHRS: calculation and reporting of AOA and SSA

libraries/AP_AHRS/AP_AHRS.cpp

@@ -326,3 +326,76 @@ AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation)
     _view = new AP_AHRS_View(*this, rotation);
     return _view;
 }
+
+/*
+ * Update AOA and SSA estimation based on airspeed, velocity vector and wind vector
+ *
+ * Based on:
+ * "On estimation of wind velocity, angle-of-attack and sideslip angle of small UAVs using standard sensors" by
+ * Tor A. Johansen, Andrea Cristofaro, Kim Sorensen, Jakob M. Hansen, Thor I. Fossen
+ *
+ * "Multi-Stage Fusion Algorithm for Estimation of Aerodynamic Angles in Mini Aerial Vehicle" by
+ * C.Ramprasadh and Hemendra Arya
+ *
+ * "ANGLE OF ATTACK AND SIDESLIP ESTIMATION USING AN INERTIAL REFERENCE PLATFORM" by
+ * JOSEPH E. ZEIS, JR., CAPTAIN, USAF
+ */
+void AP_AHRS::update_AOA_SSA(void)
+{
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    uint32_t now = AP_HAL::millis();
+    if (now - _last_AOA_update_ms < 50) {
+        // don't update at more than 20Hz
+        return;
+    }
+    _last_AOA_update_ms = now;
+    
+    Vector3f aoa_velocity, aoa_wind;
+
+    // get velocity and wind
+    if (get_velocity_NED(aoa_velocity) == false) {
+        return;
+    }
+
+    aoa_wind = wind_estimate();
+
+    // Rotate vectors to the body frame and calculate velocity and wind
+    const Matrix3f &rot = get_rotation_body_to_ned();
+    aoa_velocity = rot.mul_transpose(aoa_velocity);
+    aoa_wind = rot.mul_transpose(aoa_wind);
+
+    // calculate relative velocity in body coordinates
+    aoa_velocity = aoa_velocity - aoa_wind;
+    float vel_len = aoa_velocity.length();
+
+    // do not calculate if speed is too low
+    if (vel_len < 2.0) {
+        _AOA = 0;
+        _SSA = 0;
+        return;
+    }
+
+    // Calculate AOA and SSA
+    if (aoa_velocity.x > 0) {
+        _AOA = degrees(atanf(aoa_velocity.z / aoa_velocity.x));
+    } else {
+        _AOA = 0;
+    }
+
+    _SSA = degrees(safe_asin(aoa_velocity.y / vel_len));
+#endif
+}
+
+// return current AOA
+float AP_AHRS::getAOA(void)
+{
+    update_AOA_SSA();
+    return _AOA;
+}
+
+// return calculated SSA
+float AP_AHRS::getSSA(void)
+{
+    update_AOA_SSA();
+    return _SSA;
+}

libraries/AP_AHRS/AP_AHRS.h

@@ -515,6 +515,14 @@ public:
     // create a view
     AP_AHRS_View *create_view(enum Rotation rotation);
     
+    // return calculated AOA
+    float getAOA(void);
+
+    // return calculated SSA
+    float getSSA(void);
+
+    virtual void update_AOA_SSA(void);
+
 protected:
     AHRS_VehicleClass _vehicle_class;
 
@@ -607,6 +615,10 @@ protected:
 
     // optional view class
     AP_AHRS_View *_view;
+
+    // AOA and SSA
+    float _AOA, _SSA;
+    uint32_t _last_AOA_update_ms;
 };
 
 #include "AP_AHRS_DCM.h"

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -89,6 +89,9 @@ AP_AHRS_DCM::update(bool skip_ins_update)
 
     // update trig values including _cos_roll, cos_pitch
     update_trig();
+
+    // update AOA and SSA
+    update_AOA_SSA();
 }
 
 // update the DCM matrix using only the gyros

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -243,7 +243,7 @@ public:
 
     // get the index of the current primary gyro sensor
     uint8_t get_primary_gyro_index(void) const override;
-    
+
 private:
     enum EKF_TYPE {EKF_TYPE_NONE=0,
                    EKF_TYPE3=3,