AP_AHRS: added support for AP_AHRS_View

libraries/AP_AHRS/AP_AHRS.cpp

@@ -15,7 +15,9 @@
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 #include "AP_AHRS.h"
+#include "AP_AHRS_View.h"
 #include <AP_HAL/AP_HAL.h>
+
 extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
@@ -229,6 +231,55 @@ Vector2f AP_AHRS::groundspeed_vector(void)
     return Vector2f(0.0f, 0.0f);
 }
 
+/*
+  calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix
+ */
+void AP_AHRS::calc_trig(const Matrix3f &rot,
+                        float &cr, float &cp, float &cy,
+                        float &sr, float &sp, float &sy) const
+{
+    Vector2f yaw_vector;
+
+    yaw_vector.x = rot.a.x;
+    yaw_vector.y = rot.b.x;
+    if (fabsf(yaw_vector.x) > 0 ||
+        fabsf(yaw_vector.y) > 0) {
+        yaw_vector.normalize();
+    }
+    sy = constrain_float(yaw_vector.y, -1.0, 1.0);
+    cy = constrain_float(yaw_vector.x, -1.0, 1.0);
+
+    // sanity checks
+    if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
+        sy = 0.0f;
+        cy = 1.0f;
+    }
+    
+    float cx2 = rot.c.x * rot.c.x;
+    if (cx2 >= 1.0f) {
+        cp = 0;
+        cr = 1.0f;
+    } else {
+        cp = safe_sqrt(1 - cx2);
+        cr = rot.c.z / cp;
+    }
+    cp = constrain_float(cp, 0, 1.0);
+    cr = constrain_float(cr, -1.0, 1.0); // this relies on constrain_float() of infinity doing the right thing
+
+    sp = -rot.c.x;
+
+    if (!is_zero(cp)) {
+        sr = rot.c.y / cp;
+    }
+
+    if (is_zero(cp) || isinf(cr) || isnan(cr) || isinf(sr) || isnan(sr)) {
+        float r, p, y;
+        rot.to_euler(&r, &p, &y);
+        cr = cosf(r);
+        sr = sinf(r);
+    }
+}
+
 // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
 //      should be called after _dcm_matrix is updated
 void AP_AHRS::update_trig(void)
@@ -239,51 +290,9 @@ void AP_AHRS::update_trig(void)
         _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
     }
 
-    Vector2f yaw_vector;
+    calc_trig(get_rotation_body_to_ned(),
-    const Matrix3f &temp = get_rotation_body_to_ned();
+              _cos_roll, _cos_pitch, _cos_yaw,
-
+              _sin_roll, _sin_pitch, _sin_yaw);
-    // sin_yaw, cos_yaw
-    yaw_vector.x = temp.a.x;
-    yaw_vector.y = temp.b.x;
-    yaw_vector.normalize();
-    _sin_yaw = constrain_float(yaw_vector.y, -1.0, 1.0);
-    _cos_yaw = constrain_float(yaw_vector.x, -1.0, 1.0);
-
-    // cos_roll, cos_pitch
-    float cx2 = temp.c.x * temp.c.x;
-    if (cx2 >= 1.0f) {
-        _cos_pitch = 0;
-        _cos_roll = 1.0f;
-    } else {
-        _cos_pitch = safe_sqrt(1 - cx2);
-        _cos_roll = temp.c.z / _cos_pitch;
-    }
-    _cos_pitch = constrain_float(_cos_pitch, 0, 1.0);
-    _cos_roll = constrain_float(_cos_roll, -1.0, 1.0); // this relies on constrain_float() of infinity doing the right thing
-
-    // sin_roll, sin_pitch
-    _sin_pitch = -temp.c.x;
-    if (is_zero(_cos_pitch)) {
-        _sin_roll = sinf(roll);
-    } else {
-        _sin_roll = temp.c.y / _cos_pitch;
-    }
-
-    // sanity checks
-    if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
-        yaw_vector.x = 0.0f;
-        yaw_vector.y = 0.0f;
-        _sin_yaw = 0.0f;
-        _cos_yaw = 1.0f;
-    }
-
-    if (isinf(_cos_roll) || isnan(_cos_roll)) {
-        _cos_roll = cosf(roll);
-    }
-
-    if (isinf(_sin_roll) || isnan(_sin_roll)) {
-        _sin_roll = sinf(roll);
-    }
 }
 
 /*
@@ -297,3 +306,16 @@ void AP_AHRS::update_cd_values(void)
     if (yaw_sensor < 0)
         yaw_sensor += 36000;
 }
+
+/*
+  create a rotated view of AP_AHRS
+ */
+AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation)
+{
+    if (_view != nullptr) {
+        // can only have one
+        return nullptr;
+    }
+    _view = new AP_AHRS_View(*this, rotation);
+    return _view;
+}

