update ecl/EKF with improved covariance prediction stability and chan…

…ge default IMU integration period 4000 us -> 2500 us - bring in PX4/PX4-ECL#795 "EKF: Improve covariance prediction stability" - the ecl/EKF filter update period has changed from 8 ms to 10 ms - change default integration period 4000 us -> 2500 us (aligns with new EKF filter update period)
PX4 · Apr 23, 2020 · 31f3a21 · 31f3a21
1 parent 5819c82
commit 31f3a21
Show file tree

Hide file tree

Showing 25 changed files with 26 additions and 26 deletions.
diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp
@@ -302,7 +302,7 @@ void ICM20602::ConfigureGyro()
 void ICM20602::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
+		sample_rate = 800; // default to 800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp
@@ -155,7 +155,7 @@ class ICM20602 : public device::SPI, public I2CSPIDriver<ICM20602>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
@@ -315,7 +315,7 @@ void ICM20608G::ConfigureGyro()
 void ICM20608G::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
+		sample_rate = 800; // default to 800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
@@ -158,7 +158,7 @@ class ICM20608G : public device::SPI, public I2CSPIDriver<ICM20608G>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp
@@ -317,7 +317,7 @@ void ICM20649::ConfigureGyro()
 void ICM20649::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to ~1 kHz
+		sample_rate = 800; // default to ~800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp
@@ -169,7 +169,7 @@ class ICM20649 : public device::SPI, public I2CSPIDriver<ICM20649>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp
@@ -315,7 +315,7 @@ void ICM20689::ConfigureGyro()
 void ICM20689::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
+		sample_rate = 800; // default to 800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp
@@ -158,7 +158,7 @@ class ICM20689 : public device::SPI, public I2CSPIDriver<ICM20689>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
@@ -308,7 +308,7 @@ void ICM40609D::ConfigureGyro()
 void ICM40609D::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
+		sample_rate = 800; // default to 800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
@@ -158,7 +158,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver<ICM40609D>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
@@ -310,7 +310,7 @@ void ICM42688P::ConfigureGyro()
 void ICM42688P::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
+		sample_rate = 800; // default to 800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
@@ -158,7 +158,7 @@ class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
@@ -310,7 +310,7 @@ void MPU6000::ConfigureGyro()
 void MPU6000::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
+		sample_rate = 800; // default to 800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
@@ -155,7 +155,7 @@ class MPU6000 : public device::SPI, public I2CSPIDriver<MPU6000>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
@@ -310,7 +310,7 @@ void MPU6500::ConfigureGyro()
 void MPU6500::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
+		sample_rate = 800; // default to 800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp
@@ -155,7 +155,7 @@ class MPU6500 : public device::SPI, public I2CSPIDriver<MPU6500>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
@@ -343,7 +343,7 @@ void MPU9250::ConfigureGyro()
 void MPU9250::ConfigureSampleRate(int sample_rate)
 {
 	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
+		sample_rate = 800; // default to 800 Hz
 	}
 
 	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER

diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
@@ -167,7 +167,7 @@ class MPU9250 : public device::SPI, public I2CSPIDriver<MPU9250>
 
 	STATE _state{STATE::RESET};
 
-	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
 

diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
@@ -124,7 +124,7 @@ void PX4Accelerometer::set_update_rate(uint16_t rate)
 	const uint32_t update_interval = 1000000 / rate;
 
 	// TODO: set this intelligently
-	_integrator_reset_samples = 4000 / update_interval;
+	_integrator_reset_samples = 2500 / update_interval;
 }
 
 void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)

diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
@@ -98,7 +98,7 @@ class PX4Accelerometer : public cdev::CDev
 
 	hrt_abstime	_status_last_publish{0};
 
-	Integrator		_integrator{4000, false};
+	Integrator		_integrator{2500, false};
 
 	matrix::Vector3f	_calibration_scale{1.f, 1.f, 1.f};
 	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};

diff --git a/src/lib/drivers/device/integrator.h b/src/lib/drivers/device/integrator.h
@@ -48,7 +48,7 @@
 class Integrator
 {
 public:
-	Integrator(uint32_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
+	Integrator(uint32_t auto_reset_interval = 2500 /* 400 Hz */, bool coning_compensation = false);
 	~Integrator() = default;
 
 	/**

diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
@@ -126,7 +126,7 @@ void PX4Gyroscope::set_update_rate(uint16_t rate)
 	const uint32_t update_interval = 1000000 / rate;
 
 	// TODO: set this intelligently
-	_integrator_reset_samples = 4000 / update_interval;
+	_integrator_reset_samples = 2500 / update_interval;
 }
 
 void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)

diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
@@ -100,7 +100,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams
 
 	hrt_abstime	_status_last_publish{0};
 
-	Integrator		_integrator{4000, true};
+	Integrator		_integrator{2500, true};
 
 	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
 

diff --git a/src/lib/ecl b/src/lib/ecl
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -318,8 +318,8 @@ MulticopterPositionControl::init()
 		return false;
 	}
 
-	// limit to every other vehicle_local_position update (~62.5 Hz)
-	_local_pos_sub.set_interval_us(16_ms);
+	// limit to every other vehicle_local_position update (50 Hz)
+	_local_pos_sub.set_interval_us(20_ms);
 
 	_time_stamp_last_loop = hrt_absolute_time();