IMU: parameterize IMU integration time (IMU_INTEG_RATE)

- default integration rate now 200 Hz (5000 us interval)
 - set update rate for all drivers and simulators using PX4Accelerometer/PX4Gyroscope

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -150,6 +150,9 @@ then
 	# LPE: GPS only mode
 	param set LPE_FUSION 145
 
+	# Simulator IMU data provided at 250 Hz
+	param set IMU_INTEG_RATE 250
+
 	param set MC_PITCH_P 6
 	param set MC_PITCHRATE_P 0.2
 	param set MC_ROLL_P 6

src/drivers/imu/adis16448/ADIS16448.cpp

@@ -115,6 +115,9 @@ bool ADIS16448::reset()
 		return false;
 	}
 
+	_px4_accel.set_update_rate(_sample_rate);
+	_px4_gyro.set_update_rate(_sample_rate);
+
 	// Set gyroscope scale to default value.
 	//if (!set_gyro_dyn_range(GYRO_INITIAL_SENSITIVITY)) {
 	//	return false;

src/drivers/imu/adis16477/ADIS16477.cpp

@@ -69,8 +69,10 @@ ADIS16477::ADIS16477(I2CSPIBusOption bus_option, int bus, int32_t device, enum R
 #endif // GPIO_SPI1_RESET_ADIS16477
 
 	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
+	_px4_accel.set_update_rate(ADIS16477_DEFAULT_RATE);
 
 	_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
+	_px4_gyro.set_update_rate(ADIS16477_DEFAULT_RATE);
 }
 
 ADIS16477::~ADIS16477()

src/drivers/imu/adis16497/ADIS16497.cpp

@@ -84,6 +84,9 @@ ADIS16497::ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum R
 	// Configure hardware reset line
 	px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16497);
 #endif // GPIO_SPI1_RESET_ADIS16497
+
+	_px4_accel.set_update_rate(ADIS16497_DEFAULT_RATE);
+	_px4_gyro.set_update_rate(ADIS16497_DEFAULT_RATE);
 }
 
 ADIS16497::~ADIS16497()

src/drivers/imu/bma180/bma180.cpp

