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

AP_NavEKF3: integrate Source for position

Browse Source
This commit is contained in:
Randy Mackay 2020-06-17 20:28:09 +09:00
parent 9b84abecaa
commit c21d58ebea
9 changed files with 101 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -129,12 +129,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// GPS measurement parameters
// @Param: GPS_TYPE
// @DisplayName: GPS mode control
// @Description: This controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = Inhibit GPS use - this can be useful when flying with an optical flow sensor in an environment where GPS quality is poor and subject to large multipath errors.
// @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS
// @User: Advanced
AP_GROUPINFO("GPS_TYPE", 1, NavEKF3, _fusionModeGPS, 0),
// 1 was GPS_TYPE
// @Param: VELNE_M_NSE
// @DisplayName: GPS horizontal velocity measurement noise (m/s)
@ -650,6 +645,10 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @RebootRequired: True
AP_GROUPINFO("AFFINITY", 62, NavEKF3, _affinity, 0),
// @Group: SRC_
// @Path: ../AP_NavEKF/AP_NavEKF_Source.cpp
AP_SUBGROUPINFO(_sources, "SRC", 63, NavEKF3, AP_NavEKF_Source),
AP_GROUPEND
};
@ -677,6 +676,9 @@ bool NavEKF3::InitialiseFilter(void)
// expected number of IMU frames per prediction
_framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));
// initialise sources
_sources.init();
#if APM_BUILD_TYPE(APM_BUILD_Replay)
if (ins.get_accel_count() == 0) {
return false;
@ -993,6 +995,12 @@ void NavEKF3::resetCoreErrors(void)
}
}
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void NavEKF3::setPosVelYawSource(uint8_t source_set_idx)
{
_sources.setPosVelYawSource(source_set_idx);
}
// Check basic filter health metrics and return a consolidated health status
bool NavEKF3::healthy(void) const
{
@ -1308,10 +1316,9 @@ bool NavEKF3::setOriginLLH(const Location &loc)
if (!core) {
return false;
}
if (_fusionModeGPS != 3) {
// we don't allow setting of the EKF origin unless we are
// flying in non-GPS mode. This is to prevent accidental set
// of EKF origin with invalid position or height
if (_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) {
// we don't allow setting of the EKF origin if using GPS to prevent
// accidental setting of EKF origin with invalid position or height
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 refusing set origin");
return false;
}

7
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -24,6 +24,7 @@
#include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_NavEKF/AP_Nav_Common.h>
#include <AP_NavEKF/AP_NavEKF_Source.h>
class NavEKF3_core;
@ -406,6 +407,9 @@ public:
*/
void requestYawReset(void);
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void setPosVelYawSource(uint8_t source_set_idx);
// write EKF information to on-board logs
void Log_Write();
@ -444,7 +448,6 @@ private:
AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
AP_Int16 _hgtDelay_ms; // effective average delay of Height measurements relative to inertial measurements (msec)
AP_Int8 _fusionModeGPS; // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity, 3 = do not use GPS
AP_Int16 _gpsVelInnovGate; // Percentage number of standard deviations applied to GPS velocity innovation consistency check
AP_Int16 _gpsPosInnovGate; // Percentage number of standard deviations applied to GPS position innovation consistency check
AP_Int16 _hgtInnovGate; // Percentage number of standard deviations applied to height innovation consistency check
@ -615,4 +618,6 @@ private:
void Log_Write_Timing(uint8_t core, uint64_t time_us) const;
void Log_Write_GSF(uint8_t core, uint64_t time_us) const;
// position, velocity and yaw source control
AP_NavEKF_Source _sources;
};

61
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -396,7 +396,6 @@ void NavEKF3_core::setAidingMode()
// Always reset the position and velocity when changing mode
ResetVelocity(velResetSource);
ResetPosition(posResetSource);
}
}
@ -461,7 +460,11 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const
// return true if the filter to be ready to use gps
bool NavEKF3_core::readyToUseGPS(void) const
{
return validOrigin && tiltAlignComplete && yawAlignComplete && (delAngBiasLearned || assume_zero_sideslip()) && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse && !gpsInhibit;
if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS) {
return false;
}
return validOrigin && tiltAlignComplete && yawAlignComplete && (delAngBiasLearned || assume_zero_sideslip()) && gpsGoodToAlign && gpsDataToFuse && !gpsInhibit;
}
// return true if the filter to be ready to use the beacon range measurements
@ -509,10 +512,11 @@ bool NavEKF3_core::assume_zero_sideslip(void) const
// set the LLH location of the filters NED origin
bool NavEKF3_core::setOriginLLH(const Location &loc)
{
if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3)) {
if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) {
// reject attempt to set origin if GPS is being used
return false;
}
EKF_origin = loc;
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
@ -613,32 +617,51 @@ void NavEKF3_core::updateFilterStatus(void)
filterStatus.flags.takeoff = expectTakeoff; // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator
filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3); // GPS glitching is affecting navigation accuracy
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy
filterStatus.flags.gps_quality_good = gpsGoodToAlign;
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
}
void NavEKF3_core::runYawEstimatorPrediction()
{
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
float trueAirspeed;
if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {
if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
trueAirspeed = tasDataDelayed.tas;
} else {
trueAirspeed = defaultAirSpeed * dal.get_EAS2TAS();
}
} else {
trueAirspeed = 0.0f;
}
yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
// exit immediately if no yaw estimator
if (yawEstimator == nullptr) {
return;
}
// ensure GPS is used for horizontal position and velocity
if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS ||
frontend->_sources.getVelXYSource() != AP_NavEKF_Source::SourceXY::GPS) {
return;
}
float trueAirspeed;
if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {
if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
trueAirspeed = tasDataDelayed.tas;
} else {
trueAirspeed = defaultAirSpeed * dal.get_EAS2TAS();
}
} else {
trueAirspeed = 0.0f;
}
yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
}
void NavEKF3_core::runYawEstimatorCorrection()
{
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1 && EKFGSF_run_filterbank) {
// exit immediately if no yaw estimator
if (yawEstimator == nullptr) {
return;
}
// ensure GPS is used for horizontal position and velocity
if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS ||
frontend->_sources.getVelXYSource() != AP_NavEKF_Source::SourceXY::GPS) {
return;
}
if (EKFGSF_run_filterbank) {
if (gpsDataToFuse) {
Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
@ -658,6 +681,8 @@ void NavEKF3_core::runYawEstimatorCorrection()
} else {
EKFGSF_yaw_valid_count = 0;
}
} else {
EKFGSF_yaw_valid_count = 0;
}
// action an external reset request

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -616,7 +616,7 @@ void NavEKF3_core::readGpsData()
}
// Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
if (gps.have_vertical_velocity(selected_gps) && (frontend->_fusionModeGPS == 0) && !frontend->inhibitGpsVertVelUse) {
if (gps.have_vertical_velocity(selected_gps) && (frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -119,7 +119,7 @@ bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl
bool NavEKF3_core::getHeightControlLimit(float &height) const
{
// only ask for limiting if we are doing optical flow navigation
if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
if ((frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::OPTFLOW) && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
// If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
const auto *_rng = dal.rangefinder();
if (_rng == nullptr) {

32
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -245,7 +245,7 @@ void NavEKF3_core::ResetHeight(void)
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
// Check that GPS vertical velocity data is available and can be used
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
if (inFlight && !gpsNotAvailable && (frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
stateStruct.velocity.z = extNavVelDelayed.vel.z;
@ -424,19 +424,22 @@ void NavEKF3_core::SelectVelPosFusion()
CorrectGPSForAntennaOffset(gpsDataDelayed);
}
// detect position source changes. Trigger position reset if position source is valid
AP_NavEKF_Source::SourceXY pos_source = frontend->_sources.getPosXYSource();
if (pos_source != pos_source_last) {
pos_source_reset = (pos_source != AP_NavEKF_Source::SourceXY::NONE);
pos_source_last = pos_source;
}
// initialise all possible data we may fuse
fusePosData = false;
fuseVelData = false;
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3)) {
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) {
// Don't fuse velocity data if GPS doesn't support it
if (frontend->_fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
}
fuseVelData = frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::GPS;
fusePosData = true;
extNavUsedForPos = false;
@ -449,7 +452,7 @@ void NavEKF3_core::SelectVelPosFusion()
}
velPosObs[3] = gpsDataDelayed.pos.x;
velPosObs[4] = gpsDataDelayed.pos.y;
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS == 3)) {
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV)) {
// use external nav system for horizontal position
extNavUsedForPos = true;
fusePosData = true;
@ -459,7 +462,7 @@ void NavEKF3_core::SelectVelPosFusion()
// fuse external navigation velocity data if available
// extNavVelDelayed is already corrected for sensor position
if (extNavVelToFuse && (frontend->_fusionModeGPS == 3)) {
if (extNavVelToFuse && (frontend->_sources.getVelXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV)) {
fuseVelData = true;
velPosObs[0] = extNavVelDelayed.vel.x;
velPosObs[1] = extNavVelDelayed.vel.y;
@ -475,7 +478,10 @@ void NavEKF3_core::SelectVelPosFusion()
selectHeightForFusion();
// if we are using GPS, check for a change in receiver and reset position and height
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3) && (gpsDataDelayed.sensor_idx != last_gps_idx)) {
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) && (gpsDataDelayed.sensor_idx != last_gps_idx || pos_source_reset)) {
// mark a source reset as consumed
pos_source_reset = false;
// record the ID of the GPS that we are using for the reset
last_gps_idx = gpsDataDelayed.sensor_idx;
@ -489,7 +495,9 @@ void NavEKF3_core::SelectVelPosFusion()
}
// check for external nav position reset
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS == 3) && extNavDataDelayed.posReset) {
if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || pos_source_reset)) {
// mark a source reset as consumed
pos_source_reset = false;
ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y);
if (activeHgtSource == HGT_SOURCE_EXTNAV) {
ResetPositionD(-hgtMea);
@ -689,7 +697,7 @@ void NavEKF3_core::FuseVelPosNED()
// test velocity measurements
uint8_t imax = 2;
// Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
if ((frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) {
if ((frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::NONE || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations

6
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -83,14 +83,14 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
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) && !gps.have_vertical_velocity(preferred_gps)) {
} else if ((frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::GPS) && !gps.have_vertical_velocity(preferred_gps)) {
// 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 (gps.status(preferred_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {
frontend->_fusionModeGPS.set(1);
if (gps.status(preferred_gps) >= AP_GPS::GPS_OK_FIX_3D) {
frontend->_sources.setVelZSource(AP_NavEKF_Source::SourceZ::NONE);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EK3: Changed EK3_GPS_TYPE to 1");
}
} else {

5
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -46,7 +46,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
))));
// GPS sensing can have large delays and should not be included if disabled
if (frontend->_fusionModeGPS != 3) {
if (frontend->_sources.usingGPS()) {
// 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 (!dal.gps().get_lag(selected_gps, gps_delay_sec)) {
@ -533,6 +533,9 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
ResetPosition(resetDataSource::DEFAULT);
ResetHeight();
// initialise the position source
pos_source_last = frontend->_sources.getPosXYSource();
// define Earth rotation vector in the NED navigation frame
calcEarthRateNED(earthRateNED, dal.get_home().lat);

6
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -28,6 +28,7 @@
#include <AP_Math/AP_Math.h>
#include <AP_Math/vectorN.h>
#include <AP_NavEKF/AP_NavEKF_core_common.h>
#include <AP_NavEKF/AP_NavEKF_Source.h>
#include <AP_NavEKF3/AP_NavEKF3_Buffer.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
@ -1478,4 +1479,9 @@ private:
uint8_t preferred_gps;
uint8_t selected_baro;
uint8_t selected_airspeed;
// source reset handling
AP_NavEKF_Source::SourceXY pos_source_last; // position source on previous iteration (used to detect a changes)
bool pos_source_reset; // true when the position source has changed but the position has not yet been reset
};