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_NavEKF2: remove unused state variables

Browse Source
This commit is contained in:
Peter Barker 2020-10-21 12:55:09 +11:00 committed by Peter Barker
parent a1e7c91f8e
commit 0496ecc4f6
5 changed files with 0 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -196,7 +196,6 @@ void NavEKF2_core::InitialiseVariables()
inhibitGndState = false; inhibitGndState = false;
flowGyroBias.x = 0; flowGyroBias.x = 0;
flowGyroBias.y = 0; flowGyroBias.y = 0;
heldVelNE.zero();
PV_AidingMode = AID_NONE; PV_AidingMode = AID_NONE;
PV_AidingModePrev = AID_NONE; PV_AidingModePrev = AID_NONE;
posTimeout = true; posTimeout = true;
@ -226,12 +225,6 @@ void NavEKF2_core::InitialiseVariables()
tiltErrFilt = 1.0f; tiltErrFilt = 1.0f;
tiltAlignComplete = false; tiltAlignComplete = false;
stateIndexLim = 23; stateIndexLim = 23;
baroStoreIndex = 0;
rangeStoreIndex = 0;
magStoreIndex = 0;
gpsStoreIndex = 0;
tasStoreIndex = 0;
ofStoreIndex = 0;
delAngCorrection.zero(); delAngCorrection.zero();
velErrintegral.zero(); velErrintegral.zero();
posErrintegral.zero(); posErrintegral.zero();
@ -288,7 +281,6 @@ void NavEKF2_core::InitialiseVariables()
// range beacon fusion variables // range beacon fusion variables
memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew)); memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed)); memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed));
rngBcnStoreIndex = 0;
lastRngBcnPassTime_ms = 0; lastRngBcnPassTime_ms = 0;
rngBcnTestRatio = 0.0f; rngBcnTestRatio = 0.0f;
rngBcnHealth = false; rngBcnHealth = false;

