fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 01:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix up time delay compensation loading

Browse Source
This commit is contained in:
Lorenz Meier
2014-02-23 18:55:06 -08:00
parent d10224666d
commit b4c1713b96
2 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/estimator.cpp
+15 -15
View File
@@ -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;
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], uint32_t msec);
void RecallStates(float (&statesForFusion)[n_states], uint64_t msec);
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);