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

Guard against invalid states

This commit is contained in:
Lorenz Meier 2014-03-24 10:29:13 +01:00
parent 8666ca53bf
commit 0022bbb5fb
2 changed files with 53 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
src/modules/fw_att_pos_estimator/estimator.cpp
View File

@ -1674,34 +1674,49 @@ void ResetStoredStates()
}
// Output the state vector stored at the time that best matches that specified by msec
void RecallStates(float statesForFusion[n_states], uint64_t msec)
int RecallStates(float statesForFusion[n_states], uint64_t msec)
{
// int64_t bestTimeDelta = 200;
// unsigned bestStoreIndex = 0;
// for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
// {
// // The time delta can also end up as negative number,
// // since we might compare future to past or past to future
// // therefore cast to int64.
// int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex];
// if (timeDelta < 0) {
// timeDelta = -timeDelta;
// }
int ret = 0;
// if (timeDelta < bestTimeDelta)
// {
// bestStoreIndex = storeIndex;
// bestTimeDelta = timeDelta;
// }
// }
// if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
// {
// for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex];
// }
// else // otherwise output current state
// {
for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i];
// }
int64_t bestTimeDelta = 200;
unsigned bestStoreIndex = 0;
for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
{
// The time delta can also end up as negative number,
// since we might compare future to past or past to future
// therefore cast to int64.
int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex];
if (timeDelta < 0) {
timeDelta = -timeDelta;
}
if (timeDelta < bestTimeDelta)
{
bestStoreIndex = storeIndex;
bestTimeDelta = timeDelta;
}
}
if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
{
for (uint8_t i=0; i < n_states; i++) {
if (isfinite(storedStates[i][bestStoreIndex])) {
statesForFusion[i] = storedStates[i][bestStoreIndex];
} else if (isfinite(states[i])) {
statesForFusion[i] = states[i];
} else {
// There is not much we can do here, except reporting the error we just
// found.
ret++;
}
}
else // otherwise output current state
{
for (uint8_t i=0; i < n_states; i++) {
statesForFusion[i] = states[i];
}
}
return ret;
}
void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])

14
src/modules/fw_att_pos_estimator/estimator.h
View File

@ -171,8 +171,18 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
// recall stste vector stored at closest time to the one specified by msec
void RecallStates(float statesForFusion[n_states], uint64_t msec);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
*
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float statesForFusion[n_states], uint64_t msec);
void ResetStoredStates();