IMU: parameterize IMU integration time (IMU_INTEG_RATE)

.vscode/tasks.json

@@ -24,7 +24,7 @@
             "label": "jmavsim",
             "type": "shell",
             "dependsOn": "jmavsim build",
-            "command": "java -Djava.ext.dirs= -jar jmavsim_run.jar -r 250 -lockstep -tcp localhost:4560 -qgc",
+            "command": "java -Djava.ext.dirs= -jar jmavsim_run.jar -r 200 -lockstep -tcp localhost:4560 -qgc",
             "options": {
                 "cwd": "${workspaceRoot}/Tools/jMAVSim/out/production",
                 "env": {

Tools/sitl_run.sh

@@ -75,7 +75,7 @@ SIM_PID=0
 
 if [ "$program" == "jmavsim" ] && [ ! -n "$no_sim" ]; then
 	# Start Java simulator
-	"$src_path"/Tools/jmavsim_run.sh -r 250 -l &
+	"$src_path"/Tools/jmavsim_run.sh -r 200 -l &
 	SIM_PID=`echo $!`
 elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]; then
 	if [ -x "$(command -v gazebo)" ]; then

boards/av/x-v1/init/rc.board_mavlink

@@ -5,7 +5,7 @@
 
 # AV-X: start MAVLink to companion (connected to TX2)
 mavlink start -d /dev/ttyS5 -b 500000 -m osd
-mavlink stream -d /dev/ttyS5 -s HIGHRES_IMU -r 250
+mavlink stream -d /dev/ttyS5 -s HIGHRES_IMU -r 200
 mavlink stream -d /dev/ttyS5 -s CAMERA_TRIGGER -r 100
 mavlink stream -d /dev/ttyS5 -s LOCAL_POSITION_NED -r 50
 

posix-configs/rpi/px4_hil.config

@@ -5,7 +5,7 @@
 
 # HITL configuration
 # connect to it with jMAVSim:
-# ./Tools/jmavsim_run.sh -q -i <IP> -p 14577 -r 250
+# ./Tools/jmavsim_run.sh -q -i <IP> -p 14577 -r 200
 
 uorb start
 

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, uint8_t 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},
@@ -73,6 +74,11 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
 	_rotation{rotation}
 {
 	_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
+
+	updateParams();
+
+	// set reasonable default, driver should be setting real value
+	set_update_rate(800);
 }
 
 PX4Accelerometer::~PX4Accelerometer()
@@ -119,11 +125,19 @@ 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);
+	const uint32_t update_interval = 1000000 / _update_rate;
+
+	// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
+	int32_t imu_integration_time_us = math::constrain(_param_imu_integ_time.get(), 1000, 20000);
+
+	if (imu_integration_time_us != _param_imu_integ_time.get()) {
+		_param_imu_integ_time.set(imu_integration_time_us);
+		_param_imu_integ_time.commit_no_notification();
+	}
 
-	// TODO: set this intelligently
-	_integrator_reset_samples = 2500 / update_interval;
+	_integrator_reset_samples = _param_imu_integ_time.get() / update_interval;
+	_integrator.set_autoreset_interval(_param_imu_integ_time.get());
 }
 
 void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -39,14 +39,15 @@
 #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/PublicationQueuedMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_accel_fifo.h>
 #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, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
@@ -131,4 +132,8 @@ class PX4Accelerometer : public cdev::CDev
 	uint8_t			_integrator_reset_samples{4};
 	uint8_t			_integrator_samples{0};
 	uint8_t			_integrator_fifo_samples{0};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::IMU_INTEG_TIME>) _param_imu_integ_time
+	)
 };

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -76,6 +76,9 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
 	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
 
 	updateParams();
+
+	// set reasonable default, driver should be setting real value
+	set_update_rate(800);
 }
 
 PX4Gyroscope::~PX4Gyroscope()
@@ -121,11 +124,19 @@ 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);
+	const uint32_t update_interval = 1000000 / _update_rate;
+
+	// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
+	int32_t imu_integration_time_us = math::constrain(_param_imu_integ_time.get(), 1000, 20000);
+
+	if (imu_integration_time_us != _param_imu_integ_time.get()) {
+		_param_imu_integ_time.set(imu_integration_time_us);
+		_param_imu_integ_time.commit_no_notification();
+	}
 
-	// TODO: set this intelligently
-	_integrator_reset_samples = 2500 / update_interval;
+	_integrator_reset_samples = _param_imu_integ_time.get() / update_interval;
+	_integrator.set_autoreset_interval(_param_imu_integ_time.get());
 }
 
 void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -135,6 +135,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams
 	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_TIME>) _param_imu_integ_time
 	)
 };

src/modules/sensors/vehicle_imu/imu_parameters.c

@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   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 period.
+*
+* The length of time in microseconds raw IMU data is integrated to produce
+* delta angles and delta velocities.
+*
+* @min 1000
+* @max 10000
+* @unit us
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_INT32(IMU_INTEG_TIME, 5000);

src/modules/simulator/simulator.h

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