dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

IMU handle accel/gyro FIFO as separate type

This commit is contained in:
Daniel Agar 2021-01-17 12:57:44 -05:00
parent cf2ffb1086
commit 050fa47aca
63 changed files with 1840 additions and 962 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/sensor_accel.msg
View File

@ -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

12
msg/sensor_accel_fifo.msg
View File

@ -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

12
msg/sensor_gyro_fifo.msg
View File

@ -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

2
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@ -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};

4
src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
View File

@ -419,8 +419,8 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime &timestamp_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) +

4
src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
View File

@ -416,8 +416,8 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime &timestamp_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) +

1
src/drivers/imu/bosch/bmi055/CMakeLists.txt
View File

@ -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

4
src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
View File

@ -505,8 +505,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_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

4
src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
View File

@ -417,8 +417,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_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) +

1
src/drivers/imu/bosch/bmi088/CMakeLists.txt
View File

@ -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

4
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp
View File

@ -419,8 +419,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_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

12
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp
View File

@ -364,8 +364,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_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 &timestamp_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 &timestamp_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++;
}

3
src/drivers/imu/invensense/icm20602/CMakeLists.txt
View File

@ -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
)

127
src/drivers/imu/invensense/icm20602/ICM20602.cpp
View File

@ -33,6 +33,8 @@
#include "ICM20602.hpp"
#include <lib/parameters/param.h>
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 &timestamp_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 &timestamp_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 &timestamp_sample, const FIFO::DAT
void ICM20602::ProcessGyro(const hrt_abstime &timestamp_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 &timestamp_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 {

14
src/drivers/imu/invensense/icm20602/ICM20602.hpp
View File

@ -43,13 +43,14 @@
#include "InvenSense_ICM20602_registers.hpp"
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/PublicationMulti.hpp>
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_s> _sensor_accel_fifo_pub{ORB_ID(sensor_accel_fifo)};
uORB::PublicationMulti<sensor_gyro_fifo_s> _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")};

1
src/drivers/imu/invensense/icm20608g/CMakeLists.txt
View File

@ -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

8
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
View File

@ -544,8 +544,8 @@ bool ICM20608G::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

1
src/drivers/imu/invensense/icm20649/CMakeLists.txt
View File

@ -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

8
src/drivers/imu/invensense/icm20649/ICM20649.cpp
View File

@ -601,8 +601,8 @@ bool ICM20649::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

1
src/drivers/imu/invensense/icm20689/CMakeLists.txt
View File

@ -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

127
src/drivers/imu/invensense/icm20689/ICM20689.cpp
View File

@ -33,6 +33,8 @@
#include "ICM20689.hpp"
#include <lib/parameters/param.h>
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 &timestamp_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 &timestamp_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 &timestamp_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 &timestamp_sample, const FIFO::DAT
void ICM20689::ProcessGyro(const hrt_abstime &timestamp_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 &timestamp_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;
}
}

14
src/drivers/imu/invensense/icm20689/ICM20689.hpp
View File

@ -43,13 +43,14 @@
#include "InvenSense_ICM20689_registers.hpp"
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/PublicationMulti.hpp>
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_s> _sensor_accel_fifo_pub{ORB_ID(sensor_accel_fifo)};
uORB::PublicationMulti<sensor_gyro_fifo_s> _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")};

1
src/drivers/imu/invensense/icm20948/CMakeLists.txt
View File

@ -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

8
src/drivers/imu/invensense/icm20948/ICM20948.cpp
View File

@ -646,8 +646,8 @@ bool ICM20948::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

1
src/drivers/imu/invensense/icm40609d/CMakeLists.txt
View File

@ -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

8
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
View File

@ -603,8 +603,8 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

1
src/drivers/imu/invensense/icm42605/CMakeLists.txt
View File

@ -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

8
src/drivers/imu/invensense/icm42605/ICM42605.cpp
View File

@ -604,8 +604,8 @@ void ICM42605::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
View File

@ -653,8 +653,8 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

1
src/drivers/imu/invensense/mpu6000/CMakeLists.txt
View File

@ -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

8
src/drivers/imu/invensense/mpu6000/MPU6000.cpp
View File

@ -535,8 +535,8 @@ bool MPU6000::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

1
src/drivers/imu/invensense/mpu6500/CMakeLists.txt
View File

@ -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

8
src/drivers/imu/invensense/mpu6500/MPU6500.cpp
View File

@ -548,8 +548,8 @@ bool MPU6500::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

1
src/drivers/imu/invensense/mpu9250/CMakeLists.txt
View File

@ -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

8
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
View File

@ -582,8 +582,8 @@ bool MPU9250::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
View File

@ -529,8 +529,8 @@ bool MPU9250_I2C::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

4
src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
View File

@ -357,7 +357,7 @@ bool LSM9DS1::FIFORead(const hrt_abstime &timestamp_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 &timestamp_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 {

2
src/drivers/magnetometer/isentek/ist8308/IST8308.cpp
View File

@ -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);

2
src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
View File

@ -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);

4
src/drivers/magnetometer/qmc5883l/QMC5883L.cpp
View File

@ -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);