@@ -182,6 +182,7 @@ BMA180::BMA180(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio
 	_px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read"))
 {
+	_px4_accel.set_update_rate(1000);
 }
 
 BMA180::~BMA180()

src/drivers/imu/bmi055/BMI055_accel.cpp

@@ -54,6 +54,7 @@ BMI055_accel::BMI055_accel(I2CSPIBusOption bus_option, int bus, const char *path
 	_duplicates(perf_alloc(PC_COUNT, "bmi055_accel_duplicates")),
 	_got_duplicate(false)
 {
+	_px4_accel.set_update_rate(BMI055_ACCEL_DEFAULT_RATE);
 }
 
 BMI055_accel::~BMI055_accel()

src/drivers/imu/bmi055/BMI055_gyro.cpp

@@ -57,6 +57,7 @@ BMI055_gyro::BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
 	_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers"))
 {
+	_px4_gyro.set_update_rate(BMI055_GYRO_DEFAULT_RATE);
 }
 
 BMI055_gyro::~BMI055_gyro()

src/drivers/imu/bmi088/BMI088_accel.cpp

@@ -63,6 +63,7 @@ BMI088_accel::BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path
 	_duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")),
 	_got_duplicate(false)
 {
+	_px4_accel.set_update_rate(BMI088_ACCEL_DEFAULT_RATE);
 }
 
 BMI088_accel::~BMI088_accel()

src/drivers/imu/bmi088/BMI088_gyro.cpp

@@ -65,6 +65,7 @@ BMI088_gyro::BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")),
 	_bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers"))
 {
+	_px4_gyro.set_update_rate(BMI088_GYRO_DEFAULT_RATE);
 }
 
 BMI088_gyro::~BMI088_gyro()

src/drivers/imu/bmi160/bmi160.cpp

@@ -64,6 +64,7 @@ BMI160::BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio
 	_reset_retries(perf_alloc(PC_COUNT, MODULE_NAME":reset retries")),
 	_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates"))
 {
+	_px4_gyro.set_update_rate(BMI160_GYRO_DEFAULT_RATE);
 }
 
 BMI160::~BMI160()

src/drivers/imu/fxas21002c/FXAS21002C.cpp

@@ -75,6 +75,7 @@ FXAS21002C::FXAS21002C(device::Device *interface, I2CSPIBusOption bus_option, in
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")),
 	_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicate reading"))
 {
+	_px4_gyro.set_update_rate(FXAS21002C_DEFAULT_RATE);
 }
 
 FXAS21002C::~FXAS21002C()

src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp

@@ -66,6 +66,8 @@ FXOS8701CQ::FXOS8701CQ(device::Device *interface, I2CSPIBusOption bus_option, in
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad reg")),
 	_accel_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": acc dupe"))
 {
+	_px4_accel.set_update_rate(FXOS8701C_ACCEL_DEFAULT_RATE);
+
 #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
 	_px4_mag.set_scale(0.001f);
 #endif

src/drivers/imu/l3gd20/L3GD20.cpp

@@ -45,6 +45,7 @@ L3GD20::L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotati
 	_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
 	_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe"))
 {
+	_px4_gyro.set_update_rate(L3GD20_DEFAULT_RATE);
 }
 
 L3GD20::~L3GD20()

src/drivers/imu/lsm303d/LSM303D.cpp

@@ -127,6 +127,7 @@ LSM303D::reset()
 
 	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
 	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
+	_px4_accel.set_update_rate(LSM303D_ACCEL_DEFAULT_RATE);
 
 	// we setup the anti-alias on-chip filter as 50Hz. We believe
 	// this operates in the analog domain, and is critical for

src/drivers/imu/mpu6000/MPU6000.cpp

@@ -160,6 +160,8 @@ int MPU6000::reset()
 
 	// SAMPLE RATE
 	_set_sample_rate(1000);
+	_px4_accel.set_update_rate(1000);
+	_px4_gyro.set_update_rate(1000);
 	px4_usleep(1000);
 
 	_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);

src/drivers/imu/mpu9250/mpu9250.cpp

@@ -189,6 +189,8 @@ MPU9250::reset_mpu()
 
 	// SAMPLE RATE
 	_set_sample_rate(_sample_rate);
+	_px4_accel.set_update_rate(_sample_rate);
+	_px4_gyro.set_update_rate(_sample_rate);
 
 	_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
 

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -65,6 +65,7 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit
 
 PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
 	CDev(nullptr),
+	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_accel), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
 	_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
@@ -77,6 +78,11 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum R
 	_sensor_pub.advertise();
 	_sensor_integrated_pub.advertise();
 	_sensor_status_pub.advertise();
+
+	updateParams();
+
+	// set reasonable default, driver should be setting real value
+	set_update_rate(800);
 }
 
 PX4Accelerometer::~PX4Accelerometer()
@@ -128,11 +134,21 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
 
 void PX4Accelerometer::set_update_rate(uint16_t rate)
 {
-	_update_rate = rate;
-	const uint32_t update_interval = 1000000 / rate;
+	_update_rate = math::constrain((int)rate, 50, 32000);
 
-	// TODO: set this intelligently
-	_integrator_reset_samples = 2500 / update_interval;
+	// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
+	int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000);
+
+	if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
+		_param_imu_integ_rate.set(imu_integration_rate_hz);
+		_param_imu_integ_rate.commit_no_notification();
+	}
+
+	const float update_interval_us = 1e6f / _update_rate;
+	const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz;
+
+	_integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us);
+	_integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us);
 }
 
 void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
@@ -383,10 +399,12 @@ void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
 
 void PX4Accelerometer::print_status()
 {
+#if !defined(CONSTRAINED_FLASH)
 	PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
 
 	PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
 		 (double)_calibration_scale(2));
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));
+#endif // !CONSTRAINED_FLASH
 }

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -39,6 +39,7 @@
 #include <lib/conversion/rotation.h>
 #include <lib/drivers/device/integrator.h>
 #include <lib/ecl/geo/geo.h>
