mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-02 19:53:57 -04:00
AP_NavEKF2: Add separate horizontal/vertical local position interfaces
This commit is contained in:
parent
487bbc3ed2
commit
e374ec634d
@ -686,6 +686,30 @@ bool NavEKF2::getPosNED(int8_t instance, Vector3f &pos)
|
||||
return core[instance].getPosNED(pos);
|
||||
}
|
||||
|
||||
// Return the last calculated NE position relative to the reference point (m).
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool NavEKF2::getPosNE(int8_t instance, Vector2f &posNE)
|
||||
{
|
||||
if (instance < 0 || instance >= num_cores) instance = primary;
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core[instance].getPosNE(posNE);
|
||||
}
|
||||
|
||||
// Return the last calculated D position relative to the reference point (m).
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool NavEKF2::getPosD(int8_t instance, float &posD)
|
||||
{
|
||||
if (instance < 0 || instance >= num_cores) instance = primary;
|
||||
if (!core) {
|
||||
return false;
|
||||
}
|
||||
return core[instance].getPosD(posD);
|
||||
}
|
||||
|
||||
// return NED velocity in m/s
|
||||
void NavEKF2::getVelNED(int8_t instance, Vector3f &vel)
|
||||
{
|
||||
|
@ -68,6 +68,18 @@ public:
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosNED(int8_t instance, Vector3f &pos);
|
||||
|
||||
// Return the last calculated NE position relative to the reference point (m) for the specified instance.
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosNE(int8_t instance, Vector2f &posNE);
|
||||
|
||||
// Return the last calculated D position relative to the reference point (m) for the specified instance.
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosD(int8_t instance, float &posD);
|
||||
|
||||
// return NED velocity in m/s for the specified instance
|
||||
// An out of range instance (eg -1) returns data for the the primary instance
|
||||
void getVelNED(int8_t instance, Vector3f &vel);
|
||||
|
@ -249,7 +249,57 @@ bool NavEKF2_core::getPosNED(Vector3f &pos) const
|
||||
return false;
|
||||
}
|
||||
|
||||
// Return the last estimated NE position relative to the reference point (m).
|
||||
// Return true if the estimate is valid
|
||||
bool NavEKF2_core::getPosNE(Vector2f &posNE) const
|
||||
{
|
||||
// There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
|
||||
nav_filter_status status;
|
||||
getFilterStatus(status);
|
||||
if (PV_AidingMode != AID_NONE) {
|
||||
// This is the normal mode of operation where we can use the EKF position states
|
||||
posNE.x = outputDataNew.position.x;
|
||||
posNE.y = outputDataNew.position.y;
|
||||
return true;
|
||||
} else {
|
||||
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
|
||||
if(validOrigin) {
|
||||
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
|
||||
// If the origin has been set and we have GPS, then return the GPS position relative to the origin
|
||||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||||
Vector2f tempPosNE = location_diff(EKF_origin, gpsloc);
|
||||
posNE.x = tempPosNE.x;
|
||||
posNE.y = tempPosNE.y;
|
||||
return false;
|
||||
} else {
|
||||
// If no GPS fix is available, all we can do is provide the last known position
|
||||
posNE.x = outputDataNew.position.x;
|
||||
posNE.y = outputDataNew.position.y;
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
// If the origin has not been set, then we have no means of providing a relative position
|
||||
posNE.x = 0.0f;
|
||||
posNE.y = 0.0f;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Return the last calculated D position relative to the reference point (m).
|
||||
// Return true if the estimte is valid
|
||||
bool NavEKF2_core::getPosD(float &posD) const
|
||||
{
|
||||
// The EKF always has a height estimate regardless of mode of operation
|
||||
posD = outputDataNew.position.z;
|
||||
|
||||
// Return the current height solution status
|
||||
nav_filter_status status;
|
||||
getFilterStatus(status);
|
||||
return status.flags.vert_pos;
|
||||
|
||||
}
|
||||
// return the estimated height above ground level
|
||||
bool NavEKF2_core::getHAGL(float &HAGL) const
|
||||
{
|
||||
|
@ -71,6 +71,16 @@ public:
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosNED(Vector3f &pos) const;
|
||||
|
||||
// Return the last calculated NE position relative to the reference point (m).
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosNE(Vector2f &posNE) const;
|
||||
|
||||
// Return the last calculated D position relative to the reference point (m).
|
||||
// If a calculated solution is not available, use the best available data and return false
|
||||
// If false returned, do not use for flight control
|
||||
bool getPosD(float &posD) const;
|
||||
|
||||
// return NED velocity in m/s
|
||||
void getVelNED(Vector3f &vel) const;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user