AP_AHRS: add trimed view

libraries/AP_AHRS/AP_AHRS.cpp

@@ -359,15 +359,15 @@ void AP_AHRS::update_cd_values(void)
 }
 
 /*
-  create a rotated view of AP_AHRS
+  create a rotated view of AP_AHRS with pitch trim
  */
-AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation)
+AP_AHRS_View *AP_AHRS::create_view_trim(enum Rotation rotation, float pitch_trim)
 {
     if (_view != nullptr) {
         // can only have one
         return nullptr;
     }
-    _view = new AP_AHRS_View(*this, rotation);
+    _view = new AP_AHRS_View(*this, rotation, pitch_trim);
     return _view;
 }
 

libraries/AP_AHRS/AP_AHRS.h

@@ -546,8 +546,13 @@ public:
     }
 
     // create a view
-    AP_AHRS_View *create_view(enum Rotation rotation);
-    
+    AP_AHRS_View *create_view(enum Rotation rotation) {
+        return create_view_trim(rotation, 0.0f);
+    }
+
+    // create a view with pitch trim
+    AP_AHRS_View *create_view_trim(enum Rotation rotation, float pitch_trim);
+
     // return calculated AOA
     float getAOA(void);
 

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -21,24 +21,28 @@
 #include "AP_AHRS_View.h"
 #include <stdio.h>
 
-AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) :
+AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim) :
     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);
+        y_angle = 90;
         break;
     case ROTATION_PITCH_270:
-        rot_view.from_euler(0, radians(270), 0);
+        y_angle =  270;
         break;
     default:
         AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation);
     }
 
+    // Add pitch trim
+    y_angle = wrap_360(y_angle + pitch_trim);
+
+    rot_view.from_euler(radians(x_angle), radians(y_angle), radians(z_angle));
+
     // setup initial state
     update();
 }
@@ -49,7 +53,7 @@ void AP_AHRS_View::update(bool skip_ins_update)
     rot_body_to_ned = ahrs.get_rotation_body_to_ned();
     gyro = ahrs.get_gyro();
 
-    if (rotation != ROTATION_NONE) {
+    if (!is_zero(x_angle) || !is_zero(y_angle) || !is_zero(z_angle)) {
         Matrix3f &r = rot_body_to_ned;
         r.transpose();
         r = rot_view * r;

libraries/AP_AHRS/AP_AHRS_View.h

@@ -26,7 +26,7 @@ class AP_AHRS_View
 {
 public:
     // Constructor
-    AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation);
+    AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim);
 
     // update state
     void update(bool skip_ins_update=false);
@@ -188,4 +188,8 @@ private:
         float sin_pitch;
         float sin_yaw;
     } trig;
+
+    float x_angle;
+    float y_angle;
+    float z_angle;
 };