Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: make reset source variable local

This commit is contained in:
chobits 2020-07-17 16:47:43 +08:00 committed by Randy Mackay
parent 9e06b78fda
commit 7ad9e3548f
5 changed files with 48 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -195,6 +195,9 @@ void NavEKF3_core::updateStateIndexLim()
// Set inertial navigation aiding mode
void NavEKF3_core::setAidingMode()
{
resetDataSource posResetSource = resetDataSource::DEFAULT;
resetDataSource velResetSource = resetDataSource::DEFAULT;
// Save the previous status so we can detect when it has changed
PV_AidingModePrev = PV_AidingMode;
@ -355,26 +358,23 @@ void NavEKF3_core::setAidingMode()
case AID_ABSOLUTE:
if (readyToUseGPS()) {
// We are commencing aiding using GPS - this is the preferred method
posResetSource = GPS;
velResetSource = GPS;
posResetSource = resetDataSource::GPS;
velResetSource = resetDataSource::GPS;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
} else if (readyToUseRangeBeacon()) {
// We are commencing aiding using range beacons
posResetSource = RNGBCN;
velResetSource = DEFAULT;
posResetSource = resetDataSource::RNGBCN;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z);
} else if (readyToUseExtNav()) {
// we are commencing aiding using external nav
posResetSource = EXTNAV;
posResetSource = resetDataSource::EXTNAV;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using external nav data",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z);
if (useExtNavVel) {
velResetSource = EXTNAV;
velResetSource = resetDataSource::EXTNAV;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial vel NED = %3.1f,%3.1f,%3.1f (m/s)",(unsigned)imu_index,(double)extNavVelDelayed.vel.x,(double)extNavVelDelayed.vel.y,(double)extNavVelDelayed.vel.z);
} else {
velResetSource = DEFAULT;
}
// handle height reset as special case
hgtMea = -extNavDataDelayed.pos.z;
@ -394,8 +394,8 @@ void NavEKF3_core::setAidingMode()
}
// Always reset the position and velocity when changing mode
ResetVelocity();
ResetPosition();
ResetVelocity(velResetSource);
ResetPosition(posResetSource);
}

10
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -158,10 +158,8 @@ void NavEKF3_core::realignYawGPS()
resetQuatStateYawOnly(gpsYaw, sq(radians(45.0f)));
// reset the velocity and position states as they will be inaccurate due to bad yaw
velResetSource = GPS;
ResetVelocity();
posResetSource = GPS;
ResetPosition();
ResetVelocity(resetDataSource::GPS);
ResetPosition(resetDataSource::GPS);
// send yaw alignment information to console
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
@ -1478,8 +1476,8 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
recordYawReset();
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
ResetVelocity();
ResetPosition();
ResetVelocity(resetDataSource::DEFAULT);
ResetPosition(resetDataSource::DEFAULT);
// reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset
velTestRatio = 0.0f;

