aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: make IMU buffer length depend on main loop rate

Browse Source 
this fixes a problem with Replay and baro data
This commit is contained in:
Andrew Tridgell 2015-11-18 10:15:34 +11:00
parent 1b29a1af46
commit 99da195e37
4 changed files with 34 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -392,14 +392,14 @@ void NavEKF2_core::StoreIMU()
{
// increment the index and write new data
fifoIndexNow = fifoIndexNow + 1;
if (fifoIndexNow >= IMU_BUFFER_LENGTH) {
if (fifoIndexNow >= imu_buffer_length) {
fifoIndexNow = 0;
}
storedIMU[fifoIndexNow] = imuDataDownSampledNew;
// set the index required to access the oldest data, applying an offset to the fusion time horizon that is used to
// prevent the same fusion operation being performed on the same frame across multiple EKF's
fifoIndexDelayed = fifoIndexNow + 1;
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
if (fifoIndexDelayed >= imu_buffer_length) {
fifoIndexDelayed = 0;
}
}
@ -408,12 +408,12 @@ void NavEKF2_core::StoreIMU()
void NavEKF2_core::StoreIMU_reset()
{
// write current measurement to entire table
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedIMU[i] = imuDataNew;
}
imuDataDelayed = imuDataNew;
fifoIndexDelayed = fifoIndexNow+1;
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
if (fifoIndexDelayed >= imu_buffer_length) {
fifoIndexDelayed = 0;
}
}

6
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -32,7 +32,7 @@ void NavEKF2_core::ResetVelocity(void)
stateStruct.velocity.x = gpsDataNew.vel.x; // north velocity from blended accel data
stateStruct.velocity.y = gpsDataNew.vel.y; // east velocity from blended accel data
}
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].velocity.x = stateStruct.velocity.x;
storedOutput[i].velocity.y = stateStruct.velocity.y;
}
@ -65,7 +65,7 @@ void NavEKF2_core::ResetPosition(void)
stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
}
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.x = stateStruct.position.x;
storedOutput[i].position.y = stateStruct.position.y;
}
@ -90,7 +90,7 @@ void NavEKF2_core::ResetHeight(void)
// write to the state vector
stateStruct.position.z = -baroDataNew.hgt; // down position from blended accel data
terrainState = stateStruct.position.z + rngOnGnd;
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.z = stateStruct.position.z;
}
outputDataNew.position.z = stateStruct.position.z;

27
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -61,6 +61,27 @@ void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
imu_index = _imu_index;
core_index = _core_index;
_ahrs = frontend->_ahrs;
/*
the imu_buffer_length needs to cope with a 260ms delay at a
maximum fusion rate of 100Hz. Non-imu data coming in at faster
than 100Hz is downsampled. For 50Hz main loop rate we need a
shorter buffer.
*/
switch (_ahrs->get_ins().get_sample_rate()) {
case AP_InertialSensor::RATE_50HZ:
imu_buffer_length = 13;
break;
case AP_InertialSensor::RATE_100HZ:
case AP_InertialSensor::RATE_200HZ:
case AP_InertialSensor::RATE_400HZ:
// maximum 260 msec delay at 100 Hz fusion rate
imu_buffer_length = 26;
break;
}
storedIMU = new imu_elements[imu_buffer_length];
storedOutput = new output_elements[imu_buffer_length];
}
@ -1128,7 +1149,7 @@ void NavEKF2_core::StoreOutputReset()
outputDataNew.velocity = stateStruct.velocity;
outputDataNew.position = stateStruct.position;
// write current measurement to entire table
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i] = outputDataNew;
}
outputDataDelayed = outputDataNew;
@ -1142,7 +1163,7 @@ void NavEKF2_core::StoreQuatReset()
{
outputDataNew.quat = stateStruct.quat;
// write current measurement to entire table
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].quat = outputDataNew.quat;
}
outputDataDelayed.quat = outputDataNew.quat;
@ -1153,7 +1174,7 @@ void NavEKF2_core::StoreQuatRotate(Quaternion deltaQuat)
{
outputDataNew.quat = outputDataNew.quat*deltaQuat;
// write current measurement to entire table
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].quat = storedOutput[i].quat*deltaQuat;
}
outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;

22
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -42,23 +42,6 @@
#define MASK_GPS_VERT_SPD (1<<6)
#define MASK_GPS_HORIZ_SPD (1<<7)
/*
* IMU FIFO buffer length depends on the IMU update rate being used and the maximum sensor delay
* Samples*delta_time must be > max sensor delay
*/
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
// Note that if using more than 2 instances of the EKF, as set by EK2_IMU_MASK, this delay should be increased by 2 samples
// for each additional instance to allow for the need to offset the fusion time horizon for each instance to avoid simultaneous fusion
// of measurements by each instance
#define IMU_BUFFER_LENGTH 26 // maximum 260 msec delay at 100 Hz fusion rate
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
#define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz
#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
#define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz
#else
#define IMU_BUFFER_LENGTH 104 // unknown so use max buffer length
#endif
class AP_AHRS;
class NavEKF2_core
@ -282,6 +265,7 @@ private:
NavEKF2 *frontend;
uint8_t imu_index;
uint8_t core_index;
uint8_t imu_buffer_length;
typedef float ftype;
#if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1)
@ -657,12 +641,12 @@ private:
Matrix24 KH; // intermediate result used for covariance updates
Matrix24 KHP; // intermediate result used for covariance updates
Matrix24 P; // covariance matrix
imu_elements storedIMU[IMU_BUFFER_LENGTH]; // IMU data buffer
imu_elements *storedIMU; // IMU data buffer [imu_buffer_length]
gps_elements storedGPS[OBS_BUFFER_LENGTH]; // GPS data buffer
mag_elements storedMag[OBS_BUFFER_LENGTH]; // Magnetometer data buffer
baro_elements storedBaro[OBS_BUFFER_LENGTH]; // Baro data buffer
tas_elements storedTAS[OBS_BUFFER_LENGTH]; // TAS data buffer
output_elements storedOutput[IMU_BUFFER_LENGTH];// output state buffer
output_elements *storedOutput; // output state buffer [imu_buffer_length]
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng
Vector3f correctedDelVel; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)