diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-11 13:16:06 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-11 13:16:06 +0100 |
commit | ebab4bfa7ef9ca138ac476943a87b8e3244b9a21 (patch) | |
tree | 68cfbe8257e7683ed8243637bdb49e1cb84dedca /src | |
parent | 4d8524f508fe9e74606fe80f063801a1c47baa14 (diff) | |
download | px4-firmware-ebab4bfa7ef9ca138ac476943a87b8e3244b9a21.tar.gz px4-firmware-ebab4bfa7ef9ca138ac476943a87b8e3244b9a21.tar.bz2 px4-firmware-ebab4bfa7ef9ca138ac476943a87b8e3244b9a21.zip |
Fix for recallstates function
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 52 | ||||
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.h | 2 |
2 files changed, 27 insertions, 27 deletions
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]) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index d68ac1d34..459515f3b 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -147,7 +147,7 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]); void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint64_t msec); +void RecallStates(float statesForFusion[n_states], uint64_t msec); void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); |