42
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -16,7 +16,7 @@ extern const AP_HAL::HAL& hal;
// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
void NavEKF3_core::ResetVelocity(void)
void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
{
// Store the velocity before the reset so that we can record the reset delta
velResetNE.x = stateStruct.velocity.x;
@ -32,7 +32,7 @@ void NavEKF3_core::ResetVelocity(void)
P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise);
} else {
// reset horizontal velocity states to the GPS velocity if available
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == DEFAULT) || velResetSource == GPS) {
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::GPS) {
// correct for antenna position
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
@ -43,7 +43,7 @@ void NavEKF3_core::ResetVelocity(void)
// clear the timeout flags and counters
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == DEFAULT) || velResetSource == EXTNAV) {
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::EXTNAV) {
// use external nav data as the 2nd preference
// already corrected for sensor position
stateStruct.velocity.x = extNavVelDelayed.vel.x;
@ -76,14 +76,10 @@ void NavEKF3_core::ResetVelocity(void)
// store the time of the reset
lastVelReset_ms = imuSampleTime_ms;
// clear reset data source preference
velResetSource = DEFAULT;
}
// resets position states to last GPS measurement or to zero if in constant position mode
void NavEKF3_core::ResetPosition(void)
void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
{
// Store the position before the reset so that we can record the reset delta
posResetNE.x = stateStruct.position.x;
@ -101,7 +97,7 @@ void NavEKF3_core::ResetPosition(void)
P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise);
} else {
// Use GPS data as first preference if fresh data is available
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == DEFAULT) || posResetSource == GPS) {
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::GPS) {
// correct for antenna position
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
@ -115,7 +111,7 @@ void NavEKF3_core::ResetPosition(void)
// clear the timeout flags and counters
posTimeout = false;
lastPosPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == DEFAULT) || posResetSource == RNGBCN) {
} else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) {
// use the range beacon data as a second preference
stateStruct.position.x = receiverPos.x;
stateStruct.position.y = receiverPos.y;
@ -125,7 +121,7 @@ void NavEKF3_core::ResetPosition(void)
// clear the timeout flags and counters
rngBcnTimeout = false;
lastRngBcnPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == DEFAULT) || posResetSource == EXTNAV) {
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) {
// use external nav data as the third preference
ext_nav_elements extNavCorrected = extNavDataDelayed;
CorrectExtNavForSensorOffset(extNavCorrected.pos);
@ -153,10 +149,6 @@ void NavEKF3_core::ResetPosition(void)
// store the time of the reset
lastPosReset_ms = imuSampleTime_ms;
// clear reset source preference
posResetSource = DEFAULT;
}
// reset the stateStruct's NE position to the specified position
@ -700,13 +692,13 @@ void NavEKF3_core::FuseVelPosNED()
} else if (posHealth || posTimeout) {
posHealth = true;
lastPosPassTime_ms = imuSampleTime_ms;
// if timed out or outside the specified uncertainty radius, reset to the GPS
// if timed out or outside the specified uncertainty radius, reset to the external sensor
if (posTimeout || ((P[8][8] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
// reset the position to the current GPS position
ResetPosition();
// reset the velocity to the GPS velocity
ResetVelocity();
// don't fuse GPS data on this time step
// reset the position to the current external sensor position
ResetPosition(resetDataSource::DEFAULT);
// reset the velocity to the external sensor velocity
ResetVelocity(resetDataSource::DEFAULT);
// don't fuse external sensor data on this time step
fusePosData = false;
fuseVelData = false;
// Reset the position variances and corresponding covariances to a value that will pass the checks
@ -754,11 +746,11 @@ void NavEKF3_core::FuseVelPosNED()
velHealth = true;
// restart the timeout count
lastVelPassTime_ms = imuSampleTime_ms;
// If we are doing full aiding and velocity fusion times out, reset to the GPS velocity
// If we are doing full aiding and velocity fusion times out, reset to the external sensor velocity
if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
// reset the velocity to the GPS velocity
ResetVelocity();
// don't fuse GPS velocity data on this time step
// reset the velocity to the external sensor velocity
ResetVelocity(resetDataSource::DEFAULT);
// don't fuse external sensor velocity data on this time step
fuseVelData = false;
// Reset the normalised innovation to avoid failing the bad fusion tests
velTestRatio = 0.0f;

6
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -351,8 +351,6 @@ void NavEKF3_core::InitialiseVariables()
velOffsetNED.zero();
posOffsetNED.zero();
memset(&velPosObs, 0, sizeof(velPosObs));
posResetSource = DEFAULT;
velResetSource = DEFAULT;
// range beacon fusion variables
memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
@ -555,8 +553,8 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
stateStruct.body_magfield.zero();
// set the position, velocity and height
ResetVelocity();
ResetPosition();
ResetVelocity(resetDataSource::DEFAULT);
ResetPosition(resetDataSource::DEFAULT);
ResetHeight();
// define Earth rotation vector in the NED navigation frame

32
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -629,6 +629,19 @@ private:
Vector3f accel_bias;
} inactiveBias[INS_MAX_INSTANCES];
// Specify source of data to be used for a partial state reset
// Checking the availability and quality of the data source specified is the responsibility of the caller
enum class resetDataSource {
DEFAULT=0, // Use data source selected by reset function internal rules
GPS=1, // Use GPS
RNGBCN=2, // Use beacon range data
FLOW=3, // Use optical flow rates
BARO=4, // Use Baro height
MAG=5, // Use magnetometer data
RNGFND=6, // Use rangefinder data
EXTNAV=7 // Use external nav data
};
// update the navigation filter status
void updateFilterStatus(void);
@ -798,7 +811,7 @@ private:
void InitialiseVariablesMag();
// reset the horizontal position states uing the last GPS measurement
void ResetPosition(void);
void ResetPosition(resetDataSource posResetSource);
// reset the stateStruct's NE position to the specified position
void ResetPositionNE(float posN, float posE);
@ -807,7 +820,7 @@ private:
void ResetPositionD(float posD);
// reset velocity states using the last GPS measurement
void ResetVelocity(void);
void ResetVelocity(resetDataSource velResetSource);
// reset the vertical position state using the last height measurement
void ResetHeight(void);
@ -1149,21 +1162,6 @@ private:
uint32_t firstInitTime_ms; // First time the initialise function was called (msec)
uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec)
// Specify source of data to be used for a partial state reset
// Checking the availability and quality of the data source specified is the responsibility of the caller
enum resetDataSource {
DEFAULT=0, // Use data source selected by reset function internal rules
GPS=1, // Use GPS
RNGBCN=2, // Use beacon range data
FLOW=3, // Use optical flow rates
BARO=4, // Use Baro height
MAG=5, // Use magnetometer data
RNGFND=6, // Use rangefinder data
EXTNAV=7 // Use external nav data
};
resetDataSource posResetSource; // preferred source of data for position reset
resetDataSource velResetSource; // preferred source of data for a velocity reset
// variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
struct {
float pos;