From 9c8466dc03b89391103d71cd3734e0f67a4dc749 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Dec 2017 12:02:33 +1100 Subject: [PATCH] AP_NavEKF2: use GPS singleton --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 +- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 25 ++++++++++--------- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 14 ++++++----- .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 2 +- .../AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 24 ++++++++++-------- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 2 +- 7 files changed, 38 insertions(+), 33 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 8d72d9a114..dff39779e1 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -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) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 848926950b..c65c4c918d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -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); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index e2b2ee76e5..881445c3be 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 4f4710d7bd..e1a5efd6a1 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 379739948a..210a6467d8 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 62afc987b0..2277dc1f50 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -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; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 89e7be7757..f5d13c955e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -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");