fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 06:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix for recallstates function

Browse Source
This commit is contained in:
Lorenz Meier
2014-03-11 13:16:06 +01:00
parent 4d8524f508
commit ebab4bfa7e
2 changed files with 26 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/estimator.cpp
+25 -25
View File
@@ -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;
}
// 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
{
// 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])
src/modules/fw_att_pos_estimator/estimator.h
+1 -1
View File
@@ -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]);