AP_AHRS: refactor View

* AP_AHRS: refactor View

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -42,6 +42,8 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_
     _pitch_trim_deg = pitch_trim_deg;
     // Add pitch trim
     rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0);
+    rot_view_T = rot_view;
+    rot_view_T.transpose();
 
     // setup initial state
     update();
@@ -51,6 +53,8 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_
 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);
+    rot_view_T = rot_view;
+    rot_view_T.transpose();
 };
 
 // update state
@@ -60,11 +64,8 @@ void AP_AHRS_View::update(bool skip_ins_update)
     gyro = ahrs.get_gyro();
 
     if (!is_zero(y_angle + _pitch_trim_deg)) {
-        Matrix3f &r = rot_body_to_ned;
-        r.transpose();
-        r = rot_view * r;
-        r.transpose();
-        gyro.rotate(rotation);
+        rot_body_to_ned = rot_body_to_ned * rot_view_T;
+        gyro = rot_view * gyro;
     }
 
     rot_body_to_ned.to_euler(&roll, &pitch, &yaw);

libraries/AP_AHRS/AP_AHRS_View.h

@@ -183,7 +183,10 @@ private:
     const enum Rotation rotation;
     AP_AHRS &ahrs;
 
+    // body frame rotation for this View
     Matrix3f rot_view;
+    // transpose of rot_view
+    Matrix3f rot_view_T;
     Matrix3f rot_body_to_ned;
     Vector3f gyro;