diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 1e95b0aed4..0aff3f66ca 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/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 diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp index 0555820a45..9bfc0d55e0 100644 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ b/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; diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 5b9346e1b2..8e1fbd1077 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/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() diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index 38aa2c5864..e1b6a119e7 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/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() diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index f829c07a65..5fdd698bb9 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/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() diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp index 85abe6c778..e03ab991e3 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.cpp +++ b/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() diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp index 35c05cbc83..5a155322e5 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.cpp +++ b/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() diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp index 4a7d4d2404..5cca8dc89b 100644 --- a/src/drivers/imu/bmi088/BMI088_accel.cpp +++ b/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() diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp index 66daee80e5..88eabd46a6 100644 --- a/src/drivers/imu/bmi088/BMI088_gyro.cpp +++ b/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() diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 1ef0e3ed01..b2332ad499 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/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() diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index 742a4bf1fa..1c6e05da0f 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/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() diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index e56f61de67..f7af50f6aa 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp +++ b/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 diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp index efc5dade3e..e4f41610ca 100644 --- a/src/drivers/imu/l3gd20/L3GD20.cpp +++ b/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() diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index 414686d2dd..4c81fb1d83 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/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 diff --git a/src/drivers/imu/mpu6000/MPU6000.cpp b/src/drivers/imu/mpu6000/MPU6000.cpp index b815a63af1..55fa623c29 100644 --- a/src/drivers/imu/mpu6000/MPU6000.cpp +++ b/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); diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index efd5670918..7a1ee69967 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/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); diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index bd6811c131..3f2542e1bd 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/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 } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index e19c1ed9da..c73bb9a46a 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -46,7 +47,7 @@ #include #include -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) _param_imu_integ_rate + ) }; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index dba4784d01..b52be75688 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/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 } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index a7f05bce79..d6159566e2 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/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) _param_imu_gyro_rate_max + (ParamInt) _param_imu_gyro_rate_max, + (ParamInt) _param_imu_integ_rate ) }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 403c72ccb1..9a3db19570 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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 } } diff --git a/src/modules/sensors/vehicle_imu/imu_parameters.c b/src/modules/sensors/vehicle_imu/imu_parameters.c new file mode 100644 index 0000000000..33ac69f03a --- /dev/null +++ b/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); diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index e08dc0aeef..a4b667dc33 100644 --- a/src/modules/sih/sih.cpp +++ b/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() diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 79f1e45dde..acbf068b09 100644 --- a/src/modules/simulator/simulator.h +++ b/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()