From ebab4bfa7ef9ca138ac476943a87b8e3244b9a21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 13:16:06 +0100 Subject: Fix for recallstates function --- src/modules/fw_att_pos_estimator/estimator.cpp | 52 +++++++++++++------------- 1 file changed, 26 insertions(+), 26 deletions(-) (limited to 'src/modules/fw_att_pos_estimator/estimator.cpp') diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index cbbb5d7a0..f57890146 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1615,34 +1615,34 @@ void StoreStates(uint64_t timestamp_ms) } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) +void RecallStates(float statesForFusion[n_states], uint64_t msec) { - int 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. - int 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++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - } - else // otherwise output current state - { + // 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++) statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } + // else // otherwise output current state + // { for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - } + // } } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) -- cgit v1.2.3