diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-23 18:55:06 -0800 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-23 18:55:12 -0800 |
commit | b4c1713b96d452efa8c2285537f0969d1e6dcb77 (patch) | |
tree | 8ad87697ff9bbfedabaa0dbcba3c35624c061918 /src/modules/fw_att_pos_estimator/estimator.cpp | |
parent | d10224666dc9482936183352e635d591279c8d80 (diff) | |
download | px4-firmware-b4c1713b96d452efa8c2285537f0969d1e6dcb77.tar.gz px4-firmware-b4c1713b96d452efa8c2285537f0969d1e6dcb77.tar.bz2 px4-firmware-b4c1713b96d452efa8c2285537f0969d1e6dcb77.zip |
Fix up time delay compensation loading
Diffstat (limited to 'src/modules/fw_att_pos_estimator/estimator.cpp')
-rw-r--r-- | src/modules/fw_att_pos_estimator/estimator.cpp | 30 |
1 files changed, 15 insertions, 15 deletions
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index a7c2cb0e3..e449a6602 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -6,8 +6,6 @@ float KHP[n_states][n_states]; // intermediate result used for covariance update float P[n_states][n_states]; // covariance matrix float Kfusion[n_states]; // Kalman gains float states[n_states]; // state matrix -float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps -uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) @@ -55,6 +53,9 @@ float gpsLon; float gpsHgt; uint8_t GPSstatus; +float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps +uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + // Baro input float baroHgt; @@ -1614,14 +1615,13 @@ 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], uint32_t msec) +void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) { long int bestTimeDelta = 200; - uint8_t storeIndex; - uint8_t bestStoreIndex = 0; - for (storeIndex=0; storeIndex < data_buffer_size; storeIndex++) + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) { - long int timeDelta = msec - statetimeStamp[storeIndex]; + int64_t timeDelta = (int64_t)msec - statetimeStamp[storeIndex]; if (timeDelta < 0) timeDelta = -timeDelta; if (timeDelta < bestTimeDelta) { @@ -1629,14 +1629,14 @@ void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) 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 - // { + 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]) @@ -1825,7 +1825,7 @@ void InitialiseFilter(float (&initvelNED)[3]) for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { - + storedStates[j][i] = 0.0f; } statetimeStamp[i] = 0; |