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

AP_AHRS: Split all NED methods into a collection of NED relative to home vs origin

Browse Source
This commit is contained in:
Michael du Breuil 2017-01-30 12:06:13 -07:00 committed by Andrew Tridgell
parent c079532a7a
commit 3274f1dbb1
3 changed files with 94 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_AHRS/AP_AHRS.h
View File

@ -314,19 +314,37 @@ public:
// return a position relative to home in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true
virtual bool get_relative_position_NED(Vector3f &vec) const {
virtual bool get_relative_position_NED_home(Vector3f &vec) const {
return false;
}
// return a position relative to origin in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true
virtual bool get_relative_position_NED_origin(Vector3f &vec) const {
return false;
}
// return a position relative to home in meters, North/East
// order. Return true if estimate is valid
virtual bool get_relative_position_NE(Vector2f &vecNE) const {
virtual bool get_relative_position_NE_home(Vector2f &vecNE) const {
return false;
}
// return a position relative to origin in meters, North/East
// order. Return true if estimate is valid
virtual bool get_relative_position_NE_origin(Vector2f &vecNE) const {
return false;
}
// return a Down position relative to home in meters
// if EKF is unavailable will return the baro altitude
virtual void get_relative_position_D_home(float &posD) const {
return;
}
// return a Down position relative to origin in meters
// Return true if estimate is valid
virtual bool get_relative_position_D(float &posD) const {
virtual bool get_relative_position_D_origin(float &posD) const {
return false;
}

69
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -636,9 +636,9 @@ bool AP_AHRS_NavEKF::get_hagl(float &height) const
}
}
// return a relative ground position in meters/second, North/East/Down
// order. Must only be called if have_inertial_nav() is true
bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
// return a relative ground position to the origin in meters
// North/East/Down order.
bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
@ -685,9 +685,28 @@ bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
}
}
// write a relative ground position estimate in meters, North/East order
// return a relative ground position to the home in meters
// North/East/Down order.
bool AP_AHRS_NavEKF::get_relative_position_NED_home(Vector3f &vec) const
{
Location originLLH;
Vector3f originNED;
if (!get_relative_position_NED_origin(originNED) ||
!get_origin(originLLH)) {
return false;
}
Vector3f offset = location_3d_diff_NED(originLLH, _home);
vec.x = originNED.x + offset.x;
vec.y = originNED.y + offset.y;
vec.z = originNED.z - offset.z;
return true;
}
// write a relative ground position estimate to the origin in meters, North/East order
// return true if estimate is valid
bool AP_AHRS_NavEKF::get_relative_position_NE(Vector2f &posNE) const
bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
@ -715,9 +734,30 @@ bool AP_AHRS_NavEKF::get_relative_position_NE(Vector2f &posNE) const
}
}
// write a relative ground position in meters, Down
// return a relative ground position to the home in meters
// North/East order.
bool AP_AHRS_NavEKF::get_relative_position_NE_home(Vector2f &posNE) const
{
Location originLLH;
Vector2f originNE;
if (!get_relative_position_NE_origin(originNE) ||
!get_origin(originLLH)) {
return false;
}
Vector2f offset = location_diff(originLLH, _home);
posNE.x = originNE.x + offset.x;
posNE.y = originNE.y + offset.y;
return true;
}
// write a relative ground position estimate to the origin in meters, North/East order
// write a relative ground position to the origin in meters, Down
// return true if the estimate is valid
bool AP_AHRS_NavEKF::get_relative_position_D(float &posD) const
bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
@ -744,6 +784,21 @@ bool AP_AHRS_NavEKF::get_relative_position_D(float &posD) const
}
}
// write a relative ground position to home in meters, Down
// will use the barometer if the EKF isn't available
void AP_AHRS_NavEKF::get_relative_position_D_home(float &posD) const
{
Location originLLH;
float originD;
if (!get_relative_position_D_origin(originD) ||
!get_origin(originLLH)) {
posD = -_baro.get_altitude();
return;
}
posD = originD - ((originLLH.alt - _home.alt) * 0.01f);
return;
}
/*
canonicalise _ekf_type, forcing it to be 0, 2 or 3
type 1 has been deprecated

16
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -127,15 +127,21 @@ public:
bool have_inertial_nav(void) const;
bool get_velocity_NED(Vector3f &vec) const;
bool get_relative_position_NED(Vector3f &vec) const;
// return the relative position in North/East order
// return the relative position NED to either home or origin
// return true if the estimate is valid
bool get_relative_position_NE(Vector2f &posNE) const;
bool get_relative_position_NED_home(Vector3f &vec) const;
bool get_relative_position_NED_origin(Vector3f &vec) const;
// return the relative position in North/East order
// return the relative position NE to either home or origin
// return true if the estimate is valid
bool get_relative_position_D(float &posD) const;
bool get_relative_position_NE_home(Vector2f &posNE) const;
bool get_relative_position_NE_origin(Vector2f &posNE) const;
// return the relative position down to either home or origin
// baro will be used for the _home relative one if the EKF isn't
void get_relative_position_D_home(float &posD) const;
bool get_relative_position_D_origin(float &posD) const;
// Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
// This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.