AP_AHRS: add method to set pitch trim

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -39,15 +39,20 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_
         AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation);
     }
 
+    _pitch_trim_deg = pitch_trim_deg;
     // Add pitch trim
-    y_angle = wrap_360(y_angle + pitch_trim_deg);
-
-    rot_view.from_euler(0, radians(y_angle), 0);
+    rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0);
 
     // setup initial state
     update();
 }
 
+// apply pitch trim
+void AP_AHRS_View::set_pitch_trim(float trim_deg) {
+    _pitch_trim_deg = trim_deg; 
+    rot_view.from_euler(0, radians(wrap_360(y_angle + _pitch_trim_deg)), 0);
+};
+
 // update state
 void AP_AHRS_View::update(bool skip_ins_update)
 {

libraries/AP_AHRS/AP_AHRS_View.h

@@ -48,6 +48,9 @@ public:
         return rot_body_to_ned;
     }
 
+    // apply pitch trim
+    void set_pitch_trim(float trim_deg);
+
     // helper trig value accessors
     float cos_roll() const {
         return trig.cos_roll;
@@ -190,4 +193,5 @@ private:
     } trig;
 
     float y_angle;
+    float _pitch_trim_deg;
 };