libraries/AP_AHRS/AP_AHRS.h

@@ -43,9 +43,14 @@ enum AHRS_VehicleClass {
 };
 
 
+// forward declare view class
+class AP_AHRS_View;
+
 class AP_AHRS
 {
 public:
+    friend class AP_AHRS_View;
+    
     // Constructor
     AP_AHRS(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
         roll(0.0f),
@@ -500,6 +505,9 @@ public:
     // Retrieves the corrected NED delta velocity in use by the inertial navigation
     virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { ret.zero(); _ins.get_delta_velocity(ret); dt = _ins.get_delta_velocity_dt(); }
 
+    // create a view
+    AP_AHRS_View *create_view(enum Rotation rotation);
+    
 protected:
     AHRS_VehicleClass _vehicle_class;
 
@@ -525,6 +533,11 @@ protected:
         uint8_t wind_estimation         : 1;    // 1 if we should do wind estimation
     } _flags;
 
+    // calculate sin/cos of roll/pitch/yaw from rotation
+    void calc_trig(const Matrix3f &rot,
+                   float &cr, float &cp, float &cy,
+                   float &sr, float &sp, float &sy) const;
+    
     // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
     //      should be called after _dcm_matrix is updated
     void update_trig(void);
@@ -584,6 +597,9 @@ protected:
 
     // which accelerometer instance is active
     uint8_t _active_accel_instance;
+
+    // optional view class
+    AP_AHRS_View *_view;
 };
 
 #include "AP_AHRS_DCM.h"

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -20,6 +20,7 @@
  */
 #include <AP_HAL/AP_HAL.h>
 #include "AP_AHRS.h"
+#include "AP_AHRS_View.h"
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Module/AP_Module.h>
@@ -99,6 +100,11 @@ void AP_AHRS_NavEKF::update(void)
         const Vector3f &exported_gyro_bias = get_gyro_drift();
         hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
     }
+
+    if (_view != nullptr) {
+        // update optional alternative attitude view
+        _view->update();
+    }
 }
 
 void AP_AHRS_NavEKF::update_DCM(void)

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -19,12 +19,28 @@
  */
 
 #include "AP_AHRS_View.h"
+#include <stdio.h>
 
 AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) :
     rotation(_rotation),
     ahrs(_ahrs)
 {
+    switch (rotation) {
+    case ROTATION_NONE:
+        rot_view.identity();
+        break;
+    case ROTATION_PITCH_90:
+        rot_view.from_euler(0, radians(90), 0);
+        break;
+    case ROTATION_PITCH_270:
+        rot_view.from_euler(0, radians(270), 0);
+        break;
+    default:
+        AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation);
+    }
 
+    // setup initial state
+    update();
 }
 
 // update state
@@ -33,6 +49,14 @@ void AP_AHRS_View::update(void)
     rot_body_to_ned = ahrs.get_rotation_body_to_ned();
     gyro = ahrs.get_gyro();
 
+    if (rotation != ROTATION_NONE) {
+        Matrix3f &r = rot_body_to_ned;
+        r.transpose();
+        r = rot_view * r;
+        r.transpose();
+        gyro.rotate(rotation);
+    }
+
     rot_body_to_ned.to_euler(&roll, &pitch, &yaw);
 
     roll_sensor  = degrees(roll) * 100;

libraries/AP_AHRS/AP_AHRS_View.h

@@ -141,6 +141,7 @@ private:
     const enum Rotation rotation;
     AP_AHRS &ahrs;
 
+    Matrix3f rot_view;
     Matrix3f rot_body_to_ned;
     Vector3f gyro;
 
@@ -153,4 +154,3 @@ private:
         float sin_yaw;
     } trig;
 };
-