diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-11 20:18:09 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-11 20:18:09 +0200 |
| commit | 1b3007aa813829e401849f53552ac5917da71f5b (patch) | |
| tree | 8cdd4684626c63871969e8220948876a6ed99148 /src/modules | |
| parent | 077de5eb0b0093e131f94f063a107f290d6c293b (diff) | |
| download | px4-firmware-1b3007aa813829e401849f53552ac5917da71f5b.tar.gz px4-firmware-1b3007aa813829e401849f53552ac5917da71f5b.tar.bz2 px4-firmware-1b3007aa813829e401849f53552ac5917da71f5b.zip | |
Re-enabled time compensation
Diffstat (limited to 'src/modules')
| -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 4239db669..9d60f5c0e 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])) { |