diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index db09be156d..a3b58ff4d8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -629,7 +629,7 @@ void NavEKF3::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_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 789c682248..80bfbe23ed 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -479,7 +479,7 @@ bool NavEKF3_core::setOriginLLH(const Location &loc) void NavEKF3_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_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 0be1605d2f..8c433fd1cf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -456,8 +456,10 @@ void NavEKF3_core::readGpsData() { // check for new GPS data // limit update rate to avoid overflowing the FIFO buffer - if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) { - 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 > frontend->sensorIntervalMin_ms) { + if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { // report GPS fix status gpsCheckStatus.bad_fix = false; @@ -465,13 +467,13 @@ void NavEKF3_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 // Use the driver specified delay float gps_delay_sec = 0; - _ahrs->get_gps().get_lag(gps_delay_sec); + gps.get_lag(gps_delay_sec); gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f); // Correct for the average intersampling delay due to the filter updaterate @@ -481,17 +483,17 @@ void NavEKF3_core::readGpsData() gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.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); @@ -499,7 +501,7 @@ void NavEKF3_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); @@ -507,7 +509,7 @@ void NavEKF3_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); @@ -515,16 +517,16 @@ void NavEKF3_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, vertical velocity use is permitted 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; @@ -542,7 +544,7 @@ void NavEKF3_core::readGpsData() calcGpsGoodForFlight(); // Read the GPS location 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_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 52c1c4ca76..d74e0c6ebc 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -249,9 +249,9 @@ bool NavEKF3_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; @@ -312,6 +312,8 @@ bool NavEKF3_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 NavEKF3_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); @@ -327,9 +329,9 @@ bool NavEKF3_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; @@ -342,8 +344,8 @@ bool NavEKF3_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_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 2a0c07c815..b8db247167 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -242,7 +242,7 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 7aa187145a..834cb7277f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -43,7 +43,9 @@ bool NavEKF3_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 AP_GPS &gps = AP::gps(); + + 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); @@ -71,18 +73,18 @@ bool NavEKF3_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 // EK3_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, "EK3: Changed EK3_GPS_TYPE to 1"); } @@ -124,7 +126,7 @@ bool NavEKF3_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; @@ -144,7 +146,7 @@ bool NavEKF3_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 @@ -171,24 +173,24 @@ bool NavEKF3_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; @@ -250,7 +252,7 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 009989fa51..aef7ffff83 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -72,7 +72,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c if (_frontend->_fusionModeGPS != 3) { // Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay float gps_delay_sec = 0; - if (!_ahrs->get_gps().get_lag(gps_delay_sec)) { + if (!AP::gps().get_lag(gps_delay_sec)) { if (AP_HAL::millis() - lastInitFailReport_ms > 10000) { lastInitFailReport_ms = AP_HAL::millis(); // provide an escalating series of messages @@ -397,7 +397,7 @@ void NavEKF3_core::InitialiseVariables() bool NavEKF3_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) { statesInitialised = false; return false; }