From 40deb405c1e9232475c4b5e7bda8080f85fec19a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Mar 2014 08:35:14 +0100 Subject: Modified estimator to fix internal GCC compiler error (hilarious 64bit handling), use float instead of double routines to avoid implicit casts --- src/modules/fw_att_pos_estimator/estimator.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 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 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() -- cgit v1.2.3