diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-10 17:17:25 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-10 17:17:25 +0200 |
commit | da67b2b241ca8eff0d25bfe10b0213f3d570dd50 (patch) | |
tree | 1fe790f884111454a65c8871c31d056be9a9befc /src/modules/ekf_att_pos_estimator | |
parent | 3e3a64f0eda20613aad77c13e4e8e0a2dde89baa (diff) | |
download | px4-firmware-da67b2b241ca8eff0d25bfe10b0213f3d570dd50.tar.gz px4-firmware-da67b2b241ca8eff0d25bfe10b0213f3d570dd50.tar.bz2 px4-firmware-da67b2b241ca8eff0d25bfe10b0213f3d570dd50.zip |
Disable time compensation for further testing
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 70 |
1 files changed, 35 insertions, 35 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index a6d6a9db5..5a6f879b8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1815,41 +1815,41 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) { int ret = 0; - int64_t bestTimeDelta = 200; - unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - { - // Work around a GCC compiler bug - we know 64bit support on ARM is - // sketchy in GCC. - uint64_t timeDelta; - - if (msec > statetimeStamp[storeIndex]) { - timeDelta = msec - statetimeStamp[storeIndex]; - } else { - timeDelta = statetimeStamp[storeIndex] - msec; - } - - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { - for (unsigned 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 + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // Work around a GCC compiler bug - we know 64bit support on ARM is + // // sketchy in GCC. + // uint64_t timeDelta; + + // if (msec > statetimeStamp[storeIndex]) { + // timeDelta = msec - statetimeStamp[storeIndex]; + // } else { + // timeDelta = statetimeStamp[storeIndex] - msec; + // } + + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (unsigned 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 (unsigned i = 0; i < n_states; i++) { if (isfinite(states[i])) { |