aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: use GPS singleton

Browse Source
This commit is contained in:
Peter Barker 2017-12-02 12:02:33 +11:00 committed by Francisco Ferreira
parent aea460df2c
commit 9c8466dc03
7 changed files with 38 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -603,7 +603,7 @@ void NavEKF2::check_log_write(void)
logging.log_compass = false;
}
if (logging.log_gps) {
DataFlash_Class::instance()->Log_Write_GPS(_ahrs->get_gps(), _ahrs->get_gps().primary_sensor(), imuSampleTime_us);
DataFlash_Class::instance()->Log_Write_GPS(AP::gps(), AP::gps().primary_sensor(), imuSampleTime_us);
logging.log_gps = false;
}
if (logging.log_baro) {

2
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -420,7 +420,7 @@ bool NavEKF2_core::setOriginLLH(const Location &loc)
void NavEKF2_core::setOrigin()
{
// assume origin at current GPS location (no averaging)
EKF_origin = _ahrs->get_gps().location();
EKF_origin = AP::gps().location();
// if flying, correct for height change from takeoff so that the origin is at field elevation
if (inFlight) {
EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z);

25
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -411,8 +411,9 @@ void NavEKF2_core::readGpsData()
{
// check for new GPS data
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > 70) {
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
const AP_GPS &gps = AP::gps();
if (gps.last_message_time_ms() - lastTimeGpsReceived_ms > 70) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// report GPS fix status
gpsCheckStatus.bad_fix = false;
@ -420,7 +421,7 @@ void NavEKF2_core::readGpsData()
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time
lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();
lastTimeGpsReceived_ms = gps.last_message_time_ms();
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
// ideally we should be using a timing signal from the GPS receiver to set this time
@ -433,17 +434,17 @@ void NavEKF2_core::readGpsData()
gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms);
// Get which GPS we are using for position information
gpsDataNew.sensor_idx = _ahrs->get_gps().primary_sensor();
gpsDataNew.sensor_idx = gps.primary_sensor();
// read the NED velocity from the GPS
gpsDataNew.vel = _ahrs->get_gps().velocity();
gpsDataNew.vel = gps.velocity();
// Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
// Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw;
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
if (!gps.speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f;
} else {
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
@ -451,7 +452,7 @@ void NavEKF2_core::readGpsData()
}
gpsPosAccuracy *= (1.0f - alpha);
float gpsPosAccRaw;
if (!_ahrs->get_gps().horizontal_accuracy(gpsPosAccRaw)) {
if (!gps.horizontal_accuracy(gpsPosAccRaw)) {
gpsPosAccuracy = 0.0f;
} else {
gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
@ -459,7 +460,7 @@ void NavEKF2_core::readGpsData()
}
gpsHgtAccuracy *= (1.0f - alpha);
float gpsHgtAccRaw;
if (!_ahrs->get_gps().vertical_accuracy(gpsHgtAccRaw)) {
if (!gps.vertical_accuracy(gpsHgtAccRaw)) {
gpsHgtAccuracy = 0.0f;
} else {
gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
@ -467,16 +468,16 @@ void NavEKF2_core::readGpsData()
}
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
if (gps.num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.0f;
} else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
} else if (gps.num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.4f;
} else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f;
}
// Check if GPS can output vertical velocity, if it is allowed to be used, and set GPS fusion mode accordingly
if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
if (gps.have_vertical_velocity() && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;
@ -494,7 +495,7 @@ void NavEKF2_core::readGpsData()
calcGpsGoodForFlight();
// Read the GPS locaton in WGS-84 lat,long,height coordinates
const struct Location &gpsloc = _ahrs->get_gps().location();
const struct Location &gpsloc = gps.location();
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if (gpsGoodToAlign && !validOrigin) {

14
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -251,9 +251,9 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const
} 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 ((AP::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();
const struct Location &gpsloc = AP::gps().location();
Vector2f tempPosNE = location_diff(EKF_origin, gpsloc);
posNE.x = tempPosNE.x;
posNE.y = tempPosNE.y;
@ -315,6 +315,8 @@ bool NavEKF2_core::getHAGL(float &HAGL) const
// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
bool NavEKF2_core::getLLH(struct Location &loc) const
{
const AP_GPS &gps = AP::gps();
if(validOrigin) {
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid
loc.alt = 100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z);
@ -331,9 +333,9 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
} else {
// we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS
// in this mode we cannot use the EKF states to estimate position so will return the best available data
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_2D)) {
if ((gps.status() >= AP_GPS::GPS_OK_FIX_2D)) {
// we have a GPS position fix to return
const struct Location &gpsloc = _ahrs->get_gps().location();
const struct Location &gpsloc = gps.location();
loc.lat = gpsloc.lat;
loc.lng = gpsloc.lng;
return true;
@ -347,8 +349,8 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
} else {
// If no origin has been defined for the EKF, then we cannot use its position states so return a raw
// GPS reading if available and return false
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D)) {
const struct Location &gpsloc = _ahrs->get_gps().location();
if ((gps.status() >= AP_GPS::GPS_OK_FIX_3D)) {
const struct Location &gpsloc = gps.location();
loc = gpsloc;
loc.flags.relative_alt = 0;
loc.flags.terrain_alt = 0;

2
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -238,7 +238,7 @@ void NavEKF2_core::SelectVelPosFusion()
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// correct GPS data for position offset of antenna phase centre relative to the IMU
Vector3f posOffsetBody = _ahrs->get_gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
if (!posOffsetBody.is_zero()) {
if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay

24
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -22,6 +22,8 @@ extern const AP_HAL::HAL& hal;
*/
bool NavEKF2_core::calcGpsGoodToAlign(void)
{
const AP_GPS &gps = AP::gps();
if (inFlight && assume_zero_sideslip() && !use_compass()) {
// this is a special case where a plane has launched without magnetometer
// is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
@ -45,7 +47,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
// Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary
const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location
const struct Location &gpsloc = gps.location(); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsesd since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
@ -73,18 +75,18 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
// Check that the vertical GPS vertical velocity is reasonable after noise filtering
bool gpsVertVelFail;
if (_ahrs->get_gps().have_vertical_velocity() && onGround) {
if (gps.have_vertical_velocity() && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else if ((frontend->_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
} else if ((frontend->_fusionModeGPS == 0) && !gps.have_vertical_velocity()) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
// if we have a 3D fix with no vertical velocity and
// EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
frontend->_fusionModeGPS.set(1);
gcs().send_text(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
}
@ -126,7 +128,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
if (gps.horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
@ -145,7 +147,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
// Check for vertical GPS accuracy
float vAcc = 0.0f;
bool vAccFail = false;
if (_ahrs->get_gps().vertical_accuracy(vAcc)) {
if (gps.vertical_accuracy(vAcc)) {
vAccFail = (vAcc > 7.5f * checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
}
// Report check result as a text string and bitmask
@ -172,24 +174,24 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
}
// fail if satellite geometry is poor
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
bool hdopFail = (gps.get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
// Report check result as a text string and bitmask
if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop()));
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop()));
gpsCheckStatus.bad_hdop = true;
} else {
gpsCheckStatus.bad_hdop = false;
}
// fail if not enough sats
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
bool numSatsFail = (gps.num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
// Report check result as a text string and bitmask
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
"GPS numsats %u (needs 6)", gps.num_sats());
gpsCheckStatus.bad_sats = true;
} else {
gpsCheckStatus.bad_sats = false;
@ -251,7 +253,7 @@ void NavEKF2_core::calcGpsGoodForFlight(void)
// get the receivers reported speed accuracy
float gpsSpdAccRaw;
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
if (!AP::gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccRaw = 0.0f;
}

2
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -333,7 +333,7 @@ void NavEKF2_core::InitialiseVariables()
bool NavEKF2_core::InitialiseFilterBootstrap(void)
{
// If we are a plane and don't have GPS lock then don't initialise
if (assume_zero_sideslip() && _ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) {
if (assume_zero_sideslip() && AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"EKF2 init failure: No GPS lock");