diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-24 10:31:16 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-24 10:31:16 +0100 |
commit | 60728bb6a6211407a1db12e7c015272cf998d928 (patch) | |
tree | e946c9d60aeda106e9c17e098015785f7c8561ca /src/modules/fw_att_pos_estimator/estimator.cpp | |
parent | 0022bbb5fbb0cd7237c9ce8b0006ec4ac0e14066 (diff) | |
download | px4-firmware-60728bb6a6211407a1db12e7c015272cf998d928.tar.gz px4-firmware-60728bb6a6211407a1db12e7c015272cf998d928.tar.bz2 px4-firmware-60728bb6a6211407a1db12e7c015272cf998d928.zip |
Now that the guard is updated disable time compensation again, but keep a guard against invalid state updates
Diffstat (limited to 'src/modules/fw_att_pos_estimator/estimator.cpp')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 66 |
1 files changed, 35 insertions, 31 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index f03aa9474..46e405526 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -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; - } - - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { + // 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++) { - 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; } |