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

AP_NavEKF3: Add data buffers for yaw sensor

Browse Source
This commit is contained in:
Andrew Tridgell 2017-11-11 13:54:15 +11:00
parent 87c7649d09
commit 62575a194c
2 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -47,8 +47,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
/*
The imu_buffer_length needs to cope with the worst case sensor delay at the
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.
target EKF state prediction rate. Non-IMU data coming in faster is downsampled.
*/
// Calculate the expected EKF time step
@ -130,6 +129,9 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
if(!storedWheelOdm.init(imu_buffer_length)) { // initialise to same length of IMU to allow for multiple wheel sensors
return false;
}
if(!storedYawAng.init(obs_buffer_length)) {
return false;
}
// Note: the use of dual range finders potentially doubles the amount of data to be stored
if(!storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) {
return false;
@ -406,6 +408,7 @@ void NavEKF3_core::InitialiseVariablesMag()
quatAtLastMagReset = stateStruct.quat;
magFieldLearned = false;
storedMag.reset();
storedYawAng.reset();
}
// Initialise the states from accelerometer and magnetometer data (if present)

15
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -525,6 +525,13 @@ private:
float delTime; // time interval that the measurement was accumulated over (sec)
uint32_t time_ms; // measurement timestamp (msec)
};
struct yaw_elements {
float yawAng; // yaw angle measurement (rad)
float yawAngErr; // yaw angle 1SD measurement accuracy (rad)
uint32_t time_ms; // measurement timestamp (msec)
uint8_t type; // type specifiying Euler rotation order used, 1 = 312 (ZXY), 2 = 321 (ZYX)
};
// bias estimates for the IMUs that are enabled but not being used
// by this core.
@ -1104,8 +1111,8 @@ private:
vel_odm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon
uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innovation consistency checks (msec)
Vector3 bodyVelTestRatio; // Innovation test ratios for body velocity XYZ measurements
Vector3 varInnovBodyVel; // Body velocity XYZ innovation variances (rad/sec)^2
Vector3 innovBodyVel; // Body velocity XYZ innovations (rad/sec)
Vector3 varInnovBodyVel; // Body velocity XYZ innovation variances (m/sec)^2
Vector3 innovBodyVel; // Body velocity XYZ innovations (m/sec)
uint32_t prevBodyVelFuseTime_ms; // previous time all body velocity measurement components passed their innovation consistency checks (msec)
uint32_t bodyOdmMeasTime_ms; // time body velocity measurements were accepted for input to the data buffer (msec)
bool bodyVelFusionDelayed; // true when body frame velocity fusion has been delayed
@ -1118,6 +1125,10 @@ private:
wheel_odm_elements wheelOdmDataNew; // Body frame odometry data at the current time horizon
wheel_odm_elements wheelOdmDataDelayed; // Body frame odometry data at the fusion time horizon
// yaw sensor fusion
obs_ring_buffer_t<yaw_elements> storedYawAng;
yaw_elements yawAngDataNew;
yaw_elements yawAngDataDelayed;
// Range Beacon Sensor Fusion
obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer