From 050fa47acac5264c13eb2898851bdbd94417624a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 17 Jan 2021 12:57:44 -0500 Subject: [PATCH] IMU handle accel/gyro FIFO as separate type --- msg/sensor_accel.msg | 4 +- msg/sensor_accel_fifo.msg | 12 +- msg/sensor_gyro_fifo.msg | 12 +- .../px4_work_queue/WorkQueueManager.hpp | 2 +- .../imu/bosch/bmi055/BMI055_Accelerometer.cpp | 4 +- .../imu/bosch/bmi055/BMI055_Gyroscope.cpp | 4 +- src/drivers/imu/bosch/bmi055/CMakeLists.txt | 1 + .../imu/bosch/bmi088/BMI088_Accelerometer.cpp | 4 +- .../imu/bosch/bmi088/BMI088_Gyroscope.cpp | 4 +- src/drivers/imu/bosch/bmi088/CMakeLists.txt | 1 + .../bmi088_i2c/BMI088_Accelerometer.cpp | 4 +- .../bmi088/bmi088_i2c/BMI088_Gyroscope.cpp | 12 +- .../imu/invensense/icm20602/CMakeLists.txt | 3 +- .../imu/invensense/icm20602/ICM20602.cpp | 127 ++-- .../imu/invensense/icm20602/ICM20602.hpp | 14 +- .../imu/invensense/icm20608g/CMakeLists.txt | 1 + .../imu/invensense/icm20608g/ICM20608G.cpp | 8 +- .../imu/invensense/icm20649/CMakeLists.txt | 1 + .../imu/invensense/icm20649/ICM20649.cpp | 8 +- .../imu/invensense/icm20689/CMakeLists.txt | 1 + .../imu/invensense/icm20689/ICM20689.cpp | 127 ++-- .../imu/invensense/icm20689/ICM20689.hpp | 14 +- .../imu/invensense/icm20948/CMakeLists.txt | 1 + .../imu/invensense/icm20948/ICM20948.cpp | 8 +- .../imu/invensense/icm40609d/CMakeLists.txt | 1 + .../imu/invensense/icm40609d/ICM40609D.cpp | 8 +- .../imu/invensense/icm42605/CMakeLists.txt | 1 + .../imu/invensense/icm42605/ICM42605.cpp | 8 +- .../imu/invensense/icm42688p/ICM42688P.cpp | 8 +- .../imu/invensense/mpu6000/CMakeLists.txt | 1 + .../imu/invensense/mpu6000/MPU6000.cpp | 8 +- .../imu/invensense/mpu6500/CMakeLists.txt | 1 + .../imu/invensense/mpu6500/MPU6500.cpp | 8 +- .../imu/invensense/mpu9250/CMakeLists.txt | 1 + .../imu/invensense/mpu9250/MPU9250.cpp | 8 +- .../imu/invensense/mpu9250/MPU9250_I2C.cpp | 8 +- src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp | 4 +- .../magnetometer/isentek/ist8308/IST8308.cpp | 2 +- .../magnetometer/isentek/ist8310/IST8310.cpp | 2 +- .../magnetometer/qmc5883l/QMC5883L.cpp | 4 +- src/lib/conversion/rotation.cpp | 199 ------ src/lib/conversion/rotation.h | 246 +++++++- .../accelerometer/PX4Accelerometer.cpp | 143 ----- .../accelerometer/PX4Accelerometer.hpp | 70 ++- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 103 ---- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 62 +- src/lib/mathlib/math/Functions.hpp | 15 + .../mathlib/math/filter/LowPassFilter2p.cpp | 21 - .../mathlib/math/filter/LowPassFilter2p.hpp | 20 +- .../math/filter/LowPassFilter2pArray.hpp | 30 +- .../mathlib/math/filter/NotchFilterArray.hpp | 10 +- src/modules/sensors/sensors.cpp | 67 ++- .../VehicleAcceleration.cpp | 2 +- .../VehicleAngularVelocity.cpp | 458 +++++++++++--- .../VehicleAngularVelocity.hpp | 54 +- .../imu_gyro_parameters.c | 2 +- .../sensors/vehicle_imu/CMakeLists.txt | 2 + .../sensors/vehicle_imu/Integrator.cpp | 18 +- .../sensors/vehicle_imu/Integrator.hpp | 28 +- .../sensors/vehicle_imu/VehicleIMU.cpp | 78 +-- .../sensors/vehicle_imu/VehicleIMU.hpp | 4 +- .../sensors/vehicle_imu/VehicleImuFifo.cpp | 569 ++++++++++++++++++ .../sensors/vehicle_imu/VehicleImuFifo.hpp | 151 +++++ 63 files changed, 1840 insertions(+), 962 deletions(-) create mode 100644 src/modules/sensors/vehicle_imu/VehicleImuFifo.cpp create mode 100644 src/modules/sensors/vehicle_imu/VehicleImuFifo.hpp diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index f70cffd1db..f93a18c68c 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -3,6 +3,8 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles +float32 range + float32 x # acceleration in the FRD board frame X-axis in m/s^2 float32 y # acceleration in the FRD board frame Y-axis in m/s^2 float32 z # acceleration in the FRD board frame Z-axis in m/s^2 @@ -11,6 +13,4 @@ float32 temperature # temperature in degrees Celsius uint32 error_count -uint8[3] clip_counter # clip count per axis in the sample period - uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg index 0f79bf5018..4b87854c86 100644 --- a/msg/sensor_accel_fifo.msg +++ b/msg/sensor_accel_fifo.msg @@ -8,8 +8,12 @@ float32 scale uint8 samples # number of valid samples -int16[32] x # acceleration in the FRD board frame X-axis in m/s^2 -int16[32] y # acceleration in the FRD board frame Y-axis in m/s^2 -int16[32] z # acceleration in the FRD board frame Z-axis in m/s^2 +int16[20] x # acceleration in the sensor frame X-axis in m/s^2 +int16[20] y # acceleration in the sensor frame Y-axis in m/s^2 +int16[20] z # acceleration in the sensor frame Z-axis in m/s^2 -uint8 rotation # Direction the sensor faces (see Rotation enum) +float32 temperature # temperature in degrees celsius + +uint32 error_count + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg index 7f717a5d92..b0737858f8 100644 --- a/msg/sensor_gyro_fifo.msg +++ b/msg/sensor_gyro_fifo.msg @@ -8,8 +8,12 @@ float32 scale uint8 samples # number of valid samples -int16[32] x # angular velocity in the FRD board frame X-axis in rad/s -int16[32] y # angular velocity in the FRD board frame Y-axis in rad/s -int16[32] z # angular velocity in the FRD board frame Z-axis in rad/s +int16[20] x # angular velocity in the sensor frame X-axis in rad/s +int16[20] y # angular velocity in the sensor frame Y-axis in rad/s +int16[20] z # angular velocity in the sensor frame Z-axis in rad/s -uint8 rotation # Direction the sensor faces (see Rotation enum) +float32 temperature # temperature in degrees celsius + +uint32 error_count + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index a2e81faf4e..543bcc2f83 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -66,7 +66,7 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 2336, -11}; static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12}; // PX4 att/pos controllers, highest priority after sensors. -static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1760, -13}; +static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 2112, -13}; static constexpr wq_config_t INS0{"wq:INS0", 6000, -14}; static constexpr wq_config_t INS1{"wq:INS1", 6000, -15}; diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index a3d232fe17..50e7be0883 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -419,8 +419,8 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[i] = accel_x; - accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[i] = math::negate(accel_y); + accel.z[i] = math::negate(accel_z); } _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index 232d2d2ffa..363fa66cc5 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -416,8 +416,8 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/bosch/bmi055/CMakeLists.txt b/src/drivers/imu/bosch/bmi055/CMakeLists.txt index bf24faffde..2069d37e6e 100644 --- a/src/drivers/imu/bosch/bmi055/CMakeLists.txt +++ b/src/drivers/imu/bosch/bmi055/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__bosch__bmi055 MAIN bmi055 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS Bosch_BMI055_Accelerometer_Registers.hpp Bosch_BMI055_Gyroscope_Registers.hpp diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp index b60bf850e1..2d00f5b617 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -505,8 +505,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; fifo_buffer_index += 7; // move forward to next record diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index f853cc2313..924fcae9e0 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -417,8 +417,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/bosch/bmi088/CMakeLists.txt b/src/drivers/imu/bosch/bmi088/CMakeLists.txt index 8326953f07..644b147fd9 100644 --- a/src/drivers/imu/bosch/bmi088/CMakeLists.txt +++ b/src/drivers/imu/bosch/bmi088/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__bosch__bmi088 MAIN bmi088 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS Bosch_BMI088_Accelerometer_Registers.hpp Bosch_BMI088_Gyroscope_Registers.hpp diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp index ca6f6fcd0d..821b047e8e 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp @@ -419,8 +419,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; fifo_buffer_index += 7; // move forward to next record diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp index c1c710aa33..645156c552 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp @@ -364,8 +364,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + @@ -452,8 +452,8 @@ bool BMI088_Gyroscope::NormalRead(const hrt_abstime ×tamp_sample) // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) x = gyro_x; - y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + y = math::negate(gyro_y); + z = math::negate(gyro_z); _px4_gyro.update(timestamp_sample, x, y, z); @@ -502,8 +502,8 @@ bool BMI088_Gyroscope::SimpleFIFORead(const hrt_abstime ×tamp_sample) }; gyro.x[i] = xyz[0]; - gyro.y[i] = (xyz[1] == INT16_MIN) ? INT16_MAX : -xyz[1]; - gyro.z[i] = (xyz[2] == INT16_MIN) ? INT16_MAX : -xyz[2]; + gyro.y[i] = math::negate(xyz[1]); + gyro.z[i] = math::negate(xyz[2]); gyro.samples++; } diff --git a/src/drivers/imu/invensense/icm20602/CMakeLists.txt b/src/drivers/imu/invensense/icm20602/CMakeLists.txt index cd14e15674..79255373a6 100644 --- a/src/drivers/imu/invensense/icm20602/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20602/CMakeLists.txt @@ -35,13 +35,12 @@ px4_add_module( MODULE drivers__imu__invensense__icm20602 MAIN icm20602 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS ICM20602.cpp ICM20602.hpp icm20602_main.cpp InvenSense_ICM20602_registers.hpp DEPENDS - drivers_accelerometer - drivers_gyroscope px4_work_queue ) diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 9c7173d690..7dd254f782 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -33,6 +33,8 @@ #include "ICM20602.hpp" +#include + using namespace time_literals; static constexpr int16_t combine(uint8_t msb, uint8_t lsb) @@ -45,14 +47,15 @@ ICM20602::ICM20602(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro SPI(DRV_IMU_DEVTYPE_ICM20602, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) + _rotation(rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } - ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); + int32_t imu_gyro_rate_max = 800; // default to 800 Hz + param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max); + ConfigureSampleRate(imu_gyro_rate_max); } ICM20602::~ICM20602() @@ -74,6 +77,10 @@ int ICM20602::init() return ret; } + // advertise immediately for consistent ordering + _sensor_accel_fifo_pub.advertise(); + _sensor_gyro_fifo_pub.advertise(); + return Reset() ? 0 : -1; } @@ -278,67 +285,8 @@ void ICM20602::RunImpl() } } -void ICM20602::ConfigureAccel() -{ - const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0] - - switch (ACCEL_FS_SEL) { - case ACCEL_FS_SEL_2G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f); - _px4_accel.set_range(2.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_4G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f); - _px4_accel.set_range(4.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_8G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f); - _px4_accel.set_range(8.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_16G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); - _px4_accel.set_range(16.f * CONSTANTS_ONE_G); - break; - } -} - -void ICM20602::ConfigureGyro() -{ - const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0] - - float range_dps = 0.f; - - switch (FS_SEL) { - case FS_SEL_250_DPS: - range_dps = 250.f; - break; - - case FS_SEL_500_DPS: - range_dps = 500.f; - break; - - case FS_SEL_1000_DPS: - range_dps = 1000.f; - break; - - case FS_SEL_2000_DPS: - range_dps = 2000.f; - break; - } - - _px4_gyro.set_scale(math::radians(range_dps / 32768.f)); - _px4_gyro.set_range(math::radians(range_dps)); -} - void ICM20602::ConfigureSampleRate(int sample_rate) { - if (sample_rate == 0) { - sample_rate = 800; // default to 800 Hz - } - // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); @@ -387,9 +335,6 @@ bool ICM20602::Configure() } } - ConfigureAccel(); - ConfigureGyro(); - return success; } @@ -556,9 +501,6 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { sensor_accel_fifo_s accel{}; - accel.timestamp_sample = timestamp_sample; - accel.samples = 0; - accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; bool bad_data = false; @@ -591,16 +533,25 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); + + rotate_3i(_rotation, accel.x[accel.samples], accel.y[accel.samples], accel.z[accel.samples]); + accel.samples++; } - _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + - perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); - + // publish if (accel.samples > 0) { - _px4_accel.updateFIFO(accel); + accel.timestamp_sample = timestamp_sample; + accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; + accel.device_id = get_device_id(); + accel.temperature = _temperature_last; + accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count( + _fifo_empty_perf) + perf_event_count(_fifo_overflow_perf); + accel.scale = CONSTANTS_ONE_G / 2048.f; + accel.timestamp = hrt_absolute_time(); + _sensor_accel_fifo_pub.publish(accel); } return !bad_data; @@ -609,9 +560,6 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { sensor_gyro_fifo_s gyro{}; - gyro.timestamp_sample = timestamp_sample; - gyro.samples = samples; - gyro.dt = FIFO_SAMPLE_DT; for (int i = 0; i < samples; i++) { const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L); @@ -621,14 +569,23 @@ void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); + + rotate_3i(_rotation, gyro.x[i], gyro.y[i], gyro.z[i]); } - _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + - perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); - - _px4_gyro.updateFIFO(gyro); + // publish + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_SAMPLE_DT; + gyro.device_id = get_device_id(); + gyro.temperature = _temperature_last; + gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count( + _fifo_empty_perf) + perf_event_count(_fifo_overflow_perf); + gyro.scale = math::radians(2000.f / 32768.f); + gyro.timestamp = hrt_absolute_time(); + _sensor_gyro_fifo_pub.publish(gyro); } bool ICM20602::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples) @@ -659,8 +616,8 @@ bool ICM20602::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples && (temperature_C >= TEMPERATURE_SENSOR_MIN) && (temperature_C <= TEMPERATURE_SENSOR_MAX)) { - _px4_accel.set_temperature(temperature_C); - _px4_gyro.set_temperature(temperature_C); + _temperature_last = temperature_C; + return true; } else { diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index a1a2746c47..fc3fdc5e92 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -43,13 +43,14 @@ #include "InvenSense_ICM20602_registers.hpp" #include -#include #include -#include #include #include #include #include +#include +#include +#include using namespace InvenSense_ICM20602; @@ -102,8 +103,6 @@ private: bool Reset(); bool Configure(); - void ConfigureAccel(); - void ConfigureGyro(); void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); @@ -128,8 +127,11 @@ private: const spi_drdy_gpio_t _drdy_gpio; - PX4Accelerometer _px4_accel; - PX4Gyroscope _px4_gyro; + const enum Rotation _rotation {ROTATION_NONE}; + float _temperature_last{NAN}; + + uORB::PublicationMulti _sensor_accel_fifo_pub{ORB_ID(sensor_accel_fifo)}; + uORB::PublicationMulti _sensor_gyro_fifo_pub{ORB_ID(sensor_gyro_fifo)}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; diff --git a/src/drivers/imu/invensense/icm20608g/CMakeLists.txt b/src/drivers/imu/invensense/icm20608g/CMakeLists.txt index df58f3200b..acd49a74ee 100644 --- a/src/drivers/imu/invensense/icm20608g/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20608g/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__invensense__icm20608g MAIN icm20608g COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS ICM20608G.cpp ICM20608G.hpp diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 01c850ef37..28de67c797 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -544,8 +544,8 @@ bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } @@ -574,8 +574,8 @@ void ICM20608G::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm20649/CMakeLists.txt b/src/drivers/imu/invensense/icm20649/CMakeLists.txt index 905bfe227a..44b324ea94 100644 --- a/src/drivers/imu/invensense/icm20649/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20649/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_module( MODULE drivers__imu__invensense__icm20649 MAIN icm20649 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS ICM20649.cpp ICM20649.hpp diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index ca91e3cca9..5101b0d544 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -601,8 +601,8 @@ bool ICM20649::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } @@ -631,8 +631,8 @@ void ICM20649::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm20689/CMakeLists.txt b/src/drivers/imu/invensense/icm20689/CMakeLists.txt index e1ad8fcf26..04238d50c0 100644 --- a/src/drivers/imu/invensense/icm20689/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20689/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__invensense__icm20689 MAIN icm20689 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS ICM20689.cpp ICM20689.hpp diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 35105132e7..bae8cc0a4c 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -33,6 +33,8 @@ #include "ICM20689.hpp" +#include + using namespace time_literals; static constexpr int16_t combine(uint8_t msb, uint8_t lsb) @@ -45,14 +47,15 @@ ICM20689::ICM20689(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro SPI(DRV_IMU_DEVTYPE_ICM20689, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) + _rotation(rotation) { if (drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } - ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); + int32_t imu_gyro_rate_max = 800; // default to 800 Hz + param_get(param_find("IMU_GYRO_RATEMAX"), &imu_gyro_rate_max); + ConfigureSampleRate(imu_gyro_rate_max); } ICM20689::~ICM20689() @@ -74,6 +77,10 @@ int ICM20689::init() return ret; } + // advertise immediately for consistent ordering + _sensor_accel_fifo_pub.advertise(); + _sensor_gyro_fifo_pub.advertise(); + return Reset() ? 0 : -1; } @@ -274,67 +281,8 @@ void ICM20689::RunImpl() } } -void ICM20689::ConfigureAccel() -{ - const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0] - - switch (ACCEL_FS_SEL) { - case ACCEL_FS_SEL_2G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f); - _px4_accel.set_range(2.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_4G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f); - _px4_accel.set_range(4.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_8G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f); - _px4_accel.set_range(8.f * CONSTANTS_ONE_G); - break; - - case ACCEL_FS_SEL_16G: - _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); - _px4_accel.set_range(16.f * CONSTANTS_ONE_G); - break; - } -} - -void ICM20689::ConfigureGyro() -{ - const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0] - - float range_dps = 0.f; - - switch (FS_SEL) { - case FS_SEL_250_DPS: - range_dps = 250.f; - break; - - case FS_SEL_500_DPS: - range_dps = 500.f; - break; - - case FS_SEL_1000_DPS: - range_dps = 1000.f; - break; - - case FS_SEL_2000_DPS: - range_dps = 2000.f; - break; - } - - _px4_gyro.set_scale(math::radians(range_dps / 32768.f)); - _px4_gyro.set_range(math::radians(range_dps)); -} - void ICM20689::ConfigureSampleRate(int sample_rate) { - if (sample_rate == 0) { - sample_rate = 800; // default to 800 Hz - } - // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); @@ -361,9 +309,6 @@ bool ICM20689::Configure() } } - ConfigureAccel(); - ConfigureGyro(); - return success; } @@ -473,7 +418,6 @@ bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) return false; } - ProcessGyro(timestamp_sample, buffer.f, samples); return ProcessAccel(timestamp_sample, buffer.f, samples); } @@ -509,9 +453,6 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { sensor_accel_fifo_s accel{}; - accel.timestamp_sample = timestamp_sample; - accel.samples = 0; - accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; bool bad_data = false; @@ -544,16 +485,25 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); + + rotate_3i(_rotation, accel.x[accel.samples], accel.y[accel.samples], accel.z[accel.samples]); + accel.samples++; } - _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + - perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); - + // publish if (accel.samples > 0) { - _px4_accel.updateFIFO(accel); + accel.timestamp_sample = timestamp_sample; + accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; + accel.device_id = get_device_id(); + accel.temperature = _temperature_last; + accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count( + _fifo_empty_perf) + perf_event_count(_fifo_overflow_perf); + accel.scale = CONSTANTS_ONE_G / 2048.f; + accel.timestamp = hrt_absolute_time(); + _sensor_accel_fifo_pub.publish(accel); } return !bad_data; @@ -562,9 +512,6 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { sensor_gyro_fifo_s gyro{}; - gyro.timestamp_sample = timestamp_sample; - gyro.samples = samples; - gyro.dt = FIFO_SAMPLE_DT; for (int i = 0; i < samples; i++) { const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L); @@ -574,14 +521,23 @@ void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); + + rotate_3i(_rotation, gyro.x[i], gyro.y[i], gyro.z[i]); } - _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + - perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); - - _px4_gyro.updateFIFO(gyro); + // publish + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_SAMPLE_DT; + gyro.device_id = get_device_id(); + gyro.temperature = _temperature_last; + gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count( + _fifo_empty_perf) + perf_event_count(_fifo_overflow_perf); + gyro.scale = math::radians(2000.f / 32768.f); + gyro.timestamp = hrt_absolute_time(); + _sensor_gyro_fifo_pub.publish(gyro); } void ICM20689::UpdateTemperature() @@ -599,7 +555,6 @@ void ICM20689::UpdateTemperature() const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; if (PX4_ISFINITE(TEMP_degC)) { - _px4_accel.set_temperature(TEMP_degC); - _px4_gyro.set_temperature(TEMP_degC); + _temperature_last = TEMP_degC; } } diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index fec7fe3c6e..69b7cbbb90 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -43,13 +43,14 @@ #include "InvenSense_ICM20689_registers.hpp" #include -#include #include -#include #include #include #include #include +#include +#include +#include using namespace InvenSense_ICM20689; @@ -100,8 +101,6 @@ private: bool Reset(); bool Configure(); - void ConfigureAccel(); - void ConfigureGyro(); void ConfigureSampleRate(int sample_rate); static int DataReadyInterruptCallback(int irq, void *context, void *arg); @@ -125,8 +124,11 @@ private: const spi_drdy_gpio_t _drdy_gpio; - PX4Accelerometer _px4_accel; - PX4Gyroscope _px4_gyro; + const enum Rotation _rotation {ROTATION_NONE}; + float _temperature_last{NAN}; + + uORB::PublicationMulti _sensor_accel_fifo_pub{ORB_ID(sensor_accel_fifo)}; + uORB::PublicationMulti _sensor_gyro_fifo_pub{ORB_ID(sensor_gyro_fifo)}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; diff --git a/src/drivers/imu/invensense/icm20948/CMakeLists.txt b/src/drivers/imu/invensense/icm20948/CMakeLists.txt index 3d4965bed6..1a3e51f640 100644 --- a/src/drivers/imu/invensense/icm20948/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20948/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_module( MODULE drivers__imu__icm20948 MAIN icm20948 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS AKM_AK09916_registers.hpp ICM20948.cpp diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index f8072490d8..b09ce10627 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -646,8 +646,8 @@ bool ICM20948::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } @@ -676,8 +676,8 @@ void ICM20948::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm40609d/CMakeLists.txt b/src/drivers/imu/invensense/icm40609d/CMakeLists.txt index 16ee7e8019..535cd0ae5d 100644 --- a/src/drivers/imu/invensense/icm40609d/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm40609d/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_module( MODULE drivers__imu__invensense__icm40609d MAIN icm40609d COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS icm40609d_main.cpp ICM40609D.cpp diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 965494e21b..1c9ef0c5e7 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -603,8 +603,8 @@ void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } @@ -631,8 +631,8 @@ void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm42605/CMakeLists.txt b/src/drivers/imu/invensense/icm42605/CMakeLists.txt index fdb1ac7c0c..3f44a19463 100644 --- a/src/drivers/imu/invensense/icm42605/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm42605/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_module( MODULE drivers__imu__invensense__icm42605 MAIN icm42605 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS icm42605_main.cpp ICM42605.cpp diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp index 2d6c233016..bd6d80feb8 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.cpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.cpp @@ -604,8 +604,8 @@ void ICM42605::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } @@ -632,8 +632,8 @@ void ICM42605::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 1d9916dad8..b29990a556 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -653,8 +653,8 @@ void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[i] = accel.x[i]; - accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i]; - accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i]; + accel.y[i] = math::negate(accel.y[i]); + accel.z[i] = math::negate(accel.z[i]); } _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + @@ -725,8 +725,8 @@ void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro.x[i]; - gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i]; - gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i]; + gyro.y[i] = math::negate(gyro.y[i]); + gyro.z[i] = math::negate(gyro.z[i]); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/mpu6000/CMakeLists.txt b/src/drivers/imu/invensense/mpu6000/CMakeLists.txt index 65af48a8e8..ae00881976 100644 --- a/src/drivers/imu/invensense/mpu6000/CMakeLists.txt +++ b/src/drivers/imu/invensense/mpu6000/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__invensense__mpu6000 MAIN mpu6000 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS MPU6000.cpp MPU6000.hpp diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index e843c088af..625206fe13 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -535,8 +535,8 @@ bool MPU6000::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } else if (new_sample && (_fifo_accel_samples_count > 1)) { @@ -577,8 +577,8 @@ void MPU6000::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt index c11ca2d64c..f31d83c66c 100644 --- a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt +++ b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__invensense__mpu6500 MAIN mpu6500 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS MPU6500.cpp MPU6500.hpp diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index eecb9a526f..173e3cae00 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -548,8 +548,8 @@ bool MPU6500::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } @@ -578,8 +578,8 @@ void MPU6500::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/mpu9250/CMakeLists.txt b/src/drivers/imu/invensense/mpu9250/CMakeLists.txt index a8afaa9f8c..0568f0edb7 100644 --- a/src/drivers/imu/invensense/mpu9250/CMakeLists.txt +++ b/src/drivers/imu/invensense/mpu9250/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__invensense__mpu9250 MAIN mpu9250 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS AKM_AK8963_registers.hpp InvenSense_MPU9250_registers.hpp diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 0e3b66f83e..4dca050d81 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -582,8 +582,8 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } @@ -612,8 +612,8 @@ void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp index 234ef80b8f..cdcd6e27f7 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp @@ -529,8 +529,8 @@ bool MPU9250_I2C::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO:: // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } @@ -559,8 +559,8 @@ void MPU9250_I2C::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::D // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index 6a12447992..448dbd90c4 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -357,7 +357,7 @@ bool LSM9DS1::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[gyro.samples] = gyro_x; gyro.y[gyro.samples] = gyro_y; - gyro.z[gyro.samples] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.z[gyro.samples] = math::negate(gyro_z); gyro.samples++; } else { @@ -385,7 +385,7 @@ bool LSM9DS1::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; accel.y[accel.samples] = accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.z[accel.samples] = math::negate(accel_y); accel.samples++; } else { diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index 1dcedf31cf..c7ddf52b2d 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -179,7 +179,7 @@ void IST8308::RunImpl() int16_t z = combine(buffer.DATAZH, buffer.DATAZL); // sensor's frame is +x forward, +y right, +z up - z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z + z = math::negate(z); // flip z _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); _px4_mag.update(now, x, y, z); diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index 4a5cf6c536..2a6af596dd 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -185,7 +185,7 @@ void IST8310::RunImpl() int16_t z = combine(buffer.DATAZH, buffer.DATAZL); // sensor's frame is +x forward, +y right, +z up - z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z + z = math::negate(z); // flip z _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); _px4_mag.update(now, x, y, z); diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp index 3efc443711..dd37b818d4 100644 --- a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp @@ -196,8 +196,8 @@ void QMC5883L::RunImpl() // Forward X := +X // Right Y := -Y // Down Z := -Z - y = (y == INT16_MIN) ? INT16_MAX : -y; // -y - z = (z == INT16_MIN) ? INT16_MAX : -z; // -z + y = math::negate(y); // -y + z = math::negate(z); // -z _px4_mag.update(now, x, y, z); diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index a19842c915..f7d9887ee0 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -56,202 +56,3 @@ get_rot_quaternion(enum Rotation rot) math::radians((float)rot_lookup[rot].pitch), math::radians((float)rot_lookup[rot].yaw)}}; } - -__EXPORT void -rotate_3f(enum Rotation rot, float &x, float &y, float &z) -{ - switch (rot) { - case ROTATION_NONE: - break; - - case ROTATION_YAW_90: { - float tmp = x; - x = -y; - y = tmp; - } - break; - - case ROTATION_YAW_180: { - x = -x; - y = -y; - } - break; - - case ROTATION_YAW_270: { - float tmp = x; - x = y; - y = -tmp; - } - break; - - case ROTATION_ROLL_180: { - y = -y; - z = -z; - } - break; - - case ROTATION_ROLL_180_YAW_90: - - // FALLTHROUGH - case ROTATION_PITCH_180_YAW_270: { - float tmp = x; - x = y; - y = tmp; - z = -z; - } - break; - - case ROTATION_PITCH_180: { - x = -x; - z = -z; - } - break; - - case ROTATION_ROLL_180_YAW_270: - - // FALLTHROUGH - case ROTATION_PITCH_180_YAW_90: { - float tmp = x; - x = -y; - y = -tmp; - z = -z; - } - break; - - case ROTATION_ROLL_90: { - float tmp = z; - z = y; - y = -tmp; - } - break; - - case ROTATION_ROLL_90_YAW_90: { - float tmp = x; - x = z; - z = y; - y = tmp; - } - break; - - case ROTATION_ROLL_270: { - float tmp = z; - z = -y; - y = tmp; - } - break; - - case ROTATION_ROLL_270_YAW_90: { - float tmp = x; - x = -z; - z = -y; - y = tmp; - } - break; - - case ROTATION_PITCH_90: { - float tmp = z; - z = -x; - x = tmp; - } - break; - - case ROTATION_PITCH_270: { - float tmp = z; - z = x; - x = -tmp; - } - break; - - case ROTATION_ROLL_180_PITCH_270: { - float tmp = z; - z = x; - x = tmp; - y = -y; - } - break; - - case ROTATION_ROLL_90_YAW_270: { - float tmp = x; - x = -z; - z = y; - y = -tmp; - } - break; - - case ROTATION_ROLL_90_PITCH_90: { - float tmp = x; - x = y; - y = -z; - z = -tmp; - } - break; - - case ROTATION_ROLL_180_PITCH_90: { - float tmp = x; - x = -z; - y = -y; - z = -tmp; - } - break; - - case ROTATION_ROLL_270_PITCH_90: { - float tmp = x; - x = -y; - y = z; - z = -tmp; - } - break; - - case ROTATION_ROLL_90_PITCH_180: { - float tmp = y; - x = -x; - y = -z; - z = -tmp; - } - break; - - case ROTATION_ROLL_270_PITCH_180: { - float tmp = y; - x = -x; - y = z; - z = tmp; - } - break; - - case ROTATION_ROLL_90_PITCH_270: { - float tmp = x; - x = -y; - y = -z; - z = tmp; - } - break; - - case ROTATION_ROLL_270_PITCH_270: { - float tmp = x; - x = y; - y = z; - z = tmp; - } - break; - - case ROTATION_ROLL_90_PITCH_180_YAW_90: { - float tmp = x; - x = z; - z = -y; - y = -tmp; - } - break; - - default: - - // otherwise use full rotation matrix for valid rotations - if (rot < ROTATION_MAX) { - const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}}; - x = r(0); - y = r(1); - z = r(2); - } - - break; - } -} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index decb9f4b6a..dcabb2a67c 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -158,11 +158,251 @@ get_rot_matrix(enum Rotation rot); __EXPORT matrix::Quatf get_rot_quaternion(enum Rotation rot); +template +static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z) +{ + switch (rot) { + case ROTATION_NONE: + return true; + + case ROTATION_YAW_90: { + T tmp = x; + x = math::negate(y); + y = tmp; + } + + return true; + + case ROTATION_YAW_180: { + x = math::negate(x); + y = math::negate(y); + } + + return true; + + case ROTATION_YAW_270: { + T tmp = x; + x = y; + y = math::negate(tmp); + } + + return true; + + case ROTATION_ROLL_180: { + y = math::negate(y); + z = math::negate(z); + } + + return true; + + case ROTATION_ROLL_180_YAW_90: + + // FALLTHROUGH + case ROTATION_PITCH_180_YAW_270: { + T tmp = x; + x = y; + y = tmp; + z = math::negate(z); + } + + return true; + + case ROTATION_PITCH_180: { + x = math::negate(x); + z = math::negate(z); + } + + return true; + + case ROTATION_ROLL_180_YAW_270: + + // FALLTHROUGH + case ROTATION_PITCH_180_YAW_90: { + T tmp = x; + x = math::negate(y); + y = math::negate(tmp); + z = math::negate(z); + } + + return true; + + case ROTATION_ROLL_90: { + T tmp = z; + z = y; + y = math::negate(tmp); + } + + return true; + + case ROTATION_ROLL_90_YAW_90: { + T tmp = x; + x = z; + z = y; + y = tmp; + } + + return true; + + case ROTATION_ROLL_270: { + T tmp = z; + z = math::negate(y); + y = tmp; + } + + return true; + + case ROTATION_ROLL_270_YAW_90: { + T tmp = x; + x = math::negate(z); + z = math::negate(y); + y = tmp; + } + + return true; + + case ROTATION_PITCH_90: { + T tmp = z; + z = math::negate(x); + x = tmp; + } + + return true; + + case ROTATION_PITCH_270: { + T tmp = z; + z = x; + x = math::negate(tmp); + } + + return true; + + case ROTATION_ROLL_180_PITCH_270: { + T tmp = z; + z = x; + x = tmp; + y = math::negate(y); + } + + return true; + + case ROTATION_ROLL_90_YAW_270: { + T tmp = x; + x = math::negate(z); + z = y; + y = math::negate(tmp); + } + + return true; + + case ROTATION_ROLL_90_PITCH_90: { + T tmp = x; + x = y; + y = math::negate(z); + z = math::negate(tmp); + } + + return true; + + case ROTATION_ROLL_180_PITCH_90: { + T tmp = x; + x = math::negate(z); + y = math::negate(y); + z = math::negate(tmp); + } + + return true; + + case ROTATION_ROLL_270_PITCH_90: { + T tmp = x; + x = math::negate(y); + y = z; + z = math::negate(tmp); + } + + return true; + + case ROTATION_ROLL_90_PITCH_180: { + T tmp = y; + x = math::negate(x); + y = math::negate(z); + z = math::negate(tmp); + } + + return true; + + case ROTATION_ROLL_270_PITCH_180: { + T tmp = y; + x = math::negate(x); + y = z; + z = tmp; + } + + return true; + + case ROTATION_ROLL_90_PITCH_270: { + T tmp = x; + x = math::negate(y); + y = math::negate(z); + z = tmp; + } + + return true; + + case ROTATION_ROLL_270_PITCH_270: { + T tmp = x; + x = y; + y = z; + z = tmp; + } + + return true; + + case ROTATION_ROLL_90_PITCH_180_YAW_90: { + T tmp = x; + x = z; + z = math::negate(y); + y = math::negate(tmp); + } + + return true; + + default: + break; + } + + return false; +} + +/** + * rotate a 3 element int16_t vector in-place + */ +__EXPORT inline void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z) +{ + if (!rotate_3(rot, x, y, z)) { + // otherwise use full rotation matrix for valid rotations + if (rot < ROTATION_MAX) { + const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}}; + x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); + y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); + z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); + } + } +} + /** * rotate a 3 element float vector in-place */ -__EXPORT void -rotate_3f(enum Rotation rot, float &x, float &y, float &z); - +__EXPORT inline void rotate_3f(enum Rotation rot, float &x, float &y, float &z) +{ + if (!rotate_3(rot, x, y, z)) { + // otherwise use full rotation matrix for valid rotations + if (rot < ROTATION_MAX) { + const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}}; + x = r(0); + y = r(1); + z = r(2); + } + } +} #endif /* ROTATION_H_ */ diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index f54feec673..19d0046a27 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -33,146 +33,3 @@ #include "PX4Accelerometer.hpp" - -#include -#include - -using namespace time_literals; -using matrix::Vector3f; - -static constexpr int32_t sum(const int16_t samples[16], uint8_t len) -{ - int32_t sum = 0; - - for (int n = 0; n < len; n++) { - sum += samples[n]; - } - - return sum; -} - -static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len) -{ - unsigned clip_count = 0; - - for (int n = 0; n < len; n++) { - if (abs(samples[n]) >= clip_limit) { - clip_count++; - } - } - - return clip_count; -} - -PX4Accelerometer::PX4Accelerometer(uint32_t device_id, enum Rotation rotation) : - _device_id{device_id}, - _rotation{rotation} -{ - // advertise immediately to keep instance numbering in sync - _sensor_pub.advertise(); - - param_get(param_find("IMU_GYRO_RATEMAX"), &_imu_gyro_rate_max); -} - -PX4Accelerometer::~PX4Accelerometer() -{ - _sensor_pub.unadvertise(); - _sensor_fifo_pub.unadvertise(); -} - -void PX4Accelerometer::set_device_type(uint8_t devtype) -{ - // current DeviceStructure - union device::Device::DeviceId device_id; - device_id.devid = _device_id; - - // update to new device type - device_id.devid_s.devtype = devtype; - - // copy back - _device_id = device_id.devid; -} - -void PX4Accelerometer::update(const hrt_abstime ×tamp_sample, float x, float y, float z) -{ - // clipping - uint8_t clip_count[3]; - clip_count[0] = (fabsf(x) >= _clip_limit); - clip_count[1] = (fabsf(y) >= _clip_limit); - clip_count[2] = (fabsf(z) >= _clip_limit); - - // publish - Publish(timestamp_sample, x, y, z, clip_count); -} - -void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample) -{ - // publish fifo - sample.device_id = _device_id; - sample.scale = _scale; - sample.rotation = _rotation; - - sample.timestamp = hrt_absolute_time(); - _sensor_fifo_pub.publish(sample); - - { - // trapezoidal integration (equally spaced, scaled by dt later) - const uint8_t N = sample.samples; - const Vector3f integral{ - (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)), - (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)), - (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)), - }; - - _last_sample[0] = sample.x[N - 1]; - _last_sample[1] = sample.y[N - 1]; - _last_sample[2] = sample.z[N - 1]; - - // clipping - uint8_t clip_count[3] { - clipping(sample.x, _clip_limit, N), - clipping(sample.y, _clip_limit, N), - clipping(sample.z, _clip_limit, N), - }; - - const float x = integral(0) / (float)N; - const float y = integral(1) / (float)N; - const float z = integral(2) / (float)N; - - // publish - Publish(sample.timestamp_sample, x, y, z, clip_count); - } -} - -void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]) -{ - // Apply rotation (before scaling) - rotate_3f(_rotation, x, y, z); - - float clipping_x = clip_count[0]; - float clipping_y = clip_count[1]; - float clipping_z = clip_count[2]; - rotate_3f(_rotation, clipping_x, clipping_y, clipping_z); - - sensor_accel_s report; - - report.timestamp_sample = timestamp_sample; - report.device_id = _device_id; - report.temperature = _temperature; - report.error_count = _error_count; - report.x = x * _scale; - report.y = y * _scale; - report.z = z * _scale; - report.clip_counter[0] = fabsf(roundf(clipping_x)); - report.clip_counter[1] = fabsf(roundf(clipping_y)); - report.clip_counter[2] = fabsf(roundf(clipping_z)); - report.timestamp = hrt_absolute_time(); - - _sensor_pub.publish(report); -} - -void PX4Accelerometer::UpdateClipLimit() -{ - // 99.9% of potential max - _clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX); -} diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 9fd673ae83..ae8cf5c3fa 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -40,32 +40,80 @@ #include #include +#include +#include + class PX4Accelerometer { public: - PX4Accelerometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE); - ~PX4Accelerometer(); + PX4Accelerometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE) : + _device_id{device_id}, + _rotation{rotation} + { + param_get(param_find("IMU_GYRO_RATEMAX"), &_imu_gyro_rate_max); + } + + ~PX4Accelerometer() + { + _sensor_pub.unadvertise(); + _sensor_fifo_pub.unadvertise(); + } uint32_t get_device_id() const { return _device_id; } int32_t get_max_rate_hz() const { return _imu_gyro_rate_max; } void set_device_id(uint32_t device_id) { _device_id = device_id; } - void set_device_type(uint8_t devtype); + void set_device_type(uint8_t devtype) + { + union device::Device::DeviceId device_id; + device_id.devid = _device_id; + device_id.devid_s.devtype = devtype; // update to new device type + _device_id = device_id.devid; // copy back + } + void set_error_count(uint32_t error_count) { _error_count = error_count; } void increase_error_count() { _error_count++; } - void set_range(float range) { _range = range; UpdateClipLimit(); } - void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } + void set_range(float range) { _range = range; } + void set_scale(float scale) { _scale = scale; } void set_temperature(float temperature) { _temperature = temperature; } - void update(const hrt_abstime ×tamp_sample, float x, float y, float z); + void update(const hrt_abstime ×tamp_sample, float x, float y, float z) + { + // Apply rotation (before scaling) + rotate_3f(_rotation, x, y, z); - void updateFIFO(sensor_accel_fifo_s &sample); + sensor_accel_s report; + + report.timestamp_sample = timestamp_sample; + report.device_id = _device_id; + report.temperature = _temperature; + report.error_count = _error_count; + report.x = x * _scale; + report.y = y * _scale; + report.z = z * _scale; + report.timestamp = hrt_absolute_time(); + + _sensor_pub.publish(report); + } + + void updateFIFO(sensor_accel_fifo_s &sample) + { + // publish fifo + sample.device_id = _device_id; + sample.temperature = _temperature; + sample.error_count = _error_count; + sample.scale = _scale; + + for (int i = 0; i < sample.samples; i++) { + rotate_3i(_rotation, sample.x[i], sample.y[i], sample.z[i]); + } + + sample.timestamp = hrt_absolute_time(); + _sensor_fifo_pub.publish(sample); + } private: - void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]); - void UpdateClipLimit(); - uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_accel)}; uORB::PublicationMulti _sensor_fifo_pub{ORB_ID(sensor_accel_fifo)}; @@ -78,8 +126,6 @@ private: float _scale{1.f}; float _temperature{NAN}; - float _clip_limit{_range / _scale}; - uint32_t _error_count{0}; int16_t _last_sample[3] {}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index f70f41c758..d06780c615 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -33,106 +33,3 @@ #include "PX4Gyroscope.hpp" - -#include -#include - -using namespace time_literals; -using matrix::Vector3f; - -static constexpr int32_t sum(const int16_t samples[16], uint8_t len) -{ - int32_t sum = 0; - - for (int n = 0; n < len; n++) { - sum += samples[n]; - } - - return sum; -} - -PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) : - _device_id{device_id}, - _rotation{rotation} -{ - // advertise immediately to keep instance numbering in sync - _sensor_pub.advertise(); - - param_get(param_find("IMU_GYRO_RATEMAX"), &_imu_gyro_rate_max); -} - -PX4Gyroscope::~PX4Gyroscope() -{ - _sensor_pub.unadvertise(); - _sensor_fifo_pub.unadvertise(); -} - -void PX4Gyroscope::set_device_type(uint8_t devtype) -{ - // current DeviceStructure - union device::Device::DeviceId device_id; - device_id.devid = _device_id; - - // update to new device type - device_id.devid_s.devtype = devtype; - - // copy back - _device_id = device_id.devid; -} - -void PX4Gyroscope::update(const hrt_abstime ×tamp_sample, float x, float y, float z) -{ - // publish - Publish(timestamp_sample, x, y, z); -} - -void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample) -{ - // publish fifo - sample.device_id = _device_id; - sample.scale = _scale; - sample.rotation = _rotation; - - sample.timestamp = hrt_absolute_time(); - _sensor_fifo_pub.publish(sample); - - { - // trapezoidal integration (equally spaced, scaled by dt later) - const uint8_t N = sample.samples; - const Vector3f integral{ - (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)), - (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)), - (0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)), - }; - - _last_sample[0] = sample.x[N - 1]; - _last_sample[1] = sample.y[N - 1]; - _last_sample[2] = sample.z[N - 1]; - - const float x = integral(0) / (float)N; - const float y = integral(1) / (float)N; - const float z = integral(2) / (float)N; - - // publish - Publish(sample.timestamp_sample, x, y, z); - } -} - -void PX4Gyroscope::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z) -{ - // Apply rotation (before scaling) - rotate_3f(_rotation, x, y, z); - - sensor_gyro_s report; - - report.timestamp_sample = timestamp_sample; - report.device_id = _device_id; - report.temperature = _temperature; - report.error_count = _error_count; - report.x = x * _scale; - report.y = y * _scale; - report.z = z * _scale; - report.timestamp = hrt_absolute_time(); - - _sensor_pub.publish(report); -} diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 6c2bcaeb13..4d0bf8ecab 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -39,31 +39,79 @@ #include #include +#include +#include + class PX4Gyroscope { public: - PX4Gyroscope(uint32_t device_id, enum Rotation rotation = ROTATION_NONE); - ~PX4Gyroscope(); + PX4Gyroscope(uint32_t device_id, enum Rotation rotation = ROTATION_NONE) : + _device_id{device_id}, + _rotation{rotation} + { + param_get(param_find("IMU_GYRO_RATEMAX"), &_imu_gyro_rate_max); + } + + ~PX4Gyroscope() + { + _sensor_pub.unadvertise(); + _sensor_fifo_pub.unadvertise(); + } uint32_t get_device_id() const { return _device_id; } int32_t get_max_rate_hz() const { return _imu_gyro_rate_max; } void set_device_id(uint32_t device_id) { _device_id = device_id; } - void set_device_type(uint8_t devtype); + void set_device_type(uint8_t devtype) + { + union device::Device::DeviceId device_id; + device_id.devid = _device_id; + device_id.devid_s.devtype = devtype; // update to new device type + _device_id = device_id.devid; // copy back + } + void set_error_count(uint32_t error_count) { _error_count = error_count; } void increase_error_count() { _error_count++; } void set_range(float range) { _range = range; } void set_scale(float scale) { _scale = scale; } void set_temperature(float temperature) { _temperature = temperature; } - void update(const hrt_abstime ×tamp_sample, float x, float y, float z); + void update(const hrt_abstime ×tamp_sample, float x, float y, float z) + { + // Apply rotation (before scaling) + rotate_3f(_rotation, x, y, z); - void updateFIFO(sensor_gyro_fifo_s &sample); + sensor_gyro_s report; + report.timestamp_sample = timestamp_sample; + report.device_id = _device_id; + report.temperature = _temperature; + report.error_count = _error_count; + report.x = x * _scale; + report.y = y * _scale; + report.z = z * _scale; + report.timestamp = hrt_absolute_time(); + + _sensor_pub.publish(report); + } + + void updateFIFO(sensor_gyro_fifo_s &sample) + { + // publish fifo + sample.device_id = _device_id; + sample.temperature = _temperature; + sample.error_count = _error_count; + sample.scale = _scale; + + for (int i = 0; i < sample.samples; i++) { + rotate_3i(_rotation, sample.x[i], sample.y[i], sample.z[i]); + } + + sample.timestamp = hrt_absolute_time(); + _sensor_fifo_pub.publish(sample); + } private: - void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z); - uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_gyro)}; uORB::PublicationMulti _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)}; diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 79c7dd56ce..defe973bf4 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -198,4 +198,19 @@ const T sqrt_linear(const T &value) } } +template +constexpr T negate(T value) +{ + return -value; +} + +template<> +constexpr int16_t negate(int16_t value) +{ + return (value == INT16_MIN) ? INT16_MAX : -value; +} + +//int16_t negate(int16_t value) { return (value == INT16_MIN) ? INT16_MAX : -value; } +//float negate(float value) { return -1.f * value; } + } /* namespace math */ diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 55e8cdc513..4a30440451 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -33,8 +33,6 @@ #include "LowPassFilter2p.hpp" -#include - #include namespace math @@ -72,25 +70,6 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) _a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c; } -float LowPassFilter2p::apply(float sample) -{ - // do the filtering - float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; - - if (!PX4_ISFINITE(delay_element_0)) { - // don't allow bad values to propagate via the filter - delay_element_0 = sample; - } - - const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; - - _delay_element_2 = _delay_element_1; - _delay_element_1 = delay_element_0; - - // return the value. Should be no need to check limits - return output; -} - float LowPassFilter2p::reset(float sample) { const float dval = sample / (_b0 + _b1 + _b2); diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 7d6c77caae..789fdcd688 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -38,6 +38,8 @@ #pragma once +#include + namespace math { class __EXPORT LowPassFilter2p @@ -58,7 +60,23 @@ public: * * @return retrieve the filtered result */ - float apply(float sample); + inline float apply(float sample) + { + // do the filtering + float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; + + if (!PX4_ISFINITE(delay_element_0)) { + // don't allow bad values to propagate via the filter + delay_element_0 = sample; + } + + const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + + _delay_element_2 = _delay_element_1; + _delay_element_1 = delay_element_0; + + return output; + } // Return the cutoff frequency float get_cutoff_freq() const { return _cutoff_freq; } diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp index 023a4abe90..28846fcd0a 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp @@ -51,35 +51,23 @@ public: { } - /** - * Add a new raw value to the filter - * - * @return retrieve the filtered result - */ - inline float apply(const int16_t samples[], uint8_t num_samples) + // Filter array of samples in place using the Direct form II. + inline void apply(float samples[], uint8_t num_samples) { - float output = 0.0f; - for (int n = 0; n < num_samples; n++) { - // do the filtering - float delay_element_0 = samples[n] - _delay_element_1 * _a1 - _delay_element_2 * _a2; + // Direct Form II implementation + float delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2}; - if (n == num_samples - 1) { - output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + // don't allow bad values to propagate via the filter + if (!PX4_ISFINITE(delay_element_0)) { + delay_element_0 = samples[n]; } + samples[n] = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + _delay_element_2 = _delay_element_1; _delay_element_1 = delay_element_0; } - - // don't allow bad values to propagate via the filter - if (!PX4_ISFINITE(output)) { - reset(samples[num_samples - 1]); - output = samples[num_samples - 1]; - } - - // return the value. Should be no need to check limits - return output; } }; diff --git a/src/lib/mathlib/math/filter/NotchFilterArray.hpp b/src/lib/mathlib/math/filter/NotchFilterArray.hpp index ffe86c0d05..eff13b868c 100644 --- a/src/lib/mathlib/math/filter/NotchFilterArray.hpp +++ b/src/lib/mathlib/math/filter/NotchFilterArray.hpp @@ -64,11 +64,7 @@ public: NotchFilterArray() = default; ~NotchFilterArray() = default; - /** - * Add new raw values to the filter using the Direct form II. - * - * @return retrieve the filtered result - */ + // Filter array of samples in place using the Direct form II. inline void apply(T samples[], uint8_t num_samples) { for (int n = 0; n < num_samples; n++) { @@ -96,8 +92,8 @@ public: { for (int n = 0; n < num_samples; n++) { // Direct Form II implementation - const T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - - _a2 * _delay_element_output_2; + T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - + _a2 * _delay_element_output_2; // don't allow bad values to propagate via the filter if (!isFinite(output)) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8cf3faa1f8..5fe2bdc3e1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -75,6 +76,7 @@ #include "vehicle_air_data/VehicleAirData.hpp" #include "vehicle_gps_position/VehicleGPSPosition.hpp" #include "vehicle_imu/VehicleIMU.hpp" +#include "vehicle_imu/VehicleImuFifo.hpp" #include "vehicle_magnetometer/VehicleMagnetometer.hpp" using namespace sensors; @@ -177,7 +179,8 @@ private: VehicleMagnetometer *_vehicle_magnetometer{nullptr}; VehicleGPSPosition *_vehicle_gps_position{nullptr}; - VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; + VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; + VehicleImuFifo *_vehicle_imu_fifo_list[MAX_SENSOR_COUNT] {}; /** * Update our local parameter cache. @@ -280,6 +283,13 @@ Sensors::~Sensors() } } + for (auto &vehicle_imu_fifo : _vehicle_imu_fifo_list) { + if (vehicle_imu_fifo) { + vehicle_imu_fifo->Stop(); + delete vehicle_imu_fifo; + } + } + perf_free(_loop_perf); } @@ -508,22 +518,16 @@ void Sensors::InitializeVehicleIMU() // create a VehicleIMU instance for each accel/gyro pair for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { if (_vehicle_imu_list[i] == nullptr) { + uORB::SubscriptionData accel_sub{ORB_ID(sensor_accel), i}; + uORB::SubscriptionData gyro_sub{ORB_ID(sensor_gyro), i}; - uORB::Subscription accel_sub{ORB_ID(sensor_accel), i}; - sensor_accel_s accel{}; - accel_sub.copy(&accel); - - uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i}; - sensor_gyro_s gyro{}; - gyro_sub.copy(&gyro); - - if (accel.device_id > 0 && gyro.device_id > 0) { + if (accel_sub.get().device_id > 0 && gyro_sub.get().device_id > 0) { // if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ // otherwise each VehicleIMU runs in a corresponding INSx WQ const bool multi_mode = (_param_sens_imu_mode.get() == 0); const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0; - VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config); + VehicleIMU *imu = new VehicleIMU(i, i, wq_config); if (imu != nullptr) { // Start VehicleIMU instance and store @@ -537,7 +541,39 @@ void Sensors::InitializeVehicleIMU() } else { // abort on first failure, try again later - return; + break; + } + } + } + + // create a VehicleImuFifo instance for each accel/gyro FIFO pair + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + if (_vehicle_imu_fifo_list[i] == nullptr) { + + uORB::SubscriptionData accel_sub{ORB_ID(sensor_accel_fifo), i}; + uORB::SubscriptionData gyro_sub{ORB_ID(sensor_gyro_fifo), i}; + + if (accel_sub.get().device_id > 0 && gyro_sub.get().device_id > 0) { + // if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleImuFifo in the same WQ + // otherwise each VehicleImuFifo runs in a corresponding INSx WQ + const bool multi_mode = (_param_sens_imu_mode.get() == 0); + const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0; + + VehicleImuFifo *imu_fifo = new VehicleImuFifo(i, i, wq_config); + + if (imu_fifo != nullptr) { + // Start VehicleImuFifo instance and store + if (imu_fifo->Start()) { + _vehicle_imu_fifo_list[i] = imu_fifo; + + } else { + delete imu_fifo; + } + } + + } else { + // abort on first failure, try again later + break; } } } @@ -715,6 +751,13 @@ int Sensors::print_status() } } + for (auto &i : _vehicle_imu_fifo_list) { + if (i != nullptr) { + PX4_INFO_RAW("\n"); + i->PrintStatus(); + } + } + return 0; } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index d7a86c528b..8c2598fffb 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -178,7 +178,7 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force) } } - PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id); + PX4_DEBUG("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id); _calibration.set_device_id(0); } } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 2dc0a836bc..45b0b65a1a 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -46,7 +46,20 @@ VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - CheckAndUpdateFilters(); + // gyro low pass + for (auto &lp : _lp_filter_velocity) { + lp.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get()); + } + + // notch filter + for (auto &nf : _notch_filter_velocity) { + nf.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get()); + } + + // angular acceleration low pass + for (auto &lp : _lp_filter_acceleration) { + lp.set_cutoff_frequency(kInitialRateHz, _param_imu_dgyro_cutoff.get()); + } } VehicleAngularVelocity::~VehicleAngularVelocity() @@ -66,6 +79,7 @@ bool VehicleAngularVelocity::Start() } if (!SensorSelectionUpdate(true)) { + _selected_sensor_sub_index = 0; _sensor_sub.registerCallback(); } @@ -81,46 +95,72 @@ void VehicleAngularVelocity::Stop() Deinit(); } -void VehicleAngularVelocity::CheckAndUpdateFilters() +void VehicleAngularVelocity::CheckFilters() { - bool sample_rate_changed = false; + // calculate sensor update rate + const float sample_interval_avg = _filter_sample_rate; - // get sample rate from vehicle_imu_status publication - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; + if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) { - const float sample_rate_hz = imu_status.get().gyro_rate_hz; + _update_rate_hz = 1.e6f / sample_interval_avg; - if ((imu_status.get().gyro_device_id != 0) && (imu_status.get().gyro_device_id == _calibration.device_id()) - && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) { - // check if sample rate error is greater than 1% - if ((fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { - PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz); - _filter_sample_rate = sample_rate_hz; - sample_rate_changed = true; - break; + // check if sample rate error is greater than 1% + if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { + PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz); + _reset_filters = true; + _filter_sample_rate = _update_rate_hz; + } + + if (_reset_filters || (_required_sample_updates == 0)) { + if (_param_imu_gyro_rate_max.get() > 0) { + // determine number of sensor samples that will get closest to the desired rate + const float configured_interval_us = 1e6f / _param_imu_gyro_rate_max.get(); + const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f, + (float)sensor_gyro_s::ORB_QUEUE_LENGTH); + + _sensor_sub.set_required_updates(samples); + _required_sample_updates = samples; + + } else { + _sensor_sub.set_required_updates(1); + _required_sample_updates = 1; } } + + // publish interval + if (_param_imu_gyro_rate_max.get() > 0) { + const uint64_t imu_gyro_interval = 1e6f / _param_imu_gyro_rate_max.get(); + _publish_interval_min_us = imu_gyro_interval - (sample_interval_avg / 2); + + } else { + _publish_interval_min_us = 0; + } } - // update software low pass filters - if (sample_rate_changed || (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.1f)) { - _lp_filter_velocity.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get()); - _lp_filter_velocity.reset(_angular_velocity_prev); + // reset sample interval accumulator + _timestamp_interval_last = 0; + _sample_rate_determined = true; +} + +void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, const Vector3f &angular_acceleration) +{ + for (int axis = 0; axis < 3; axis++) { + _lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get()); + _lp_filter_velocity[axis].reset(angular_velocity(axis)); } - if (sample_rate_changed - || (fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.1f) - || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.1f) - ) { - _notch_filter_velocity.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get()); - _notch_filter_velocity.reset(_angular_velocity_prev); + for (int axis = 0; axis < 3; axis++) { + _notch_filter_velocity[axis].setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), + _param_imu_gyro_nf_bw.get()); + _notch_filter_velocity[axis].reset(angular_velocity(axis)); } - if (sample_rate_changed || (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.1f)) { - _lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get()); - _lp_filter_acceleration.reset(_angular_acceleration_prev); + for (int axis = 0; axis < 3; axis++) { + _lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get()); + _lp_filter_acceleration[axis].reset(angular_acceleration(axis)); } + + _reset_filters = false; } void VehicleAngularVelocity::SensorBiasUpdate(bool force) @@ -138,7 +178,7 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force) estimator_sensor_bias_s bias; if (_estimator_sensor_bias_sub.copy(&bias)) { - if (bias.gyro_device_id == _calibration.device_id()) { + if (bias.gyro_device_id == _selected_sensor_device_id) { _bias = Vector3f{bias.gyro_bias}; } else { @@ -150,27 +190,69 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force) bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) { - if (_sensor_selection_sub.updated() || (_calibration.device_id() == 0) || force) { + if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) { sensor_selection_s sensor_selection{}; _sensor_selection_sub.copy(&sensor_selection); - if ((sensor_selection.gyro_device_id != 0) && (_calibration.device_id() != sensor_selection.gyro_device_id)) { + if (_selected_sensor_device_id != sensor_selection.gyro_device_id) { + + // see if the selected sensor publishes sensor_gyro_fifo for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; - const uint32_t device_id = sensor_gyro_sub.get().device_id; + if ((sensor_gyro_fifo_sub.get().device_id != 0) + && (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id)) { - if ((device_id != 0) && (device_id == sensor_selection.gyro_device_id)) { + if (_sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) { + // unregister + _sensor_sub.unregisterCallback(); - if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { - PX4_DEBUG("selected sensor changed %d -> %d", _calibration.device_id(), device_id); + // record selected sensor (array index) + _selected_sensor_sub_index = i; + _selected_sensor_device_id = sensor_selection.gyro_device_id; // clear bias and corrections _bias.zero(); - _calibration.set_device_id(device_id); + _calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id); - CheckAndUpdateFilters(); + // reset sample interval accumulator on sensor change + _sample_rate_determined = false; + _required_sample_updates = 0; + _reset_filters = true; + _fifo_data_filtered_prev.zero(); + + _fifo_available = true; + + return true; + } + } + } + + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + + if ((sensor_gyro_sub.get().device_id != 0) && (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id)) { + + if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { + // unregister + _sensor_fifo_sub.unregisterCallback(); + + // record selected sensor (array index) + _selected_sensor_sub_index = i; + _selected_sensor_device_id = sensor_selection.gyro_device_id; + + // clear bias and corrections + _bias.zero(); + + _calibration.set_device_id(sensor_gyro_sub.get().device_id); + + // reset sample interval accumulator on sensor change + _sample_rate_determined = false; + _required_sample_updates = 0; + _reset_filters = true; + + _fifo_available = false; return true; } @@ -178,7 +260,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) } PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.gyro_device_id); - _calibration.set_device_id(0); + _selected_sensor_device_id = 0; + _selected_sensor_sub_index = 0; } } @@ -197,91 +280,286 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) _calibration.ParametersUpdate(); - CheckAndUpdateFilters(); + // gyro low pass cutoff frequency changed + for (auto &lp : _lp_filter_velocity) { + if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) { + _reset_filters = true; + break; + } + } + + // gyro notch filter frequency or bandwidth changed + for (auto &nf : _notch_filter_velocity) { + if ((fabsf(nf.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f) + || (fabsf(nf.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)) { + + _reset_filters = true; + break; + } + } + + // gyro derivative low pass cutoff changed + for (auto &lp : _lp_filter_acceleration) { + if (fabsf(lp.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) { + _reset_filters = true; + break; + } + } } } +float VehicleAngularVelocity::GetSampleRateForGyro(uint32_t device_id) +{ + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; + + if (imu_status.get().gyro_device_id == device_id) { + if (imu_status.get().gyro_rate_hz > 0) { + return imu_status.get().gyro_rate_hz; + } + } + } + + return NAN; +} + void VehicleAngularVelocity::Run() { // backup schedule ScheduleDelayed(10_ms); // update corrections first to set _selected_sensor - bool selection_updated = SensorSelectionUpdate(); + const bool selection_updated = SensorSelectionUpdate(); _calibration.SensorCorrectionsUpdate(selection_updated); SensorBiasUpdate(selection_updated); ParametersUpdate(); - // process all outstanding messages - sensor_gyro_s sensor_data; + if (_fifo_available) { - while (_sensor_sub.update(&sensor_data)) { + // dynamic notch filter update + if (_sample_rate_determined) { + sensor_gyro_fft_s sensor_gyro_fft; - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f); - _timestamp_sample_prev = sensor_data.timestamp_sample; + if (_sensor_gyro_fft_sub.update(&sensor_gyro_fft) && (sensor_gyro_fft.device_id = _selected_sensor_device_id)) { + for (int i = 0; i < MAX_NUM_FFT_PEAKS; i++) { + for (int axis = 0; axis < 3; axis++) { - // get the sensor data and correct for thermal errors (apply offsets and scale) - const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + float *peak_frequencies = nullptr; - // correct for in-run bias errors - const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias; + switch (axis) { + case 0: + peak_frequencies = sensor_gyro_fft.peak_frequencies_x; + break; - // Gyro filtering: - // - Apply general notch filter (IMU_GYRO_NF_FREQ) - // - Apply general low-pass filter (IMU_GYRO_CUTOFF) - // - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + case 1: + peak_frequencies = sensor_gyro_fft.peak_frequencies_y; + break; - const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)}; + case 2: + peak_frequencies = sensor_gyro_fft.peak_frequencies_z; + break; + } - const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)}; + if (PX4_ISFINITE(peak_frequencies[i]) && (peak_frequencies[i] > 10.f) + && fabsf(_dynamic_notch_filter[i][axis].getNotchFreq() - peak_frequencies[i]) > 0.1f) { - const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt; - _angular_velocity_prev = angular_velocity; - _angular_acceleration_prev = angular_acceleration_raw; - const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)}; + _dynamic_notch_filter[i][axis].setParameters(_filter_sample_rate, + peak_frequencies[i], sensor_gyro_fft.resolution_hz); - - // publish once all new samples are processed - if (!_sensor_sub.updated()) { - bool publish = true; - - if (_param_imu_gyro_rate_max.get() > 0) { - const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); - - if (hrt_elapsed_time(&_last_publish) < interval) { - publish = false; + } else { + // disable + _dynamic_notch_filter[i][axis].setParameters(_filter_sample_rate, 0, sensor_gyro_fft.resolution_hz); + } + } } } + } - if (publish) { - // Publish vehicle_angular_acceleration - vehicle_angular_acceleration_s v_angular_acceleration; - v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample; - angular_acceleration.copyTo(v_angular_acceleration.xyz); - v_angular_acceleration.timestamp = hrt_absolute_time(); - _vehicle_angular_acceleration_pub.publish(v_angular_acceleration); + // process all outstanding fifo messages + sensor_gyro_fifo_s sensor_fifo_data; - // Publish vehicle_angular_velocity - vehicle_angular_velocity_s v_angular_velocity; - v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample; - angular_velocity.copyTo(v_angular_velocity.xyz); - v_angular_velocity.timestamp = hrt_absolute_time(); - _vehicle_angular_velocity_pub.publish(v_angular_velocity); + while (_sensor_fifo_sub.update(&sensor_fifo_data)) { - _last_publish = v_angular_velocity.timestamp_sample; - return; + if (sensor_fifo_data.samples > 0 && sensor_fifo_data.samples <= 32) { + const int N = sensor_fifo_data.samples; + const float dt_s = sensor_fifo_data.dt / 1e6f; + + if (_reset_filters) { + if (!_sample_rate_determined) { + // sample rate hasn't been calculated internally yet, try to get it from vehicle_imu_status + const float gyro_rate_hz = GetSampleRateForGyro(sensor_fifo_data.device_id); + + if (PX4_ISFINITE(gyro_rate_hz)) { + _filter_sample_rate = gyro_rate_hz * sensor_fifo_data.samples; + PX4_DEBUG("using FIFO sample rate %.3f Hz", (double)_filter_sample_rate); + } + } + + Vector3f angular_velocity_reset{}; + angular_velocity_reset(0) = sensor_fifo_data.x[0]; + angular_velocity_reset(1) = sensor_fifo_data.y[0]; + angular_velocity_reset(2) = sensor_fifo_data.z[0]; + + Vector3f angular_acceleration_reset{}; + + if (sensor_fifo_data.samples >= 2) { + angular_acceleration_reset(0) = (sensor_fifo_data.x[1] - sensor_fifo_data.x[0]) / dt_s; + angular_acceleration_reset(1) = (sensor_fifo_data.y[1] - sensor_fifo_data.y[0]) / dt_s; + angular_acceleration_reset(2) = (sensor_fifo_data.z[1] - sensor_fifo_data.z[0]) / dt_s; + } + + ResetFilters(angular_velocity_reset, angular_acceleration_reset); + } + + matrix::Vector3f angular_velocity_unscaled; + matrix::Vector3f angular_acceleration_unscaled; + + for (int axis = 0; axis < 3; axis++) { + // copy raw int16 sensor samples to float array for filtering + int16_t *raw_data = nullptr; + + switch (axis) { + case 0: + raw_data = sensor_fifo_data.x; + break; + + case 1: + raw_data = sensor_fifo_data.y; + break; + + case 2: + raw_data = sensor_fifo_data.z; + break; + } + + float data[N]; + + for (int n = 0; n < N; n++) { + data[n] = raw_data[n]; + } + + // Apply dynamic notch filter from FFT + for (auto &dnf : _dynamic_notch_filter) { + if (dnf[axis].getNotchFreq() > 10.f) { + dnf[axis].applyDF1(data, N); + } + } + + // Apply general notch filter (IMU_GYRO_NF_FREQ) + if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) { + _notch_filter_velocity[axis].apply(data, N); + } + + // Apply general low-pass filter (IMU_GYRO_CUTOFF) + _lp_filter_velocity[axis].apply(data, N); + + // save last filtered sample + angular_velocity_unscaled(axis) = data[N - 1]; + + + // angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + for (int n = 0; n < N; n++) { + float accel = (data[n] - _fifo_data_filtered_prev(axis)) / dt_s; + angular_acceleration_unscaled(axis) = _lp_filter_acceleration[axis].apply(accel); + _fifo_data_filtered_prev(axis) = data[n]; + } + } + + // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias + const Vector3f angular_velocity{_calibration.Correct(angular_velocity_unscaled * sensor_fifo_data.scale) - _bias}; + + // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation + const Vector3f angular_acceleration{_calibration.rotation() *angular_acceleration_unscaled * sensor_fifo_data.scale}; + + // Publish + if (!_sensor_fifo_sub.updated() && (sensor_fifo_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) { + Publish(sensor_fifo_data.timestamp_sample, angular_velocity, angular_acceleration); + _last_publish = sensor_fifo_data.timestamp_sample; + } + } + } + + } else { + // process all outstanding messages + sensor_gyro_s sensor_data; + + while (_sensor_sub.update(&sensor_data)) { + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) / 1e6f), 0.0002f, 0.02f); + _timestamp_sample_last = sensor_data.timestamp_sample; + + // Apply calibration, rotation, and correct for in-run bias errors + Vector3f angular_velocity{_calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias}; + Vector3f angular_acceleration{}; + + if (_reset_filters) { + if (!_sample_rate_determined) { + // if sample rate was previously determined use it + if (PX4_ISFINITE(_sensor_sample_rate[_selected_sensor_sub_index])) { + _filter_sample_rate = _sensor_sample_rate[_selected_sensor_sub_index]; + + } else { + // sample rate hasn't been calculated internally yet, try to get it from vehicle_imu_status + const float gyro_rate_hz = GetSampleRateForGyro(sensor_data.device_id); + + if (PX4_ISFINITE(gyro_rate_hz)) { + _filter_sample_rate = gyro_rate_hz; + PX4_DEBUG("using sample rate %.3f Hz", (double)_filter_sample_rate); + } + } + } + + ResetFilters(angular_velocity, angular_acceleration); + } + + for (int axis = 0; axis < 3; axis++) { + // Apply general notch filter (IMU_GYRO_NF_FREQ) + _notch_filter_velocity[axis].apply(&angular_velocity(axis), 1); + + // Apply general low-pass filter (IMU_GYRO_CUTOFF) + _lp_filter_velocity[axis].apply(&angular_velocity(axis), 1); + + // Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + float accel = (angular_velocity(axis) - _angular_velocity_last(axis)) / dt; + angular_acceleration(axis) = _lp_filter_acceleration[axis].apply(accel); + _angular_velocity_last(axis) = angular_velocity(axis); + } + + // Publish + if (!_sensor_sub.updated() && (sensor_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) { + Publish(sensor_data.timestamp_sample, angular_velocity, angular_acceleration); + _last_publish = sensor_data.timestamp_sample; } } } } +void VehicleAngularVelocity::Publish(const hrt_abstime ×tamp_sample, const Vector3f &angular_velocity, + const Vector3f &angular_acceleration) +{ + // Publish vehicle_angular_acceleration + vehicle_angular_acceleration_s v_angular_acceleration; + v_angular_acceleration.timestamp_sample = timestamp_sample; + angular_acceleration.copyTo(v_angular_acceleration.xyz); + v_angular_acceleration.timestamp = hrt_absolute_time(); + _vehicle_angular_acceleration_pub.publish(v_angular_acceleration); + + // Publish vehicle_angular_velocity + vehicle_angular_velocity_s v_angular_velocity; + v_angular_velocity.timestamp_sample = timestamp_sample; + angular_velocity.copyTo(v_angular_velocity.xyz); + v_angular_velocity.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_pub.publish(v_angular_velocity); + + _last_publish = timestamp_sample; +} + void VehicleAngularVelocity::PrintStatus() { - PX4_INFO("selected sensor: %d, rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]", - _calibration.device_id(), (double)_filter_sample_rate, - (double)_bias(0), (double)_bias(1), (double)_bias(2)); + PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz %s", + _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz, _fifo_available ? "FIFO" : ""); + PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2)); _calibration.PrintStatus(); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 82180faafb..225b8f8268 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -36,8 +36,9 @@ #include #include #include -#include -#include +#include +#include +#include #include #include #include @@ -49,6 +50,8 @@ #include #include #include +#include +#include #include #include #include @@ -72,11 +75,18 @@ public: private: void Run() override; - void CheckAndUpdateFilters(); + void CheckFilters(); + void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration); + + float GetSampleRateForGyro(uint32_t device_id); + void ParametersUpdate(bool force = false); void SensorBiasUpdate(bool force = false); bool SensorSelectionUpdate(bool force = false); + void Publish(const hrt_abstime ×tamp_sample, const matrix::Vector3f &angular_velocity, + const matrix::Vector3f &angular_acceleration); + static constexpr int MAX_SENSOR_COUNT = 4; uORB::Publication _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)}; @@ -84,30 +94,54 @@ private: uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; + uORB::Subscription _sensor_gyro_fft_sub{ORB_ID(sensor_gyro_fft)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)}; + uORB::SubscriptionCallbackWorkItem _sensor_fifo_sub{this, ORB_ID(sensor_gyro_fifo)}; + + matrix::Vector3f _fifo_data_filtered_prev{}; calibration::Gyroscope _calibration{}; matrix::Vector3f _bias{}; - matrix::Vector3f _angular_acceleration_prev{}; - matrix::Vector3f _angular_velocity_prev{}; - hrt_abstime _timestamp_sample_prev{0}; + matrix::Vector3f _angular_velocity_last{}; + hrt_abstime _timestamp_sample_last{0}; + hrt_abstime _publish_interval_min_us{0}; hrt_abstime _last_publish{0}; static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */ - float _filter_sample_rate{kInitialRateHz}; + float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */ + + uint8_t _required_sample_updates{0}; /**< number or sensor publications required for configured rate */ + + static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof( + sensor_gyro_fft_s::peak_frequencies_x[0]); // angular velocity filters - math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.f}; - math::NotchFilter _notch_filter_velocity{}; + math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}}; + math::NotchFilterArray _notch_filter_velocity[3] {}; + math::NotchFilterArray _dynamic_notch_filter[MAX_NUM_FFT_PEAKS][3] {}; // angular acceleration filter - math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 30.f}; + math::LowPassFilter2p _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}}; + + float _filter_sample_rate{kInitialRateHz}; + + float _sensor_sample_rate[MAX_SENSOR_COUNT] {NAN, NAN, NAN, NAN}; + + uint32_t _selected_sensor_device_id{0}; + uint8_t _selected_sensor_sub_index{0}; + + hrt_abstime _timestamp_interval_last{0}; + + bool _sample_rate_determined{false}; + bool _reset_filters{false}; + + bool _fifo_available{false}; DEFINE_PARAMETERS( (ParamFloat) _param_imu_gyro_cutoff, diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index ea6f422f72..f64d5fdb2e 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -100,7 +100,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0); +PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); /** * Cutoff frequency for angular acceleration (D-Term filter) diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index 2fc8b30243..ced49d6744 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -36,6 +36,8 @@ px4_add_library(vehicle_imu Integrator.hpp VehicleIMU.cpp VehicleIMU.hpp + VehicleImuFifo.cpp + VehicleImuFifo.hpp ) target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration) diff --git a/src/modules/sensors/vehicle_imu/Integrator.cpp b/src/modules/sensors/vehicle_imu/Integrator.cpp index cf79744e42..145082cfa5 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.cpp +++ b/src/modules/sensors/vehicle_imu/Integrator.cpp @@ -37,22 +37,13 @@ using matrix::Vector3f; -bool Integrator::put(const hrt_abstime ×tamp, const Vector3f &val) +bool Integrator::put(const Vector3f &val, const float dt) { - if ((_last_integration_time == 0) || (timestamp <= _last_integration_time)) { - /* this is the first item in the integrator */ - _last_integration_time = timestamp; - _last_reset_time = timestamp; - _last_val = val; - - return false; - } + _integral_dt += dt; // Use trapezoidal integration to calculate the delta integral - const float dt = static_cast(timestamp - _last_integration_time) * 1e-6f; const matrix::Vector3f delta_alpha = (val + _last_val) * dt * 0.5f; _last_val = val; - _last_integration_time = timestamp; _integrated_samples++; // Calculate coning corrections if required @@ -79,8 +70,9 @@ bool Integrator::reset(Vector3f &integral, uint32_t &integral_dt) integral = Vector3f{_alpha}; _alpha.zero(); - integral_dt = (_last_integration_time - _last_reset_time); - _last_reset_time = _last_integration_time; + integral_dt = roundf(_integral_dt * 1e6f); + _integral_dt = 0; + //integral_dt = (_last_integration_time - _last_reset_time); _integrated_samples = 0; // apply coning corrections if required diff --git a/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/modules/sensors/vehicle_imu/Integrator.hpp index 02c8d2fd05..edcd60d3f8 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.hpp +++ b/src/modules/sensors/vehicle_imu/Integrator.hpp @@ -58,29 +58,14 @@ public: * @param val Item to put. * @return true if data was accepted and integrated. */ - bool put(const uint64_t ×tamp, const matrix::Vector3f &val); - - /** - * Put an item into the integral. - * - * @param timestamp Timestamp of the current value. - * @param val Item to put. - * @param integral Current integral in case the integrator did reset, else the value will not be modified - * @param integral_dt Get the dt in us of the current integration (only if reset). - * @return true if putting the item triggered an integral reset and the integral should be - * published. - */ - bool put(const uint64_t ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt) - { - return put(timestamp, val) && reset(integral, integral_dt); - } + bool put(const matrix::Vector3f &val, const float dt); /** * Set reset interval during runtime. This won't reset the integrator. * * @param reset_interval New reset time interval for the integrator. */ - void set_reset_interval(uint32_t reset_interval) { _reset_interval_min = reset_interval; } + void set_reset_interval(uint32_t reset_interval) { _reset_interval_min = reset_interval * 1e-6f; } /** * Set required samples for reset. This won't reset the integrator. @@ -95,7 +80,7 @@ public: * * @return true if integrator has sufficient data (minimum interval & samples satisfied) to reset. */ - bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_last_integration_time >= (_last_reset_time + _reset_interval_min)); } + bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); } /* Reset integrator and return current integral & integration time * @@ -105,19 +90,18 @@ public: bool reset(matrix::Vector3f &integral, uint32_t &integral_dt); private: - uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */ - uint64_t _last_reset_time{0}; /**< last auto-announcement of integral value */ - matrix::Vector3f _alpha{0.f, 0.f, 0.f}; /**< integrated value before coning corrections are applied */ matrix::Vector3f _last_alpha{0.f, 0.f, 0.f}; /**< previous value of _alpha */ matrix::Vector3f _beta{0.f, 0.f, 0.f}; /**< accumulated coning corrections */ matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */ matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */ - uint32_t _reset_interval_min{1}; /**< the interval after which the content will be published and the integrator reset */ + float _reset_interval_min{0.001}; /**< the interval after which the content will be published and the integrator reset */ uint8_t _integrated_samples{0}; uint8_t _reset_samples_min{1}; + float _integral_dt{0}; + const bool _coning_comp_on{false}; /**< true to turn on coning corrections */ }; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 034c4d551d..57fb92b923 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -44,12 +44,11 @@ using math::constrain; namespace sensors { -VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) : +VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, config), _sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index), - _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index), - _instance(instance) + _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index) { const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); @@ -243,9 +242,13 @@ void VehicleIMU::Run() _gyro_temperature += gyro.temperature; _gyro_sum_count++; - _gyro_integrator.put(gyro.timestamp_sample, gyro_raw); + const float dt = (gyro.timestamp_sample - _last_timestamp_sample_gyro); _last_timestamp_sample_gyro = gyro.timestamp_sample; + if (dt > 0.f && dt < 1.f) { + _gyro_integrator.put(gyro_raw, dt); + } + // break if interval is configured and we haven't fallen behind if (_intervals_configured && _gyro_integrator.integral_ready() && (hrt_elapsed_time(&gyro.timestamp) < _imu_integration_interval_us) && !sensor_data_gap) { @@ -289,39 +292,43 @@ void VehicleIMU::Run() _accel_temperature += accel.temperature; _accel_sum_count++; - _accel_integrator.put(accel.timestamp_sample, accel_raw); + const float dt = (accel.timestamp_sample - _last_timestamp_sample_accel); _last_timestamp_sample_accel = accel.timestamp_sample; - if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { - - // rotate sensor clip counts into vehicle body frame - const Vector3f clipping{_accel_calibration.rotation() * - Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; - - // round to get reasonble clip counts per axis (after board rotation) - const uint8_t clip_x = roundf(fabsf(clipping(0))); - const uint8_t clip_y = roundf(fabsf(clipping(1))); - const uint8_t clip_z = roundf(fabsf(clipping(2))); - - _status.accel_clipping[0] += clip_x; - _status.accel_clipping[1] += clip_y; - _status.accel_clipping[2] += clip_z; - - if (clip_x > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X; - } - - if (clip_y > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y; - } - - if (clip_z > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z; - } - - publish_status = true; + if (dt > 0.f && dt < 1.f) { + _accel_integrator.put(accel_raw, dt); } + // if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { + + // // rotate sensor clip counts into vehicle body frame + // const Vector3f clipping{_accel_calibration.rotation() * + // Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; + + // // round to get reasonble clip counts per axis (after board rotation) + // const uint8_t clip_x = roundf(fabsf(clipping(0))); + // const uint8_t clip_y = roundf(fabsf(clipping(1))); + // const uint8_t clip_z = roundf(fabsf(clipping(2))); + + // _status.accel_clipping[0] += clip_x; + // _status.accel_clipping[1] += clip_y; + // _status.accel_clipping[2] += clip_z; + + // if (clip_x > 0) { + // _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X; + // } + + // if (clip_y > 0) { + // _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y; + // } + + // if (clip_z > 0) { + // _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z; + // } + + // publish_status = true; + // } + // break once caught up to gyro if (!sensor_data_gap && _intervals_configured && (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval))) { @@ -487,11 +494,12 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle) void VehicleIMU::PrintStatus() { if (_accel_calibration.device_id() == _gyro_calibration.device_id()) { - PX4_INFO("%d - IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _instance, _accel_calibration.device_id(), + PX4_INFO("%d - IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _vehicle_imu_pub.get_instance(), + _accel_calibration.device_id(), (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval); } else { - PX4_INFO("%d - Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _instance, + PX4_INFO("%d - Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _vehicle_imu_pub.get_instance(), _accel_calibration.device_id(), (double)_accel_interval.update_interval, _gyro_calibration.device_id(), (double)_gyro_interval.update_interval); } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 15d091c3e7..29875f28a4 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -62,7 +62,7 @@ class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem { public: VehicleIMU() = delete; - VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config); + VehicleIMU(uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config); ~VehicleIMU() override; @@ -129,8 +129,6 @@ private: bool _intervals_configured{false}; - const uint8_t _instance; - perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")}; perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; perf_counter_t _gyro_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro update interval")}; diff --git a/src/modules/sensors/vehicle_imu/VehicleImuFifo.cpp b/src/modules/sensors/vehicle_imu/VehicleImuFifo.cpp new file mode 100644 index 0000000000..7efe585558 --- /dev/null +++ b/src/modules/sensors/vehicle_imu/VehicleImuFifo.cpp @@ -0,0 +1,569 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#include "VehicleImuFifo.hpp" + +#include + +#include + +using namespace matrix; + +using math::constrain; + +namespace sensors +{ + +static constexpr uint8_t clipping(const int16_t samples[20], int16_t clip_limit, uint8_t len) +{ + unsigned clip_count = 0; + + for (int n = 0; n < len; n++) { + if (abs(samples[n]) >= clip_limit) { + clip_count++; + } + } + + return clip_count; +} + +VehicleImuFifo::VehicleImuFifo(uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, config), + _sensor_accel_sub(this, ORB_ID(sensor_accel_fifo), accel_index), + _sensor_gyro_sub(this, ORB_ID(sensor_gyro_fifo), gyro_index) +{ + const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); + + _accel_integrator.set_reset_interval(configured_interval_us); + _accel_integrator.set_reset_samples(sensor_accel_fifo_s::ORB_QUEUE_LENGTH); + + _gyro_integrator.set_reset_interval(configured_interval_us); + _gyro_integrator.set_reset_samples(sensor_gyro_fifo_s::ORB_QUEUE_LENGTH); + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + // currently with lockstep every raw sample needs a corresponding vehicle_imu publication + _sensor_accel_sub.set_required_updates(1); + _sensor_gyro_sub.set_required_updates(1); +#else + // schedule conservatively until the actual accel & gyro rates are known + _sensor_accel_sub.set_required_updates(sensor_accel_fifo_s::ORB_QUEUE_LENGTH / 2); + _sensor_gyro_sub.set_required_updates(sensor_gyro_fifo_s::ORB_QUEUE_LENGTH / 2); +#endif + + // advertise immediately to ensure consistent ordering + _vehicle_imu_pub.advertise(); + _vehicle_imu_status_pub.advertise(); +} + +VehicleImuFifo::~VehicleImuFifo() +{ + Stop(); + + perf_free(_accel_generation_gap_perf); + perf_free(_accel_update_perf); + perf_free(_gyro_generation_gap_perf); + perf_free(_gyro_update_perf); + + _vehicle_imu_pub.unadvertise(); + _vehicle_imu_status_pub.unadvertise(); +} + +bool VehicleImuFifo::Start() +{ + // force initial updates + ParametersUpdate(true); + + _sensor_gyro_sub.registerCallback(); + _sensor_accel_sub.registerCallback(); + ScheduleNow(); + return true; +} + +void VehicleImuFifo::Stop() +{ + // clear all registered callbacks + _sensor_accel_sub.unregisterCallback(); + _sensor_gyro_sub.unregisterCallback(); + + Deinit(); +} + +void VehicleImuFifo::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + const auto imu_integ_rate_prev = _param_imu_integ_rate.get(); + + updateParams(); + + _accel_calibration.ParametersUpdate(); + _gyro_calibration.ParametersUpdate(); + + // constrain IMU integration time 1-10 milliseconds (100-1000 Hz) + int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), + 100, math::max(_param_imu_gyro_ratemax.get(), 1000)); + + if (imu_integration_rate_hz != _param_imu_integ_rate.get()) { + PX4_WARN("IMU_INTEG_RATE updated %d -> %d", _param_imu_integ_rate.get(), imu_integration_rate_hz); + _param_imu_integ_rate.set(imu_integration_rate_hz); + _param_imu_integ_rate.commit_no_notification(); + } + + _imu_integration_interval_us = 1000000 / imu_integration_rate_hz; + + if (_param_imu_integ_rate.get() != imu_integ_rate_prev) { + // force update + UpdateIntegratorConfiguration(); + } + } +} + +bool VehicleImuFifo::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample, + uint8_t samples) +{ + bool updated = false; + + // conservative maximum time between samples to reject large gaps and reset averaging + float max_interval_us = 10000; // 100 Hz + float min_interval_us = 50; // 20,000 Hz + + if (intavg.update_interval > 0) { + // if available use previously calculated interval as bounds + max_interval_us = 1.5f * intavg.update_interval; + min_interval_us = 0.5f * intavg.update_interval; + } + + const float interval_us = (timestamp_sample - intavg.timestamp_sample_last); + + if ((intavg.timestamp_sample_last > 0) && (interval_us < max_interval_us) && (interval_us > min_interval_us)) { + + intavg.interval_sum += interval_us; + intavg.sample_count += samples; + intavg.interval_count++; + + // periodically calculate sensor update rate + if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) { + + const float interval_avg = intavg.interval_sum / intavg.interval_count; + const float sample_interval_avg = intavg.interval_sum / intavg.sample_count; + + if (PX4_ISFINITE(interval_avg) && (interval_avg > 0.f)) { + // update if interval has changed by more than 0.5% + if ((fabsf(intavg.update_interval - interval_avg) / intavg.update_interval) > 0.005f) { + + intavg.update_interval = interval_avg; + intavg.sample_interval = sample_interval_avg; + updated = true; + } + } + + // reset sample interval accumulator + intavg.interval_sum = 0.f; + intavg.sample_count = 0; + intavg.interval_count = 0.f; + } + + } else { + // reset + intavg.interval_sum = 0.f; + intavg.sample_count = 0; + intavg.interval_count = 0.f; + } + + intavg.timestamp_sample_last = timestamp_sample; + + return updated; +} + +void VehicleImuFifo::Run() +{ + // backup schedule + ScheduleDelayed(10_ms); + + ParametersUpdate(); + + if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) { + return; + } + + + bool sensor_data_gap = false; + bool update_integrator_config = false; + + // integrate queued gyro + sensor_gyro_fifo_s sensor_gyro_fifo; + + while (_sensor_gyro_sub.update(&sensor_gyro_fifo)) { + perf_count_interval(_gyro_update_perf, sensor_gyro_fifo.timestamp_sample); + + if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { + sensor_data_gap = true; + perf_count(_gyro_generation_gap_perf); + + _gyro_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging + + } else { + // collect sample interval average for filters + if (!_intervals_configured + && UpdateIntervalAverage(_gyro_interval, sensor_gyro_fifo.timestamp_sample, sensor_gyro_fifo.samples)) { + update_integrator_config = true; + _publish_status = true; + _status.gyro_rate_hz = 1e6f / _gyro_interval.sample_interval; + } + } + + _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); + + _gyro_calibration.set_device_id(sensor_gyro_fifo.device_id); + + if (sensor_gyro_fifo.error_count != _status.gyro_error_count) { + _publish_status = true; + _status.gyro_error_count = sensor_gyro_fifo.error_count; + } + + _last_timestamp_sample_gyro = sensor_gyro_fifo.timestamp_sample; + + const float dt_s = sensor_gyro_fifo.dt * 1e-6f; + const int N = sensor_gyro_fifo.samples; + + for (int n = 0; n < N; n++) { + const Vector3f gyro_raw{ + sensor_gyro_fifo.scale *sensor_gyro_fifo.x[n], + sensor_gyro_fifo.scale *sensor_gyro_fifo.y[n], + sensor_gyro_fifo.scale *sensor_gyro_fifo.z[n] + }; + + _gyro_integrator.put(gyro_raw, dt_s); + + _gyro_sum += gyro_raw; + _gyro_sum_count++; + } + + _gyro_temperature += sensor_gyro_fifo.temperature; + _gyro_temperature_count++; + + // break if interval is configured and we haven't fallen behind + if (_intervals_configured && _gyro_integrator.integral_ready() + && (hrt_elapsed_time(&sensor_gyro_fifo.timestamp) < _imu_integration_interval_us) && !sensor_data_gap) { + + break; + } + } + + // update accel, stopping once caught up to the last gyro sample + sensor_accel_fifo_s sensor_accel_fifo; + + while (_sensor_accel_sub.update(&sensor_accel_fifo)) { + perf_count_interval(_accel_update_perf, sensor_accel_fifo.timestamp_sample); + + if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) { + sensor_data_gap = true; + perf_count(_accel_generation_gap_perf); + + _accel_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging + + } else { + // collect sample interval average for filters + if (!_intervals_configured + && UpdateIntervalAverage(_accel_interval, sensor_accel_fifo.timestamp_sample, sensor_accel_fifo.samples)) { + update_integrator_config = true; + _publish_status = true; + _status.accel_rate_hz = 1e6f / _accel_interval.sample_interval; + } + } + + _accel_last_generation = _sensor_accel_sub.get_last_generation(); + + _accel_calibration.set_device_id(sensor_accel_fifo.device_id); + + if (sensor_accel_fifo.error_count != _status.accel_error_count) { + _publish_status = true; + _status.accel_error_count = sensor_accel_fifo.error_count; + } + + _last_timestamp_sample_accel = sensor_accel_fifo.timestamp_sample; + + const float dt_s = sensor_accel_fifo.dt * 1e-6f; + const int N = sensor_accel_fifo.samples; + + // clipping + Vector3f clip_count { + (float)clipping(sensor_accel_fifo.x, UINT16_MAX - 1, N), + (float)clipping(sensor_accel_fifo.y, UINT16_MAX - 1, N), + (float)clipping(sensor_accel_fifo.z, UINT16_MAX - 1, N), + }; + + // rotate sensor clip counts into vehicle body frame + const Vector3f clipping{_accel_calibration.rotation() *clip_count}; + + // round to get reasonble clip counts per axis (after board rotation) + if (fabsf(clipping(0)) > 0.f) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X; + } + + if (fabsf(clipping(1)) > 0.f) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y; + } + + if (fabsf(clipping(2)) > 0.f) { + _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z; + } + + + for (int n = 0; n < N; n++) { + const Vector3f accel_raw{ + sensor_accel_fifo.scale *sensor_accel_fifo.x[n], + sensor_accel_fifo.scale *sensor_accel_fifo.y[n], + sensor_accel_fifo.scale *sensor_accel_fifo.z[n] + }; + + _accel_integrator.put(accel_raw, dt_s); + + _accel_sum += accel_raw; + _accel_sum_count++; + + // break once caught up to gyro + if (!sensor_data_gap && _intervals_configured) { + // && (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval))) { + + if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { + Publish(); + } + } + } + + _accel_temperature += sensor_accel_fifo.temperature; + _accel_temperature_count++; + } + + if (sensor_data_gap) { + _consecutive_data_gap++; + + // if there's consistently a gap in data start monitoring publication interval again + if (_consecutive_data_gap > 10) { + _intervals_configured = false; + } + + } else { + _consecutive_data_gap = 0; + } + + // reconfigure integrators if calculated sensor intervals have changed + if (update_integrator_config) { + UpdateIntegratorConfiguration(); + } + + Publish(); +} + +void VehicleImuFifo::Publish() +{ + // publish if both accel & gyro integrators are ready + if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { + + uint32_t accel_integral_dt; + uint32_t gyro_integral_dt; + Vector3f delta_angle; + Vector3f delta_velocity; + + if (_accel_integrator.reset(delta_velocity, accel_integral_dt) + && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) { + + if (_accel_calibration.enabled() && _gyro_calibration.enabled()) { + + // delta angle: apply offsets, scale, and board rotation + _gyro_calibration.SensorCorrectionsUpdate(); + const float gyro_dt_inv = 1.e6f / gyro_integral_dt; + const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv}; + + // delta velocity: apply offsets, scale, and board rotation + _accel_calibration.SensorCorrectionsUpdate(); + const float accel_dt_inv = 1.e6f / accel_integral_dt; + Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv}; + + UpdateAccelVibrationMetrics(delta_velocity_corrected); + UpdateGyroVibrationMetrics(delta_angle_corrected); + + // vehicle_imu_status + // publish before vehicle_imu so that error counts are available synchronously if needed + if (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) { + _status.accel_device_id = _accel_calibration.device_id(); + _status.gyro_device_id = _gyro_calibration.device_id(); + + // mean accel + const Vector3f accel_mean{_accel_calibration.Correct(_accel_sum / _accel_sum_count)}; + accel_mean.copyTo(_status.mean_accel); + _status.temperature_accel = _accel_temperature / _accel_temperature_count; + _accel_sum.zero(); + _accel_sum_count = 0; + _accel_temperature = 0; + _accel_temperature_count = 0; + + // mean gyro + const Vector3f gyro_mean{_gyro_calibration.Correct(_gyro_sum / _gyro_sum_count)}; + gyro_mean.copyTo(_status.mean_gyro); + _status.temperature_gyro = _gyro_temperature / _gyro_temperature_count; + _gyro_sum.zero(); + _gyro_sum_count = 0; + _gyro_temperature = 0; + _gyro_temperature_count = 0; + + _status.timestamp = hrt_absolute_time(); + _vehicle_imu_status_pub.publish(_status); + + _publish_status = false; + } + + + // publish vehicle_imu + vehicle_imu_s imu; + imu.timestamp_sample = _last_timestamp_sample_gyro; + imu.accel_device_id = _accel_calibration.device_id(); + imu.gyro_device_id = _gyro_calibration.device_id(); + delta_angle_corrected.copyTo(imu.delta_angle); + delta_velocity_corrected.copyTo(imu.delta_velocity); + imu.delta_angle_dt = gyro_integral_dt; + imu.delta_velocity_dt = accel_integral_dt; + imu.delta_velocity_clipping = _delta_velocity_clipping; + imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count(); + imu.timestamp = hrt_absolute_time(); + _vehicle_imu_pub.publish(imu); + } + + // reset clip counts + _delta_velocity_clipping = 0; + + return; + } + } +} + +void VehicleImuFifo::UpdateIntegratorConfiguration() +{ + if ((_accel_interval.update_interval > 0) && (_gyro_interval.update_interval > 0)) { + + const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); + + // determine number of sensor samples that will get closest to the desired integration interval + const uint8_t accel_pub_integral_samples = math::max(1.f, + roundf(configured_interval_us / _accel_interval.update_interval)); + const uint8_t gyro_pub_integral_samples = math::max(1.f, + roundf(configured_interval_us / _gyro_interval.update_interval)); + + const uint8_t accel_sample_integral_samples = math::max(1.f, + roundf(configured_interval_us / _accel_interval.sample_interval)); + const uint8_t gyro_sample_integral_samples = math::max(1.f, + roundf(configured_interval_us / _gyro_interval.sample_interval)); + + // let the gyro set the configuration and scheduling + // accel integrator will be forced to reset when gyro integrator is ready + _gyro_integrator.set_reset_samples(gyro_sample_integral_samples); + _accel_integrator.set_reset_samples(accel_sample_integral_samples); + + // relaxed minimum integration time required + _accel_integrator.set_reset_interval(roundf((accel_sample_integral_samples - 0.5f) * _accel_interval.sample_interval)); + _gyro_integrator.set_reset_interval(roundf((gyro_sample_integral_samples - 0.5f) * _gyro_interval.sample_interval)); + + // gyro: find largest integer multiple of gyro_integral_samples + for (int n = sensor_gyro_fifo_s::ORB_QUEUE_LENGTH; n > 0; n--) { + if (gyro_pub_integral_samples % n == 0) { + _sensor_gyro_sub.set_required_updates(n); + + // run when there are enough new gyro samples, unregister accel + _sensor_accel_sub.unregisterCallback(); + + _intervals_configured = true; // stop monitoring topic publication rates + + PX4_INFO("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f (%.1f), gyro interval: %.1f (%.1f) sub samples: %d", + _accel_calibration.device_id(), _gyro_calibration.device_id(), + accel_pub_integral_samples, gyro_pub_integral_samples, + (double)_accel_interval.update_interval, (double)_accel_interval.sample_interval, + (double)_gyro_interval.update_interval, (double)_gyro_interval.sample_interval, + n); + + break; + } + } + } +} + +void VehicleImuFifo::UpdateAccelVibrationMetrics(const Vector3f &delta_velocity) +{ + // Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) + const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev; + _status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric + 0.01f * delta_velocity_diff.norm(); + + _delta_velocity_prev = delta_velocity; +} + +void VehicleImuFifo::UpdateGyroVibrationMetrics(const Vector3f &delta_angle) +{ + // Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) + const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev; + _status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric + 0.01f * delta_angle_diff.norm(); + + // Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) + const Vector3f coning_metric = delta_angle % _delta_angle_prev; + _status.gyro_coning_vibration = 0.99f * _status.gyro_coning_vibration + 0.01f * coning_metric.norm(); + + _delta_angle_prev = delta_angle; +} + +void VehicleImuFifo::PrintStatus() +{ + if (_accel_calibration.device_id() == _gyro_calibration.device_id()) { + PX4_INFO("%d - IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _vehicle_imu_pub.get_instance(), + _accel_calibration.device_id(), + (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval); + + } else { + PX4_INFO("%d - Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _vehicle_imu_pub.get_instance(), + _accel_calibration.device_id(), (double)_accel_interval.update_interval, _gyro_calibration.device_id(), + (double)_gyro_interval.update_interval); + } + + perf_print_counter(_accel_generation_gap_perf); + perf_print_counter(_gyro_generation_gap_perf); + perf_print_counter(_accel_update_perf); + perf_print_counter(_gyro_update_perf); + + _accel_calibration.PrintStatus(); + _gyro_calibration.PrintStatus(); +} + +} // namespace sensors diff --git a/src/modules/sensors/vehicle_imu/VehicleImuFifo.hpp b/src/modules/sensors/vehicle_imu/VehicleImuFifo.hpp new file mode 100644 index 0000000000..7474307f25 --- /dev/null +++ b/src/modules/sensors/vehicle_imu/VehicleImuFifo.hpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#pragma once + +#include "Integrator.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ + +class VehicleImuFifo : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + VehicleImuFifo() = delete; + VehicleImuFifo(uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config); + + ~VehicleImuFifo() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void ParametersUpdate(bool force = false); + void Run() override; + void Publish(); + + struct IntervalAverage { + hrt_abstime timestamp_sample_last{0}; + float interval_sum{0.f}; + float interval_count{0.f}; + int sample_count{0}; + float update_interval{0.f}; + float sample_interval{0.f}; + }; + + bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample, uint8_t samples); + void UpdateIntegratorConfiguration(); + void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle); + void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity); + + uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; + uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _sensor_accel_sub; + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub; + + calibration::Accelerometer _accel_calibration{}; + calibration::Gyroscope _gyro_calibration{}; + + Integrator _accel_integrator{}; // 200 Hz default + Integrator _gyro_integrator{true}; // 200 Hz default, coning compensation enabled + + hrt_abstime _last_timestamp_sample_accel{0}; + hrt_abstime _last_timestamp_sample_gyro{0}; + + uint32_t _imu_integration_interval_us{4000}; + + IntervalAverage _accel_interval{}; + IntervalAverage _gyro_interval{}; + + unsigned _accel_last_generation{0}; + unsigned _gyro_last_generation{0}; + unsigned _consecutive_data_gap{0}; + + matrix::Vector3f _accel_sum{}; + matrix::Vector3f _gyro_sum{}; + int _accel_sum_count{0}; + int _gyro_sum_count{0}; + + float _accel_temperature{0}; + float _gyro_temperature{0}; + int _accel_temperature_count{0}; + int _gyro_temperature_count{0}; + + matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement + matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement + + vehicle_imu_status_s _status{}; + + uint8_t _delta_velocity_clipping{0}; + + bool _intervals_configured{false}; + + bool _publish_status{false}; + + perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")}; + perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; + perf_counter_t _gyro_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro update interval")}; + perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_imu_integ_rate, + (ParamInt) _param_imu_gyro_ratemax + ) +}; + +} // namespace sensors