Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-31 04:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: Add separate interfaces for local horizontal and vertical position

Browse Source
This commit is contained in:
priseborough 2016-07-09 21:36:09 +10:00 committed by Andrew Tridgell
parent a8cd037f56
commit 5454f76702
3 changed files with 103 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_AHRS/AP_AHRS.h
View File

@ -293,6 +293,18 @@ public:
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 {
return false;
}
// return a Down position relative to home in meters
// Return true if estimate is valid
virtual bool get_relative_position_D(float &posD) const {
return false;
}
// return ground speed estimate in meters/second. Used by ground vehicles.
float groundspeed(void) {
return groundspeed_vector().length();

89
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -343,7 +343,7 @@ bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
switch (active_EKF_type()) {
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
if (EKF1.getLLH(loc) && EKF1.getPosNED(ned_pos) && EKF1.getOriginLLH(origin)) {
if (EKF1.getLLH(loc) && EKF1.getPosD(ned_pos.z) && EKF1.getOriginLLH(origin)) {
// fixup altitude using relative position from EKF origin
loc.alt = origin.alt - ned_pos.z*100;
return true;
@ -351,7 +351,7 @@ bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
break;
#endif
case EKF_TYPE2:
if (EKF2.getLLH(loc) && EKF2.getPosNED(-1,ned_pos) && EKF2.getOriginLLH(origin)) {
if (EKF2.getLLH(loc) && EKF2.getPosD(-1,ned_pos.z) && EKF2.getOriginLLH(origin)) {
// fixup altitude using relative position from EKF origin
loc.alt = origin.alt - ned_pos.z*100;
return true;
@ -677,13 +677,27 @@ bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
return false;
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
return EKF1.getPosNED(vec);
case EKF_TYPE1: {
Vector2f posNE;
float posD;
bool position_is_valid = (EKF1.getPosNE(posNE) && EKF1.getPosD(posD));
vec.x = posNE.x;
vec.y = posNE.y;
vec.z = posD;
return position_is_valid;
}
#endif
case EKF_TYPE2:
default:
return EKF2.getPosNED(-1,vec);
default: {
Vector2f posNE;
float posD;
bool position_is_valid = (EKF2.getPosNE(-1,posNE) && EKF2.getPosD(-1,posD));
vec.x = posNE.x;
vec.y = posNE.y;
vec.z = posD;
return position_is_valid;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL: {
@ -699,6 +713,69 @@ bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
}
}
// return a relative ground position in meters/second, North/East
// order. return true if estimate is valid
bool AP_AHRS_NavEKF::get_relative_position_NE(Vector2f &posNE) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1: {
bool position_is_valid = EKF1.getPosNE(posNE);
return position_is_valid;
}
#endif
case EKF_TYPE2:
default: {
bool position_is_valid = EKF2.getPosNE(-1,posNE);
return position_is_valid;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL: {
Location loc;
get_position(loc);
posNE = location_diff(get_home(), loc);
return true;
}
#endif
}
}
// return a relative ground position in meters/second, Down
// return true if the estimate is valid
bool AP_AHRS_NavEKF::get_relative_position_D(float &posD) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1: {
bool position_is_valid = EKF1.getPosD(posD);
return position_is_valid;
}
#endif
case EKF_TYPE2:
default: {
bool position_is_valid = EKF2.getPosD(-1,posD);
return position_is_valid;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL: {
const struct SITL::sitl_fdm &fdm = _sitl->state;
posD = -(fdm.altitude - get_home().alt*0.01f);
return true;
}
#endif
}
}
/*
canonicalise _ekf_type, forcing it to be 0, 1 or 2
*/

8
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -140,6 +140,14 @@ public:
bool get_velocity_NED(Vector3f &vec) const;
bool get_relative_position_NED(Vector3f &vec) const;
// return the relative position in North/East order
// return true if the estimate is valid
bool get_relative_position_NE(Vector2f &posNE) const;
// return the relative position in North/East order
// return true if the estimate is valid
bool get_relative_position_D(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 verical position due to the various errors that are being corrected for.
bool get_vert_pos_rate(float &velocity);