Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: drop rotate prefix on 2D rotates

Browse Source
This commit is contained in:
Andrew Tridgell 2020-06-02 10:16:33 +10:00
parent 2cd5519d5d
commit f896213770
4 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -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);

24
libraries/AP_AHRS/AP_AHRS.h
View File

@ -543,11 +543,21 @@ 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);
@ -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[];

4
libraries/AP_AHRS/AP_AHRS_View.cpp
View File

@ -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);

4
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -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