Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Adjust IMU FIFO buffer for different IMU data rates 

The values chosen ensure that up to consistent 250 msec of sensor delay compensation is available for different platform types
The revised values also ensure that fusion occurs at different time to when the 10Hz magnetometer measurements are read
This commit is contained in:
Paul Riseborough 2015-10-19 09:04:51 +11:00 committed by Andrew Tridgell
parent baa8692960
commit e0ed2dab63
2 changed files with 19 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -156,7 +156,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Param: GPS_DELAY
// @DisplayName: GPS measurement delay (msec)
// @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements.
// @Range: 0 500
// @Range: 0 250
// @Increment: 10
// @User: Advanced
// @Units: milliseconds
@ -191,7 +191,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Param: HGT_DELAY
// @DisplayName: Height measurement delay (msec)
// @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
// @Range: 0 500
// @Range: 0 250
// @Increment: 10
// @User: Advanced
// @Units: milliseconds
@ -290,7 +290,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = {
// @Param: FLOW_DELAY
// @DisplayName: Optical Flow measurement delay (msec)
// @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
// @Range: 0 - 500
// @Range: 0 - 250
// @Increment: 10
// @User: Advanced
// @Units: milliseconds

19
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -42,6 +42,18 @@
#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)
#define IMU_BUFFER_LENGTH 104 // maximum 260 msec delay at 400 Hz
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
#define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz
#else
#define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz
#endif
class AP_AHRS;
class NavEKF2_core
@ -599,9 +611,10 @@ private:
// Round to the nearest multiple of a integer
uint32_t roundToNearest(uint32_t dividend, uint32_t divisor );
// measurement buffer sizes
static const uint32_t IMU_BUFFER_LENGTH = 100; // number of IMU samples stored in the buffer. Samples*delta_time must be > max sensor delay
static const uint32_t OBS_BUFFER_LENGTH = 5; // number of non-IMU sensor samples stored in the buffer.
// Length of FIFO buffers used for non-IMU sensor data.
// Must be larger than the maximum number of sensor samples that will arrive during the time period defined by IMU_BUFFER_LENGTH
// OBS_BUFFER_LENGTH > IMU_BUFFER_LENGTH * dtIMUavg * 'max sensor rate'
static const uint32_t OBS_BUFFER_LENGTH = 5;
// Variables
bool statesInitialised; // boolean true when filter states have been initialised