AP_AHRS: added earth to body 2D rotations

libraries/AP_AHRS/AP_AHRS.cpp

@@ -397,3 +397,17 @@ float AP_AHRS::getSSA(void)
     update_AOA_SSA();
     return _SSA;
 }
+
+// rotate a 2D vector from earth frame to body frame
+Vector2f AP_AHRS::rotate_earth_to_body2D(const Vector2f &ef) const
+{
+    return Vector2f(ef.x * _cos_yaw + ef.y * _sin_yaw,
+                    -ef.x * _sin_yaw + ef.y * _cos_yaw);
+}
+
+// rotate a 2D vector from earth frame to body frame
+Vector2f AP_AHRS::rotate_body_to_earth2D(const Vector2f &bf) const
+{
+    return Vector2f(bf.x * _cos_yaw - bf.y * _sin_yaw,
+                    bf.x * _sin_yaw + bf.y * _cos_yaw);
+}

libraries/AP_AHRS/AP_AHRS.h

@@ -556,6 +556,14 @@ public:
     // return calculated SSA
     float getSSA(void);
 
+    // rotate a 2D vector from earth frame to body frame
+    // in result, x is forward, y is right
+    Vector2f rotate_earth_to_body2D(const Vector2f &ef_vector) const;
+
+    // rotate a 2D vector from earth frame to body frame
+    // in input, x is forward, y is right
+    Vector2f rotate_body_to_earth2D(const Vector2f &bf) const;
+    
     virtual void update_AOA_SSA(void);
 
     // get_hgt_ctrl_limit - get maximum height to be observed by the

libraries/AP_AHRS/AP_AHRS_View.cpp

@@ -77,3 +77,17 @@ Vector3f AP_AHRS_View::get_gyro_latest(void) const {
     gyro_latest.rotate(rotation);
     return gyro_latest;
 }
+
+// rotate a 2D vector from earth frame to body frame
+Vector2f AP_AHRS_View::rotate_earth_to_body2D(const Vector2f &ef) const
+{
+    return Vector2f(ef.x * trig.cos_yaw + ef.y * trig.sin_yaw,
+                    -ef.x * trig.sin_yaw + ef.y * trig.cos_yaw);
+}
+
+// rotate a 2D vector from earth frame to body frame
+Vector2f AP_AHRS_View::rotate_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);
+}

libraries/AP_AHRS/AP_AHRS_View.h

@@ -145,6 +145,14 @@ public:
         return ahrs.getLastPosDownReset(posDelta);
     }
 
+    // rotate a 2D vector from earth frame to body frame
+    // in result, x is forward, y is right
+    Vector2f rotate_earth_to_body2D(const Vector2f &ef_vector) const;
+
+    // rotate a 2D vector from earth frame to body frame
+    // in input, x is forward, y is right
+    Vector2f rotate_body_to_earth2D(const Vector2f &bf) const;
+    
     // return the average size of the roll/pitch error estimate
     // since last call
     float get_error_rp(void) const {