diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 3c2c967f77..15fcd58a75 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -472,14 +472,14 @@ float AP_AHRS::getSSA(void) } // rotate a 2D vector from earth frame to body frame -Vector2f AP_AHRS::rotate_earth_to_body2D(const Vector2f &ef) const +Vector2f AP_AHRS::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 +Vector2f AP_AHRS::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); diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f1b671a353..5ecfd71da6 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -543,12 +543,22 @@ public: // 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; + Vector2f 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; + Vector2f body_to_earth2D(const Vector2f &bf) const; + // convert a vector from body to earth frame + Vector3f body_to_earth(const Vector3f &v) const { + return v * get_rotation_body_to_ned(); + } + + // convert a vector from earth to body frame + Vector3f earth_to_body(const Vector3f &v) const { + return get_rotation_body_to_ned().mul_transpose(v); + } + virtual void update_AOA_SSA(void); // get_hgt_ctrl_limit - get maximum height to be observed by the @@ -573,16 +583,6 @@ public: return _rsem; } - // convert a vector from body to earth frame - Vector3f body_to_earth(const Vector3f &v) const { - return v * get_rotation_body_to_ned(); - } - - // convert a vector from earth to body frame - Vector3f earth_to_body(const Vector3f &v) const { - return get_rotation_body_to_ned().mul_transpose(v); - } - // for holding parameters static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 25fd1414bf..c401cb474f 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -90,14 +90,14 @@ Vector3f AP_AHRS_View::get_gyro_latest(void) const { } // rotate a 2D vector from earth frame to body frame -Vector2f AP_AHRS_View::rotate_earth_to_body2D(const Vector2f &ef) const +Vector2f AP_AHRS_View::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 +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); diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index 597117b71e..01832e621b 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -154,11 +154,11 @@ public: // 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; + Vector2f 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; + Vector2f body_to_earth2D(const Vector2f &bf) const; // return the average size of the roll/pitch error estimate // since last call