fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 09:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Disable time compensation for further testing

Browse Source
This commit is contained in:
Lorenz Meier
2014-05-10 17:17:25 +02:00
parent 3e3a64f0ed
commit da67b2b241
1 changed files with 33 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator.cpp
+33 -33
View File
@@ -1815,41 +1815,41 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
{ {
int ret = 0; int ret = 0;
int64_t bestTimeDelta = 200; // int64_t bestTimeDelta = 200;
unsigned bestStoreIndex = 0; // unsigned bestStoreIndex = 0;
for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
{ // {
// Work around a GCC compiler bug - we know 64bit support on ARM is // // Work around a GCC compiler bug - we know 64bit support on ARM is
// sketchy in GCC. // // sketchy in GCC.
uint64_t timeDelta; // uint64_t timeDelta;
if (msec > statetimeStamp[storeIndex]) { // if (msec > statetimeStamp[storeIndex]) {
timeDelta = msec - statetimeStamp[storeIndex]; // timeDelta = msec - statetimeStamp[storeIndex];
} else { // } else {
timeDelta = statetimeStamp[storeIndex] - msec; // timeDelta = statetimeStamp[storeIndex] - msec;
} // }
if (timeDelta < bestTimeDelta) // if (timeDelta < bestTimeDelta)
{ // {
bestStoreIndex = storeIndex; // bestStoreIndex = storeIndex;
bestTimeDelta = timeDelta; // bestTimeDelta = timeDelta;
} // }
} // }
if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
{ // {
for (unsigned i=0; i < n_states; i++) { // for (unsigned i=0; i < n_states; i++) {
if (isfinite(storedStates[i][bestStoreIndex])) { // if (isfinite(storedStates[i][bestStoreIndex])) {
statesForFusion[i] = storedStates[i][bestStoreIndex]; // statesForFusion[i] = storedStates[i][bestStoreIndex];
} else if (isfinite(states[i])) { // } else if (isfinite(states[i])) {
statesForFusion[i] = states[i]; // statesForFusion[i] = states[i];
} else { // } else {
// There is not much we can do here, except reporting the error we just // // There is not much we can do here, except reporting the error we just
// found. // // found.
ret++; // ret++;
} // }
} // }
} // }
else // otherwise output current state // else // otherwise output current state
{ {
for (unsigned i = 0; i < n_states; i++) { for (unsigned i = 0; i < n_states; i++) {
if (isfinite(states[i])) { if (isfinite(states[i])) {