mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 00:28:30 -04:00
AP_NavEKF2: remove unused state variables
This commit is contained in:
parent
a1e7c91f8e
commit
0496ecc4f6
@ -196,7 +196,6 @@ void NavEKF2_core::InitialiseVariables()
|
||||
inhibitGndState = false;
|
||||
flowGyroBias.x = 0;
|
||||
flowGyroBias.y = 0;
|
||||
heldVelNE.zero();
|
||||
PV_AidingMode = AID_NONE;
|
||||
PV_AidingModePrev = AID_NONE;
|
||||
posTimeout = true;
|
||||
@ -226,12 +225,6 @@ void NavEKF2_core::InitialiseVariables()
|
||||
tiltErrFilt = 1.0f;
|
||||
tiltAlignComplete = false;
|
||||
stateIndexLim = 23;
|
||||
baroStoreIndex = 0;
|
||||
rangeStoreIndex = 0;
|
||||
magStoreIndex = 0;
|
||||
gpsStoreIndex = 0;
|
||||
tasStoreIndex = 0;
|
||||
ofStoreIndex = 0;
|
||||
delAngCorrection.zero();
|
||||
velErrintegral.zero();
|
||||
posErrintegral.zero();
|
||||
@ -288,7 +281,6 @@ void NavEKF2_core::InitialiseVariables()
|
||||
// range beacon fusion variables
|
||||
memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
|
||||
memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed));
|
||||
rngBcnStoreIndex = 0;
|
||||
lastRngBcnPassTime_ms = 0;
|
||||
rngBcnTestRatio = 0.0f;
|
||||
rngBcnHealth = false;
|
||||
|
@ -938,18 +938,13 @@ private:
|
||||
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 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 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 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
|
||||
uint8_t magStoreIndex; // Magnetometer data storage index
|
||||
gps_elements gpsDataNew; // GPS data at the current 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 outputDataDelayed; // output state data at the current time step
|
||||
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
|
||||
of_elements ofDataNew; // OF data at the current 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 flowDataValid; // true while optical flow data is still fresh
|
||||
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 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
|
||||
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.
|
||||
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
|
||||
@ -1083,7 +1076,6 @@ private:
|
||||
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 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)
|
||||
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
|
||||
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
|
||||
|
@ -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
|
||||
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
||||
magFusePerformed = true;
|
||||
magFuseRequired = true;
|
||||
|
||||
} else if (obsIndex == 1) { // Fuse Y axis
|
||||
|
||||
// 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
|
||||
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
|
||||
magFusePerformed = true;
|
||||
magFuseRequired = true;
|
||||
}
|
||||
else if (obsIndex == 2) // we are now fusing the Z measurement
|
||||
{
|
||||
|
@ -260,7 +260,6 @@ void NavEKF3_core::InitialiseVariables()
|
||||
inhibitGndState = false;
|
||||
flowGyroBias.x = 0;
|
||||
flowGyroBias.y = 0;
|
||||
heldVelNE.zero();
|
||||
PV_AidingMode = AID_NONE;
|
||||
PV_AidingModePrev = AID_NONE;
|
||||
posTimeout = true;
|
||||
@ -292,11 +291,7 @@ void NavEKF3_core::InitialiseVariables()
|
||||
yawAlignComplete = false;
|
||||
have_table_earth_field = false;
|
||||
stateIndexLim = 23;
|
||||
baroStoreIndex = 0;
|
||||
rangeStoreIndex = 0;
|
||||
last_gps_idx = 0;
|
||||
tasStoreIndex = 0;
|
||||
ofStoreIndex = 0;
|
||||
delAngCorrection.zero();
|
||||
velErrintegral.zero();
|
||||
posErrintegral.zero();
|
||||
@ -353,7 +348,6 @@ void NavEKF3_core::InitialiseVariables()
|
||||
|
||||
// range beacon fusion variables
|
||||
memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed));
|
||||
rngBcnStoreIndex = 0;
|
||||
lastRngBcnPassTime_ms = 0;
|
||||
rngBcnTestRatio = 0.0f;
|
||||
rngBcnHealth = false;
|
||||
@ -459,7 +453,6 @@ void NavEKF3_core::InitialiseVariablesMag()
|
||||
mag_state.q0 = 1;
|
||||
mag_state.DCM.identity();
|
||||
inhibitMagStates = true;
|
||||
magStoreIndex = 0;
|
||||
magSelectIndex = 0;
|
||||
lastMagOffsetsValid = false;
|
||||
magStateResetRequest = false;
|
||||
|
@ -1017,7 +1017,6 @@ private:
|
||||
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.
|
||||
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
|
||||
uint32_t prevTasStep_ms; // time stamp of last TAS 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
|
||||
baro_elements baroDataNew; // Baro data at the current 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 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 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
|
||||
uint8_t magStoreIndex; // Magnetometer data storage index
|
||||
gps_elements gpsDataNew; // GPS data at the current 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
|
||||
@ -1170,7 +1165,6 @@ private:
|
||||
obs_ring_buffer_t<of_elements> storedOF; // OF data buffer
|
||||
of_elements ofDataNew; // OF data at the current 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
|
||||
Vector2f auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
|
||||
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 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
|
||||
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.
|
||||
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
|
||||
@ -1248,7 +1241,6 @@ private:
|
||||
// Range Beacon Sensor Fusion
|
||||
obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
|
||||
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)
|
||||
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
|
||||
bool rngBcnHealth; // boolean true if range beacon measurements have passed innovation consistency check
|
||||
|
Loading…
Reference in New Issue
Block a user