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; logging.log_compass = false;
} }
if (logging.log_gps) { 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; logging.log_gps = false;
} }
if (logging.log_baro) { 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() void NavEKF3_core::setOrigin()
{ {
// assume origin at current GPS location (no averaging) // 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 flying, correct for height change from takeoff so that the origin is at field elevation
if (inFlight) { if (inFlight) {
EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z); 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 // check for new GPS data
// limit update rate to avoid overflowing the FIFO buffer // limit update rate to avoid overflowing the FIFO buffer
if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) { const AP_GPS &gps = AP::gps();
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
if (gps.last_message_time_ms() - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// report GPS fix status // report GPS fix status
gpsCheckStatus.bad_fix = false; gpsCheckStatus.bad_fix = false;
@ -465,13 +467,13 @@ void NavEKF3_core::readGpsData()
secondLastGpsTime_ms = lastTimeGpsReceived_ms; secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time // 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 // 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 // ideally we should be using a timing signal from the GPS receiver to set this time
// Use the driver specified delay // Use the driver specified delay
float gps_delay_sec = 0; 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); gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f);
// Correct for the average intersampling delay due to the filter updaterate // 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); gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms);
// Get which GPS we are using for position information // 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 // 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. // 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 // 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); float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha); gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw; float gpsSpdAccRaw;
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { if (!gps.speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f; gpsSpdAccuracy = 0.0f;
} else { } else {
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
@ -499,7 +501,7 @@ void NavEKF3_core::readGpsData()
} }
gpsPosAccuracy *= (1.0f - alpha); gpsPosAccuracy *= (1.0f - alpha);
float gpsPosAccRaw; float gpsPosAccRaw;
if (!_ahrs->get_gps().horizontal_accuracy(gpsPosAccRaw)) { if (!gps.horizontal_accuracy(gpsPosAccRaw)) {
gpsPosAccuracy = 0.0f; gpsPosAccuracy = 0.0f;
} else { } else {
gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
@ -507,7 +509,7 @@ void NavEKF3_core::readGpsData()
} }
gpsHgtAccuracy *= (1.0f - alpha); gpsHgtAccuracy *= (1.0f - alpha);
float gpsHgtAccRaw; float gpsHgtAccRaw;
if (!_ahrs->get_gps().vertical_accuracy(gpsHgtAccRaw)) { if (!gps.vertical_accuracy(gpsHgtAccRaw)) {
gpsHgtAccuracy = 0.0f; gpsHgtAccuracy = 0.0f;
} else { } else {
gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw); 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 // 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; 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; gpsNoiseScaler = 1.4f;
} else { // <= 4 satellites or in constant position mode } else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f; gpsNoiseScaler = 2.0f;
} }
// Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly // 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; useGpsVertVel = true;
} else { } else {
useGpsVertVel = false; useGpsVertVel = false;
@ -542,7 +544,7 @@ void NavEKF3_core::readGpsData()
calcGpsGoodForFlight(); calcGpsGoodForFlight();
// Read the GPS location in WGS-84 lat,long,height coordinates // 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 // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if (gpsGoodToAlign && !validOrigin) { 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 { } else {
// In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
if(validOrigin) { 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 // 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); Vector2f tempPosNE = location_diff(EKF_origin, gpsloc);
posNE.x = tempPosNE.x; posNE.x = tempPosNE.x;
posNE.y = tempPosNE.y; 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 // 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 bool NavEKF3_core::getLLH(struct Location &loc) const
{ {
const AP_GPS &gps = AP::gps();
if(validOrigin) { if(validOrigin) {
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
loc.alt = 100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z); loc.alt = 100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z);
@ -327,9 +329,9 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
} else { } else {
// we could be in constant position mode because the vehicle has taken off without GPS, or has lost GPS // 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 // 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 // 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.lat = gpsloc.lat;
loc.lng = gpsloc.lng; loc.lng = gpsloc.lng;
return true; return true;
@ -342,8 +344,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
} else { } else {
// If no origin has been defined for the EKF, then we cannot use its position states so return a raw // 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 // GPS reading if available and return false
if ((_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D)) { if ((gps.status() >= AP_GPS::GPS_OK_FIX_3D)) {
const struct Location &gpsloc = _ahrs->get_gps().location(); const struct Location &gpsloc = gps.location();
loc = gpsloc; loc = gpsloc;
loc.flags.relative_alt = 0; loc.flags.relative_alt = 0;
loc.flags.terrain_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 // Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) { if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// correct GPS data for position offset of antenna phase centre relative to the IMU // 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 (!posOffsetBody.is_zero()) {
if (fuseVelData) { if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay // 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 // Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary // 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 const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsesd since last update and limit to prevent numerical errors // 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); 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 // Check that the vertical GPS vertical velocity is reasonable after noise filtering
bool gpsVertVelFail; 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 // check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); 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 // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true; gpsVertVelFail = true;
// if we have a 3D fix with no vertical velocity and // 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 // EK3_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity // 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); frontend->_fusionModeGPS.set(1);
gcs().send_text(MAV_SEVERITY_WARNING, "EK3: Changed EK3_GPS_TYPE to 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 // fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f; float hAcc = 0.0f;
bool hAccFail; 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); hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
} else { } else {
hAccFail = false; hAccFail = false;
@ -144,7 +146,7 @@ bool NavEKF3_core::calcGpsGoodToAlign(void)
// Check for vertical GPS accuracy // Check for vertical GPS accuracy
float vAcc = 0.0f; float vAcc = 0.0f;
bool vAccFail = false; 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); vAccFail = (vAcc > 7.5f * checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
} }
// Report check result as a text string and bitmask // Report check result as a text string and bitmask
@ -171,24 +173,24 @@ bool NavEKF3_core::calcGpsGoodToAlign(void)
} }
// fail if satellite geometry is poor // 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 // Report check result as a text string and bitmask
if (hdopFail) { if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), 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; gpsCheckStatus.bad_hdop = true;
} else { } else {
gpsCheckStatus.bad_hdop = false; gpsCheckStatus.bad_hdop = false;
} }
// fail if not enough sats // 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 // Report check result as a text string and bitmask
if (numSatsFail) { if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), 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; gpsCheckStatus.bad_sats = true;
} else { } else {
gpsCheckStatus.bad_sats = false; gpsCheckStatus.bad_sats = false;
@ -250,7 +252,7 @@ void NavEKF3_core::calcGpsGoodForFlight(void)
// get the receivers reported speed accuracy // get the receivers reported speed accuracy
float gpsSpdAccRaw; float gpsSpdAccRaw;
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { if (!AP::gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccRaw = 0.0f; 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) { 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 // 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; 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) { if (AP_HAL::millis() - lastInitFailReport_ms > 10000) {
lastInitFailReport_ms = AP_HAL::millis(); lastInitFailReport_ms = AP_HAL::millis();
// provide an escalating series of messages // provide an escalating series of messages
@ -397,7 +397,7 @@ void NavEKF3_core::InitialiseVariables()
bool NavEKF3_core::InitialiseFilterBootstrap(void) bool NavEKF3_core::InitialiseFilterBootstrap(void)
{ {
// If we are a plane and don't have GPS lock then don't initialise // 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; statesInitialised = false;
return false; return false;
} }