Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

Now that the guard is updated disable time compensation again, but keep a guard against invalid state updates

Browse Source
This commit is contained in:
Lorenz Meier
2014-03-24 10:31:16 +01:00
parent 0022bbb5fb
commit 60728bb6a6
1 changed files with 34 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/estimator.cpp
+34 -30
View File
@@ -1678,43 +1678,47 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec)
{
int ret = 0;
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;
}
// 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
{
// 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++) {
if (isfinite(storedStates[i][bestStoreIndex])) {
statesForFusion[i] = storedStates[i][bestStoreIndex];
} else if (isfinite(states[i])) {
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;
}