Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: use GPS singleton

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

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -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) {

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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);

28
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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) {

14
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -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;

2
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -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

24
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -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;
}

4
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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;
}