mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF: Always return a NED relative position if possible
If a calculated position is not available, the function will return a value based on raw GPS or last calculation if available, but the status will be set to false to indicate that it cannot be used for control.
This commit is contained in:
parent
20ba1e6b1b
commit
b71b8f4bda
|
@ -3608,23 +3608,44 @@ void NavEKF::getVelNED(Vector3f &vel) const
|
|||
vel = state.velocity;
|
||||
}
|
||||
|
||||
// return the last calculated NED position relative to the reference point (m).
|
||||
// return false if no position is available
|
||||
// Return the last calculated NED position relative to the reference point (m).
|
||||
// if a calculated solution is not available, use the best available data and return false
|
||||
bool NavEKF::getPosNED(Vector3f &pos) const
|
||||
{
|
||||
if (constPosMode) {
|
||||
pos.x = state.position.x + lastKnownPositionNE.x;
|
||||
pos.y = state.position.y + lastKnownPositionNE.y;
|
||||
} else {
|
||||
// The EKF always has a height estimate regardless of mode of operation
|
||||
pos.z = state.position.z;
|
||||
// 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 (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) {
|
||||
// This is the normal mode of operation where we can use the EKF position states
|
||||
pos.x = state.position.x;
|
||||
pos.y = state.position.y;
|
||||
}
|
||||
if (_fusionModeGPS != 3) {
|
||||
pos.z = state.position.z;
|
||||
return true;
|
||||
} else {
|
||||
pos.z = state.position.z - terrainState;
|
||||
// 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);
|
||||
pos.x = tempPosNE.x;
|
||||
pos.y = tempPosNE.y;
|
||||
return false;
|
||||
} else {
|
||||
// If no GPS fix is available, all we can do is provide the last known position
|
||||
pos.x = state.position.x + lastKnownPositionNE.x;
|
||||
pos.y = state.position.y + lastKnownPositionNE.y;
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
// If the origin has not been set, then we have no means of providing a relative position
|
||||
pos.x = 0.0f;
|
||||
pos.y = 0.0f;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
// return body axis gyro bias estimates in rad/sec
|
||||
|
|
|
@ -105,8 +105,9 @@ public:
|
|||
// Check basic filter health metrics and return a consolidated health status
|
||||
bool healthy(void) const;
|
||||
|
||||
// return the last calculated NED position relative to the reference point (m).
|
||||
// return false if no position is available
|
||||
// Return the last calculated NED 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 getPosNED(Vector3f &pos) const;
|
||||
|
||||
// return NED velocity in m/s
|
||||
|
|
Loading…
Reference in New Issue