AP_AHRS_view: add rotate method to move from AHRS to AHRS view referance frames

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -100,3 +100,9 @@ Vector2f AP_AHRS_View::body_to_earth2D(const Vector2f &bf) const
     return Vector2f(bf.x * trig.cos_yaw - bf.y * trig.sin_yaw,
                     bf.x * trig.sin_yaw + bf.y * trig.cos_yaw);
 }
+
+// Rotate vector from AHRS reference frame to AHRS view reference frame
+void AP_AHRS_View::rotate(Vector3f &vec) const
+{
+    vec = rot_view * vec;
+}

libraries/AP_AHRS/AP_AHRS_View.h

@@ -195,6 +195,9 @@ public:
     // get pitch trim (deg)
     float get_pitch_trim() const { return _pitch_trim_deg; }
 
+    // Rotate vector from AHRS reference frame to AHRS view refences frame
+    void rotate(Vector3f &vec) const;
+
 private:
     const enum Rotation rotation;
     AP_AHRS &ahrs;