+#include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
@@ -46,7 +47,7 @@
 #include <uORB/topics/sensor_accel_integrated.h>
 #include <uORB/topics/sensor_accel_status.h>
 
-class PX4Accelerometer : public cdev::CDev
+class PX4Accelerometer : public cdev::CDev, public ModuleParams
 {
 public:
 	PX4Accelerometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
@@ -98,7 +99,7 @@ private:
 
 	hrt_abstime	_status_last_publish{0};
 
-	Integrator		_integrator{2500, false};
+	Integrator		_integrator{5000, false}; // 200 Hz default
 
 	matrix::Vector3f	_calibration_scale{1.f, 1.f, 1.f};
 	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
@@ -131,4 +132,8 @@ private:
 	uint8_t			_integrator_reset_samples{4};
 	uint8_t			_integrator_samples{0};
 	uint8_t			_integrator_fifo_samples{0};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
+	)
 };

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -80,6 +80,9 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation
 	_sensor_status_pub.advertise();
 
 	updateParams();
+
+	// set reasonable default, driver should be setting real value
+	set_update_rate(800);
 }
 
 PX4Gyroscope::~PX4Gyroscope()
@@ -130,11 +133,21 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
 
 void PX4Gyroscope::set_update_rate(uint16_t rate)
 {
-	_update_rate = rate;
-	const uint32_t update_interval = 1000000 / rate;
+	_update_rate = math::constrain((int)rate, 50, 32000);
 
-	// TODO: set this intelligently
-	_integrator_reset_samples = 2500 / update_interval;
+	// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
+	int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000);
+
+	if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
+		_param_imu_integ_rate.set(imu_integration_rate_hz);
+		_param_imu_integ_rate.commit_no_notification();
+	}
+
+	const float update_interval_us = 1e6f / _update_rate;
+	const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz;
+
+	_integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us);
+	_integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us);
 }
 
 void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)
@@ -390,8 +403,10 @@ void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
 
 void PX4Gyroscope::print_status()
 {
+#if !defined(CONSTRAINED_FLASH)
 	PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
 
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));
+#endif // !CONSTRAINED_FLASH
 }

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -100,7 +100,7 @@ private:
 
 	hrt_abstime	_status_last_publish{0};
 
-	Integrator		_integrator{2500, true};
+	Integrator		_integrator{5000, true}; // 200 Hz default
 
 	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
 
@@ -135,6 +135,7 @@ private:
 	uint8_t			_integrator_fifo_samples{0};
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
 	)
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -2196,6 +2196,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 			if (_px4_gyro == nullptr) {
 				PX4_ERR("PX4Gyroscope alloc failed");
+
+			} else {
+				_px4_gyro->set_update_rate(200); // TODO: measure actual
 			}
 		}
 
@@ -2213,6 +2216,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 			if (_px4_accel == nullptr) {
 				PX4_ERR("PX4Accelerometer alloc failed");
+
+			} else {
+				_px4_accel->set_update_rate(200); // TODO: measure actual
 			}
 		}
 

src/modules/sensors/vehicle_imu/imu_parameters.c

@@ -0,0 +1,45 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* IMU integration rate.
+*
+* The rate at which raw IMU data is integrated to produce delta angles and delta velocities.
+*
+* @min 100
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_INT32(IMU_INTEG_RATE, 200);

src/modules/sih/sih.cpp

@@ -95,6 +95,8 @@ Sih::Sih() :
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": execution")),
 	_sampling_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sampling"))
 {
+	_px4_accel.set_update_rate(LOOP_INTERVAL);
+	_px4_gyro.set_update_rate(LOOP_INTERVAL);
 }
 
 void Sih::run()

src/modules/simulator/simulator.h

@@ -133,6 +133,9 @@ public:
 private:
 	Simulator() : ModuleParams(nullptr)
 	{
+		// current default
+		_px4_accel.set_update_rate(250);
+		_px4_gyro.set_update_rate(250);
 	}
 
 	~Simulator()