dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AttPosEKF: Reset states to current state

This commit is contained in:
Johan Jansen 2015-03-12 09:41:38 +01:00
parent 588edd794d
commit 5f5a6e841f
1 changed files with 15 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -2329,15 +2329,20 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE
// Store states in a history array along with time stamp // Store states in a history array along with time stamp
void AttPosEKF::StoreStates(uint64_t timestamp_ms) void AttPosEKF::StoreStates(uint64_t timestamp_ms)
{ {
for (size_t i=0; i<EKF_STATE_ESTIMATES; i++) for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
storedStates[i][storeIndex] = states[i]; storedStates[i][storeIndex] = states[i];
}
storedOmega[0][storeIndex] = angRate.x; storedOmega[0][storeIndex] = angRate.x;
storedOmega[1][storeIndex] = angRate.y; storedOmega[1][storeIndex] = angRate.y;
storedOmega[2][storeIndex] = angRate.z; storedOmega[2][storeIndex] = angRate.z;
statetimeStamp[storeIndex] = timestamp_ms; statetimeStamp[storeIndex] = timestamp_ms;
// increment to next storage index
storeIndex++; storeIndex++;
if (storeIndex == EKF_DATA_BUFFER_SIZE) if (storeIndex >= EKF_DATA_BUFFER_SIZE) {
storeIndex = 0; storeIndex = 0;
}
} }
void AttPosEKF::ResetStoredStates() void AttPosEKF::ResetStoredStates()
@ -2350,10 +2355,8 @@ void AttPosEKF::ResetStoredStates()
// reset store index to first // reset store index to first
storeIndex = 0; storeIndex = 0;
statetimeStamp[storeIndex] = millis(); //Reset stored state to current state
StoreStates(millis());
// increment to next storage index
storeIndex++;
} }
// Output the state vector stored at the time that best matches that specified by msec // Output the state vector stored at the time that best matches that specified by msec
@ -2643,9 +2646,11 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
ret = val; ret = val;
} }
#if 0
if (!isfinite(val)) { if (!isfinite(val)) {
//ekf_debug("constrain: non-finite!"); ekf_debug("constrain: non-finite!");
} }
#endif
return ret; return ret;
} }
@ -3040,10 +3045,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
// Fill error report // Fill error report
GetFilterState(&last_ekf_error); GetFilterState(&last_ekf_error);
ResetStoredStates();
ResetVelocity(); ResetVelocity();
ResetPosition(); ResetPosition();
ResetHeight(); ResetHeight();
ResetStoredStates();
// Timeout cleared with this reset // Timeout cleared with this reset
current_ekf_state.imuTimeout = false; current_ekf_state.imuTimeout = false;
@ -3057,10 +3062,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
// Fill error report, but not setting error flag // Fill error report, but not setting error flag
GetFilterState(&last_ekf_error); GetFilterState(&last_ekf_error);
ResetStoredStates();
ResetVelocity(); ResetVelocity();
ResetPosition(); ResetPosition();
ResetHeight(); ResetHeight();
ResetStoredStates();
ret = 0; ret = 0;
} }
@ -3230,10 +3235,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
states[20] = magBias.y; // Magnetic Field Bias Y states[20] = magBias.y; // Magnetic Field Bias Y
states[21] = magBias.z; // Magnetic Field Bias Z states[21] = magBias.z; // Magnetic Field Bias Z
ResetStoredStates();
ResetVelocity(); ResetVelocity();
ResetPosition(); ResetPosition();
ResetHeight(); ResetHeight();
ResetStoredStates();
// initialise focal length scale factor estimator states // initialise focal length scale factor estimator states
flowStates[0] = 1.0f; flowStates[0] = 1.0f;
@ -3245,7 +3250,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
//Define Earth rotation vector in the NED navigation frame //Define Earth rotation vector in the NED navigation frame
calcEarthRateNED(earthRateNED, latRef); calcEarthRateNED(earthRateNED, latRef);
} }
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
@ -3337,13 +3341,6 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
current_ekf_state.useAirspeed = useAirspeed; current_ekf_state.useAirspeed = useAirspeed;
memcpy(err, &current_ekf_state, sizeof(*err)); memcpy(err, &current_ekf_state, sizeof(*err));
// err->velHealth = current_ekf_state.velHealth;
// err->posHealth = current_ekf_state.posHealth;
// err->hgtHealth = current_ekf_state.hgtHealth;
// err->velTimeout = current_ekf_state.velTimeout;
// err->posTimeout = current_ekf_state.posTimeout;
// err->hgtTimeout = current_ekf_state.hgtTimeout;
} }
void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)