199
src/lib/conversion/rotation.cpp
View File

@ -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;
}
}

246
src/lib/conversion/rotation.h
View File

@ -158,11 +158,251 @@ get_rot_matrix(enum Rotation rot);
__EXPORT matrix::Quatf
get_rot_quaternion(enum Rotation rot);
template<typename T>
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_ */

143
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@ -33,146 +33,3 @@
#include "PX4Accelerometer.hpp"
#include <lib/drivers/device/Device.hpp>
#include <lib/parameters/param.h>
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 &timestamp_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 &timestamp_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);
}

70
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@ -40,32 +40,80 @@
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/parameters/param.h>
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 &timestamp_sample, float x, float y, float z);
void update(const hrt_abstime &timestamp_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 &timestamp_sample, float x, float y, float z, uint8_t clip_count[3]);
void UpdateClipLimit();
uORB::PublicationMulti<sensor_accel_s> _sensor_pub{ORB_ID(sensor_accel)};
uORB::PublicationMulti<sensor_accel_fifo_s> _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] {};

103
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -33,106 +33,3 @@
#include "PX4Gyroscope.hpp"
#include <lib/drivers/device/Device.hpp>
#include <lib/parameters/param.h>
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 &timestamp_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 &timestamp_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);
}

62
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -39,31 +39,79 @@
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/parameters/param.h>
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 &timestamp_sample, float x, float y, float z);
void update(const hrt_abstime &timestamp_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 &timestamp_sample, float x, float y, float z);
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};

15
src/lib/mathlib/math/Functions.hpp
View File

@ -198,4 +198,19 @@ const T sqrt_linear(const T &value)
}
}
template<typename T>
constexpr T negate(T value)
{
return -value;
}
template<>
constexpr int16_t negate<int16_t>(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 */

21
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@ -33,8 +33,6 @@
#include "LowPassFilter2p.hpp"
#include <px4_platform_common/defines.h>
#include <math.h>
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);

20
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@ -38,6 +38,8 @@
#pragma once
#include <px4_platform_common/defines.h>
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; }

30
src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp
View File

@ -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;
}
};

10
src/lib/mathlib/math/filter/NotchFilterArray.hpp
View File

@ -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)) {

67
src/modules/sensors/sensors.cpp
View File

@ -45,6 +45,7 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <lib/airspeed/airspeed.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@ -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<sensor_accel_s> accel_sub{ORB_ID(sensor_accel), i};
uORB::SubscriptionData<sensor_gyro_s> 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<sensor_accel_fifo_s> accel_sub{ORB_ID(sensor_accel_fifo), i};
uORB::SubscriptionData<sensor_gyro_fifo_s> 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;
}

2
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@ -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);
}
}

458
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -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<vehicle_imu_status_s> 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_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
uORB::SubscriptionData<sensor_gyro_fifo_s> 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_s> 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<vehicle_imu_status_s> 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 &timestamp_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();
}

54
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -36,8 +36,9 @@
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/mathlib/math/filter/NotchFilter.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/NotchFilterArray.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@ -49,6 +50,8 @@
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_fft.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
@ -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 &timestamp_sample, const matrix::Vector3f &angular_velocity,
const matrix::Vector3f &angular_acceleration);
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<vehicle_angular_acceleration_s> _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<matrix::Vector3f> _notch_filter_velocity{};
math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::NotchFilterArray<float> _notch_filter_velocity[3] {};
math::NotchFilterArray<float> _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<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,

2
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
View File

@ -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)

2
src/modules/sensors/vehicle_imu/CMakeLists.txt
View File

@ -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)

18
src/modules/sensors/vehicle_imu/Integrator.cpp
View File

@ -37,22 +37,13 @@
using matrix::Vector3f;
bool Integrator::put(const hrt_abstime &timestamp, 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<float>(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

28
src/modules/sensors/vehicle_imu/Integrator.hpp
View File

@ -58,29 +58,14 @@ public:
* @param val Item to put.
* @return true if data was accepted and integrated.
*/
bool put(const uint64_t &timestamp, 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 &timestamp, 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 */
};

78
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -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);
}

4
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -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")};

569
src/modules/sensors/vehicle_imu/VehicleImuFifo.cpp Normal file
View File

@ -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 <px4_platform_common/log.h>
#include <float.h>
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(&param_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 &timestamp_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

151
src/modules/sensors/vehicle_imu/VehicleImuFifo.hpp Normal file
View File

@ -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 <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Accelerometer.hpp>
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
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 &timestamp_sample, uint8_t samples);
void UpdateIntegratorConfiguration();
void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);
uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
uORB::PublicationMulti<vehicle_imu_status_s> _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<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax
)
};
} // namespace sensors