diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-06 08:35:14 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-06 08:35:14 +0100 |
commit | 40deb405c1e9232475c4b5e7bda8080f85fec19a (patch) | |
tree | a1e8f815a09cf26216eae939c842e6e7958f74f0 /src/modules/fw_att_pos_estimator | |
parent | 3c5bcf77cc3dc9cccbe82893afefa66cb7cc427c (diff) | |
download | px4-firmware-40deb405c1e9232475c4b5e7bda8080f85fec19a.tar.gz px4-firmware-40deb405c1e9232475c4b5e7bda8080f85fec19a.tar.bz2 px4-firmware-40deb405c1e9232475c4b5e7bda8080f85fec19a.zip |
Modified estimator to fix internal GCC compiler error (hilarious 64bit handling), use float instead of double routines to avoid implicit casts
Diffstat (limited to 'src/modules/fw_att_pos_estimator')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 18 |
1 files changed, 12 insertions, 6 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index c9e809280..cbbb5d7a0 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1617,12 +1617,18 @@ 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) { - int64_t bestTimeDelta = 200; + int bestTimeDelta = 200; unsigned bestStoreIndex = 0; for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) { - int64_t timeDelta = (int)msec - statetimeStamp[storeIndex]; - if (timeDelta < 0) timeDelta = -timeDelta; + // 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; @@ -1739,9 +1745,9 @@ void OnGroundCheck() void calcEarthRateNED(Vector3f &omega, float latitude) { //Define Earth rotation vector in the NED navigation frame - omega.x = earthRate*cos(latitude); - omega.y = 0.0; - omega.z = -earthRate*sin(latitude); + omega.x = earthRate*cosf(latitude); + omega.y = 0.0f; + omega.z = -earthRate*sinf(latitude); } void CovarianceInit() |