8
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -938,18 +938,13 @@ private:
uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon
baro_elements baroDataNew; // Baro data at the current time horizon baro_elements baroDataNew; // Baro data at the current time horizon
baro_elements baroDataDelayed; // Baro data at the fusion time horizon baro_elements baroDataDelayed; // Baro data at the fusion time horizon
uint8_t baroStoreIndex; // Baro data storage index
range_elements rangeDataNew; // Range finder data at the current time horizon range_elements rangeDataNew; // Range finder data at the current time horizon
range_elements rangeDataDelayed;// Range finder data at the fusion time horizon range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
uint8_t rangeStoreIndex; // Range finder data storage index
tas_elements tasDataNew; // TAS data at the current time horizon tas_elements tasDataNew; // TAS data at the current time horizon
tas_elements tasDataDelayed; // TAS data at the fusion time horizon tas_elements tasDataDelayed; // TAS data at the fusion time horizon
uint8_t tasStoreIndex; // TAS data storage index
mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
uint8_t magStoreIndex; // Magnetometer data storage index
gps_elements gpsDataNew; // GPS data at the current time horizon gps_elements gpsDataNew; // GPS data at the current time horizon
gps_elements gpsDataDelayed; // GPS data at the fusion time horizon gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
uint8_t gpsStoreIndex; // GPS data storage index
output_elements outputDataNew; // output state data at the current time step output_elements outputDataNew; // output state data at the current time step
output_elements outputDataDelayed; // output state data at the current time step output_elements outputDataDelayed; // output state data at the current time step
Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF Vector3f delAngCorrection; // correction applied to delta angles used by output observer to track the EKF
@ -1027,7 +1022,6 @@ private:
obs_ring_buffer_t<of_elements> storedOF; // OF data buffer obs_ring_buffer_t<of_elements> storedOF; // OF data buffer
of_elements ofDataNew; // OF data at the current time horizon of_elements ofDataNew; // OF data at the current time horizon
of_elements ofDataDelayed; // OF data at the fusion time horizon of_elements ofDataDelayed; // OF data at the fusion time horizon
uint8_t ofStoreIndex; // OF data storage index
bool flowDataToFuse; // true when optical flow data is ready for fusion bool flowDataToFuse; // true when optical flow data is ready for fusion
bool flowDataValid; // true while optical flow data is still fresh bool flowDataValid; // true while optical flow data is still fresh
Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
@ -1056,7 +1050,6 @@ private:
bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon. bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon.
bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon. bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon.
bool magDataToFuse; // true when valid magnetometer data has arrived at the fusion time horizon bool magDataToFuse; // true when valid magnetometer data has arrived at the fusion time horizon
Vector2f heldVelNE; // velocity held when no aiding is available
enum AidingMode {AID_ABSOLUTE=0, // GPS or some other form of absolute position reference aiding is being used (optical flow may also be used in parallel) so position estimates are absolute. enum AidingMode {AID_ABSOLUTE=0, // GPS or some other form of absolute position reference aiding is being used (optical flow may also be used in parallel) so position estimates are absolute.
AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift. AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative
@ -1083,7 +1076,6 @@ private:
obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
uint8_t rngBcnStoreIndex; // Range beacon data storage index
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innvovation consistency checks (msec) uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innvovation consistency checks (msec)
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check

3
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -638,8 +638,6 @@ void NavEKF3_core::FuseMagnetometer()
// set flags to indicate to other processes that fusion has been performed and is required on the next frame // set flags to indicate to other processes that fusion has been performed and is required on the next frame
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true; magFusePerformed = true;
magFuseRequired = true;
} else if (obsIndex == 1) { // Fuse Y axis } else if (obsIndex == 1) { // Fuse Y axis
// calculate observation jacobians // calculate observation jacobians
@ -716,7 +714,6 @@ void NavEKF3_core::FuseMagnetometer()
// set flags to indicate to other processes that fusion has been performed and is required on the next frame // set flags to indicate to other processes that fusion has been performed and is required on the next frame
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true; magFusePerformed = true;
magFuseRequired = true;
} }
else if (obsIndex == 2) // we are now fusing the Z measurement else if (obsIndex == 2) // we are now fusing the Z measurement
{ {

7
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -260,7 +260,6 @@ void NavEKF3_core::InitialiseVariables()
inhibitGndState = false; inhibitGndState = false;
flowGyroBias.x = 0; flowGyroBias.x = 0;
flowGyroBias.y = 0; flowGyroBias.y = 0;
heldVelNE.zero();
PV_AidingMode = AID_NONE; PV_AidingMode = AID_NONE;
PV_AidingModePrev = AID_NONE; PV_AidingModePrev = AID_NONE;
posTimeout = true; posTimeout = true;
@ -292,11 +291,7 @@ void NavEKF3_core::InitialiseVariables()
yawAlignComplete = false; yawAlignComplete = false;
have_table_earth_field = false; have_table_earth_field = false;
stateIndexLim = 23; stateIndexLim = 23;
baroStoreIndex = 0;
rangeStoreIndex = 0;
last_gps_idx = 0; last_gps_idx = 0;
tasStoreIndex = 0;
ofStoreIndex = 0;
delAngCorrection.zero(); delAngCorrection.zero();
velErrintegral.zero(); velErrintegral.zero();
posErrintegral.zero(); posErrintegral.zero();
@ -353,7 +348,6 @@ void NavEKF3_core::InitialiseVariables()
// range beacon fusion variables // range beacon fusion variables
memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed)); memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed));
rngBcnStoreIndex = 0;
lastRngBcnPassTime_ms = 0; lastRngBcnPassTime_ms = 0;
rngBcnTestRatio = 0.0f; rngBcnTestRatio = 0.0f;
rngBcnHealth = false; rngBcnHealth = false;
@ -459,7 +453,6 @@ void NavEKF3_core::InitialiseVariablesMag()
mag_state.q0 = 1; mag_state.q0 = 1;
mag_state.DCM.identity(); mag_state.DCM.identity();
inhibitMagStates = true; inhibitMagStates = true;
magStoreIndex = 0;
magSelectIndex = 0; magSelectIndex = 0;
lastMagOffsetsValid = false; lastMagOffsetsValid = false;
magStateResetRequest = false; magStateResetRequest = false;

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1017,7 +1017,6 @@ private:
ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements
float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive. float defaultAirSpeed; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step
bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step
MagCal effectiveMagCal; // the actual mag calibration and yaw fusion method being used as the default MagCal effectiveMagCal; // the actual mag calibration and yaw fusion method being used as the default
uint32_t prevTasStep_ms; // time stamp of last TAS fusion step uint32_t prevTasStep_ms; // time stamp of last TAS fusion step
uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step uint32_t prevBetaStep_ms; // time stamp of last synthetic sideslip fusion step
@ -1079,15 +1078,11 @@ private:
uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon
baro_elements baroDataNew; // Baro data at the current time horizon baro_elements baroDataNew; // Baro data at the current time horizon
baro_elements baroDataDelayed; // Baro data at the fusion time horizon baro_elements baroDataDelayed; // Baro data at the fusion time horizon
uint8_t baroStoreIndex; // Baro data storage index
range_elements rangeDataNew; // Range finder data at the current time horizon range_elements rangeDataNew; // Range finder data at the current time horizon
range_elements rangeDataDelayed;// Range finder data at the fusion time horizon range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
uint8_t rangeStoreIndex; // Range finder data storage index
tas_elements tasDataNew; // TAS data at the current time horizon tas_elements tasDataNew; // TAS data at the current time horizon
tas_elements tasDataDelayed; // TAS data at the fusion time horizon tas_elements tasDataDelayed; // TAS data at the fusion time horizon
uint8_t tasStoreIndex; // TAS data storage index
mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon
uint8_t magStoreIndex; // Magnetometer data storage index
gps_elements gpsDataNew; // GPS data at the current time horizon gps_elements gpsDataNew; // GPS data at the current time horizon
gps_elements gpsDataDelayed; // GPS data at the fusion time horizon gps_elements gpsDataDelayed; // GPS data at the fusion time horizon
uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset uint8_t last_gps_idx; // sensor ID of the GPS receiver used for the last fusion or reset
@ -1170,7 +1165,6 @@ private:
obs_ring_buffer_t<of_elements> storedOF; // OF data buffer obs_ring_buffer_t<of_elements> storedOF; // OF data buffer
of_elements ofDataNew; // OF data at the current time horizon of_elements ofDataNew; // OF data at the current time horizon
of_elements ofDataDelayed; // OF data at the fusion time horizon of_elements ofDataDelayed; // OF data at the fusion time horizon
uint8_t ofStoreIndex; // OF data storage index
bool flowDataValid; // true while optical flow data is still fresh bool flowDataValid; // true while optical flow data is still fresh
Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec) uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
@ -1198,7 +1192,6 @@ private:
bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon. bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon.
bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon. bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon.
bool magDataToFuse; // true when valid magnetometer data has arrived at the fusion time horizon bool magDataToFuse; // true when valid magnetometer data has arrived at the fusion time horizon
Vector2f heldVelNE; // velocity held when no aiding is available
enum AidingMode {AID_ABSOLUTE=0, // GPS or some other form of absolute position reference aiding is being used (optical flow may also be used in parallel) so position estimates are absolute. enum AidingMode {AID_ABSOLUTE=0, // GPS or some other form of absolute position reference aiding is being used (optical flow may also be used in parallel) so position estimates are absolute.
AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift. AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative
@ -1248,7 +1241,6 @@ private:
// Range Beacon Sensor Fusion // Range Beacon Sensor Fusion
obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
uint8_t rngBcnStoreIndex; // Range beacon data storage index
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec) uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check