From fce2242adb4179b446a856a76103d67418ae43db Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 15 Apr 2020 10:25:35 -0400
Subject: [PATCH] [DO NOT MERGE]: change default integration period 4000 us ->
 2500 us

 - bring in PX4/ecl#795
---
 src/drivers/imu/invensense/icm20602/ICM20602.cpp   | 2 +-
 src/drivers/imu/invensense/icm20602/ICM20602.hpp   | 2 +-
 src/drivers/imu/invensense/icm20608g/ICM20608G.cpp | 2 +-
 src/drivers/imu/invensense/icm20608g/ICM20608G.hpp | 2 +-
 src/drivers/imu/invensense/icm20689/ICM20689.cpp   | 2 +-
 src/drivers/imu/invensense/icm20689/ICM20689.hpp   | 2 +-
 src/drivers/imu/invensense/icm40609d/ICM40609D.cpp | 2 +-
 src/drivers/imu/invensense/icm40609d/ICM40609D.hpp | 2 +-
 src/drivers/imu/invensense/icm42688p/ICM42688P.cpp | 2 +-
 src/drivers/imu/invensense/icm42688p/ICM42688P.hpp | 2 +-
 src/drivers/imu/invensense/mpu6000/MPU6000.cpp     | 2 +-
 src/drivers/imu/invensense/mpu6000/MPU6000.hpp     | 2 +-
 src/drivers/imu/invensense/mpu6500/MPU6500.cpp     | 2 +-
 src/drivers/imu/invensense/mpu6500/MPU6500.hpp     | 2 +-
 src/drivers/imu/invensense/mpu9250/MPU9250.cpp     | 2 +-
 src/drivers/imu/invensense/mpu9250/MPU9250.hpp     | 2 +-
 src/lib/drivers/accelerometer/PX4Accelerometer.cpp | 2 +-
 src/lib/drivers/accelerometer/PX4Accelerometer.hpp | 2 +-
 src/lib/drivers/device/integrator.h                | 2 +-
 src/lib/drivers/gyroscope/PX4Gyroscope.cpp         | 2 +-
 src/lib/drivers/gyroscope/PX4Gyroscope.hpp         | 2 +-
 src/lib/ecl                                        | 2 +-
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++--
 23 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp
index d0ed9184097a..d46804fdb409 100644
--- 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
index bb3135a80f87..b97c3004d5b5 100644
--- 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
index 2aa96bcc4417..98cd27677963 100644
--- 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
index f47254490a74..5d16e2cd3ec8 100644
--- 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/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp
index 85ea248ccaf4..14aad4fb23e5 100644
--- 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
index 5be6cbba5d2f..6762719f09ac 100644
--- 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
index 333a37b4989e..c6187ad1766e 100644
--- 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
index 336d752248a8..40e4e23b410a 100644
--- 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
index 792917dd1263..5716c85b2812 100644
--- 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
index de8547a93495..b520e3e6a6a5 100644
--- 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
index d2b10a69d2e8..bf4adfd700e3 100644
--- 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
index a7a320805263..8355b76590d2 100644
--- 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
index 21287d6086bd..6887da69b3cb 100644
--- 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
index 1117fa26ce9e..21498ecb565c 100644
--- 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
index 52e37b7ff0ed..11dbc4d17675 100644
--- 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
index 3048bb7ad6ff..200fecdcaf82 100644
--- 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
index 63a4a09d7b50..5dc95b84fef4 100644
--- 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
index 6819ea94331b..449cc6d56531 100644
--- 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
index 02586aa03de6..805476785de2 100644
--- 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
index b342252962e9..633738dec1be 100644
--- 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
index e2785e3d6cae..97a1b6ed7c8b 100644
--- 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
index 47624a0f021e..292111d4e9c4 160000
--- a/src/lib/ecl
+++ b/src/lib/ecl
@@ -1 +1 @@
-Subproject commit 47624a0f021e2c187cc4763ad829afe2aa4fb11a
+Subproject commit 292111d4e9c4586c4c8324852ab4568c5834f549
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 36c2c06d7285..50de09701931 100644
--- 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();