diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-24 10:29:13 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-24 10:29:13 +0100 |
commit | 0022bbb5fbb0cd7237c9ce8b0006ec4ac0e14066 (patch) | |
tree | 7ac4c5607e8c7b72890831e2474fa9f0e56a189b | |
parent | 8666ca53bf5b8eebbf60908c16273b711f80a19e (diff) | |
download | px4-firmware-0022bbb5fbb0cd7237c9ce8b0006ec4ac0e14066.tar.gz px4-firmware-0022bbb5fbb0cd7237c9ce8b0006ec4ac0e14066.tar.bz2 px4-firmware-0022bbb5fbb0cd7237c9ce8b0006ec4ac0e14066.zip |
Guard against invalid states
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 69 | ||||
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.h | 14 |
2 files changed, 54 insertions, 29 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index bde46ba03..f03aa9474 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1674,34 +1674,49 @@ void ResetStoredStates() } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float statesForFusion[n_states], uint64_t msec) +int RecallStates(float statesForFusion[n_states], uint64_t msec) { - // 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]; - // } + int ret = 0; + + 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++) { + 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 (uint8_t i=0; i < n_states; i++) { + statesForFusion[i] = states[i]; + } + } + + return ret; } 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 55e6eb12e..c5a5e9d8d 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -171,8 +171,18 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]); // store staes along with system time stamp in msces 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); +/** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + * + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ +int RecallStates(float statesForFusion[n_states], uint64_t msec); void ResetStoredStates(); |