forked from Archive/PX4-Autopilot
IMU handle accel/gyro FIFO as separate type
This commit is contained in:
parent
cf2ffb1086
commit
050fa47aca
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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};
|
||||
|
|
|
@ -419,8 +419,8 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[i] = accel_x;
|
||||
accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[i] = math::negate(accel_y);
|
||||
accel.z[i] = math::negate(accel_z);
|
||||
}
|
||||
|
||||
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -416,8 +416,8 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -505,8 +505,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
|
||||
fifo_buffer_index += 7; // move forward to next record
|
||||
|
|
|
@ -417,8 +417,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -419,8 +419,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
|
||||
fifo_buffer_index += 7; // move forward to next record
|
||||
|
|
|
@ -364,8 +364,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
@ -452,8 +452,8 @@ bool BMI088_Gyroscope::NormalRead(const hrt_abstime ×tamp_sample)
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
x = gyro_x;
|
||||
y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
y = math::negate(gyro_y);
|
||||
z = math::negate(gyro_z);
|
||||
|
||||
_px4_gyro.update(timestamp_sample, x, y, z);
|
||||
|
||||
|
@ -502,8 +502,8 @@ bool BMI088_Gyroscope::SimpleFIFORead(const hrt_abstime ×tamp_sample)
|
|||
};
|
||||
|
||||
gyro.x[i] = xyz[0];
|
||||
gyro.y[i] = (xyz[1] == INT16_MIN) ? INT16_MAX : -xyz[1];
|
||||
gyro.z[i] = (xyz[2] == INT16_MIN) ? INT16_MAX : -xyz[2];
|
||||
gyro.y[i] = math::negate(xyz[1]);
|
||||
gyro.z[i] = math::negate(xyz[2]);
|
||||
gyro.samples++;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
)
|
||||
|
|
|
@ -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 ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_accel_fifo_s accel{};
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = 0;
|
||||
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
|
||||
|
||||
bool bad_data = false;
|
||||
|
||||
|
@ -591,16 +533,25 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_z);
|
||||
|
||||
rotate_3i(_rotation, accel.x[accel.samples], accel.y[accel.samples], accel.z[accel.samples]);
|
||||
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
// publish
|
||||
if (accel.samples > 0) {
|
||||
_px4_accel.updateFIFO(accel);
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
|
||||
accel.device_id = get_device_id();
|
||||
accel.temperature = _temperature_last;
|
||||
accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(
|
||||
_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
|
||||
accel.scale = CONSTANTS_ONE_G / 2048.f;
|
||||
accel.timestamp = hrt_absolute_time();
|
||||
_sensor_accel_fifo_pub.publish(accel);
|
||||
}
|
||||
|
||||
return !bad_data;
|
||||
|
@ -609,9 +560,6 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_gyro_fifo_s gyro{};
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
|
||||
|
@ -621,14 +569,23 @@ void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
|
||||
rotate_3i(_rotation, gyro.x[i], gyro.y[i], gyro.z[i]);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
// publish
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = FIFO_SAMPLE_DT;
|
||||
gyro.device_id = get_device_id();
|
||||
gyro.temperature = _temperature_last;
|
||||
gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(
|
||||
_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
|
||||
gyro.scale = math::radians(2000.f / 32768.f);
|
||||
gyro.timestamp = hrt_absolute_time();
|
||||
_sensor_gyro_fifo_pub.publish(gyro);
|
||||
}
|
||||
|
||||
bool ICM20602::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
|
||||
|
@ -659,8 +616,8 @@ bool ICM20602::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples
|
|||
&& (temperature_C >= TEMPERATURE_SENSOR_MIN)
|
||||
&& (temperature_C <= TEMPERATURE_SENSOR_MAX)) {
|
||||
|
||||
_px4_accel.set_temperature(temperature_C);
|
||||
_px4_gyro.set_temperature(temperature_C);
|
||||
_temperature_last = temperature_C;
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
|
|
|
@ -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")};
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -544,8 +544,8 @@ bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
|
@ -574,8 +574,8 @@ void ICM20608G::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -601,8 +601,8 @@ bool ICM20649::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
|
@ -631,8 +631,8 @@ void ICM20649::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 ×tamp_sample, uint8_t samples)
|
|||
return false;
|
||||
}
|
||||
|
||||
|
||||
ProcessGyro(timestamp_sample, buffer.f, samples);
|
||||
return ProcessAccel(timestamp_sample, buffer.f, samples);
|
||||
}
|
||||
|
@ -509,9 +453,6 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
|
|||
bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_accel_fifo_s accel{};
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = 0;
|
||||
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
|
||||
|
||||
bool bad_data = false;
|
||||
|
||||
|
@ -544,16 +485,25 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_z);
|
||||
|
||||
rotate_3i(_rotation, accel.x[accel.samples], accel.y[accel.samples], accel.z[accel.samples]);
|
||||
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
// publish
|
||||
if (accel.samples > 0) {
|
||||
_px4_accel.updateFIFO(accel);
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
|
||||
accel.device_id = get_device_id();
|
||||
accel.temperature = _temperature_last;
|
||||
accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(
|
||||
_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
|
||||
accel.scale = CONSTANTS_ONE_G / 2048.f;
|
||||
accel.timestamp = hrt_absolute_time();
|
||||
_sensor_accel_fifo_pub.publish(accel);
|
||||
}
|
||||
|
||||
return !bad_data;
|
||||
|
@ -562,9 +512,6 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_gyro_fifo_s gyro{};
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
|
||||
|
@ -574,14 +521,23 @@ void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
|
||||
rotate_3i(_rotation, gyro.x[i], gyro.y[i], gyro.z[i]);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
// publish
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = FIFO_SAMPLE_DT;
|
||||
gyro.device_id = get_device_id();
|
||||
gyro.temperature = _temperature_last;
|
||||
gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(
|
||||
_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
|
||||
gyro.scale = math::radians(2000.f / 32768.f);
|
||||
gyro.timestamp = hrt_absolute_time();
|
||||
_sensor_gyro_fifo_pub.publish(gyro);
|
||||
}
|
||||
|
||||
void ICM20689::UpdateTemperature()
|
||||
|
@ -599,7 +555,6 @@ void ICM20689::UpdateTemperature()
|
|||
const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
|
||||
|
||||
if (PX4_ISFINITE(TEMP_degC)) {
|
||||
_px4_accel.set_temperature(TEMP_degC);
|
||||
_px4_gyro.set_temperature(TEMP_degC);
|
||||
_temperature_last = TEMP_degC;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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")};
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -646,8 +646,8 @@ bool ICM20948::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
|
@ -676,8 +676,8 @@ void ICM20948::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -603,8 +603,8 @@ void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
|
@ -631,8 +631,8 @@ void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -604,8 +604,8 @@ void ICM42605::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
|
@ -632,8 +632,8 @@ void ICM42605::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -653,8 +653,8 @@ void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[i] = accel.x[i];
|
||||
accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
|
||||
accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
|
||||
accel.y[i] = math::negate(accel.y[i]);
|
||||
accel.z[i] = math::negate(accel.z[i]);
|
||||
}
|
||||
|
||||
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
@ -725,8 +725,8 @@ void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro.x[i];
|
||||
gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
|
||||
gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
|
||||
gyro.y[i] = math::negate(gyro.y[i]);
|
||||
gyro.z[i] = math::negate(gyro.z[i]);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -535,8 +535,8 @@ bool MPU6000::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
|
||||
} else if (new_sample && (_fifo_accel_samples_count > 1)) {
|
||||
|
@ -577,8 +577,8 @@ void MPU6000::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -548,8 +548,8 @@ bool MPU6500::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
|
@ -578,8 +578,8 @@ void MPU6500::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -582,8 +582,8 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
|
@ -612,8 +612,8 @@ void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -529,8 +529,8 @@ bool MPU9250_I2C::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.y[accel.samples] = math::negate(accel_y);
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
}
|
||||
|
||||
|
@ -559,8 +559,8 @@ void MPU9250_I2C::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::D
|
|||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
|
|
|
@ -357,7 +357,7 @@ bool LSM9DS1::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
|||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[gyro.samples] = gyro_x;
|
||||
gyro.y[gyro.samples] = gyro_y;
|
||||
gyro.z[gyro.samples] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.z[gyro.samples] = math::negate(gyro_z);
|
||||
gyro.samples++;
|
||||
|
||||
} else {
|
||||
|
@ -385,7 +385,7 @@ bool LSM9DS1::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
|||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.z[accel.samples] = math::negate(accel_y);
|
||||
accel.samples++;
|
||||
|
||||
} else {
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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_ */
|
||||
|
|
|
@ -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 ×tamp_sample, float x, float y, float z)
|
||||
{
|
||||
// clipping
|
||||
uint8_t clip_count[3];
|
||||
clip_count[0] = (fabsf(x) >= _clip_limit);
|
||||
clip_count[1] = (fabsf(y) >= _clip_limit);
|
||||
clip_count[2] = (fabsf(z) >= _clip_limit);
|
||||
|
||||
// publish
|
||||
Publish(timestamp_sample, x, y, z, clip_count);
|
||||
}
|
||||
|
||||
void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
|
||||
{
|
||||
// publish fifo
|
||||
sample.device_id = _device_id;
|
||||
sample.scale = _scale;
|
||||
sample.rotation = _rotation;
|
||||
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(sample);
|
||||
|
||||
{
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
const uint8_t N = sample.samples;
|
||||
const Vector3f integral{
|
||||
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
|
||||
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
|
||||
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
|
||||
};
|
||||
|
||||
_last_sample[0] = sample.x[N - 1];
|
||||
_last_sample[1] = sample.y[N - 1];
|
||||
_last_sample[2] = sample.z[N - 1];
|
||||
|
||||
// clipping
|
||||
uint8_t clip_count[3] {
|
||||
clipping(sample.x, _clip_limit, N),
|
||||
clipping(sample.y, _clip_limit, N),
|
||||
clipping(sample.z, _clip_limit, N),
|
||||
};
|
||||
|
||||
const float x = integral(0) / (float)N;
|
||||
const float y = integral(1) / (float)N;
|
||||
const float z = integral(2) / (float)N;
|
||||
|
||||
// publish
|
||||
Publish(sample.timestamp_sample, x, y, z, clip_count);
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3])
|
||||
{
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
|
||||
float clipping_x = clip_count[0];
|
||||
float clipping_y = clip_count[1];
|
||||
float clipping_z = clip_count[2];
|
||||
rotate_3f(_rotation, clipping_x, clipping_y, clipping_z);
|
||||
|
||||
sensor_accel_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.error_count = _error_count;
|
||||
report.x = x * _scale;
|
||||
report.y = y * _scale;
|
||||
report.z = z * _scale;
|
||||
report.clip_counter[0] = fabsf(roundf(clipping_x));
|
||||
report.clip_counter[1] = fabsf(roundf(clipping_y));
|
||||
report.clip_counter[2] = fabsf(roundf(clipping_z));
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
void PX4Accelerometer::UpdateClipLimit()
|
||||
{
|
||||
// 99.9% of potential max
|
||||
_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
|
||||
}
|
||||
|
|
|
@ -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 ×tamp_sample, float x, float y, float z);
|
||||
void update(const hrt_abstime ×tamp_sample, float x, float y, float z)
|
||||
{
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
|
||||
void updateFIFO(sensor_accel_fifo_s &sample);
|
||||
sensor_accel_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.error_count = _error_count;
|
||||
report.x = x * _scale;
|
||||
report.y = y * _scale;
|
||||
report.z = z * _scale;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
void updateFIFO(sensor_accel_fifo_s &sample)
|
||||
{
|
||||
// publish fifo
|
||||
sample.device_id = _device_id;
|
||||
sample.temperature = _temperature;
|
||||
sample.error_count = _error_count;
|
||||
sample.scale = _scale;
|
||||
|
||||
for (int i = 0; i < sample.samples; i++) {
|
||||
rotate_3i(_rotation, sample.x[i], sample.y[i], sample.z[i]);
|
||||
}
|
||||
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(sample);
|
||||
}
|
||||
|
||||
private:
|
||||
void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]);
|
||||
void UpdateClipLimit();
|
||||
|
||||
uORB::PublicationMulti<sensor_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] {};
|
||||
|
|
|
@ -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 ×tamp_sample, float x, float y, float z)
|
||||
{
|
||||
// publish
|
||||
Publish(timestamp_sample, x, y, z);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
|
||||
{
|
||||
// publish fifo
|
||||
sample.device_id = _device_id;
|
||||
sample.scale = _scale;
|
||||
sample.rotation = _rotation;
|
||||
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(sample);
|
||||
|
||||
{
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
const uint8_t N = sample.samples;
|
||||
const Vector3f integral{
|
||||
(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
|
||||
(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
|
||||
(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
|
||||
};
|
||||
|
||||
_last_sample[0] = sample.x[N - 1];
|
||||
_last_sample[1] = sample.y[N - 1];
|
||||
_last_sample[2] = sample.z[N - 1];
|
||||
|
||||
const float x = integral(0) / (float)N;
|
||||
const float y = integral(1) / (float)N;
|
||||
const float z = integral(2) / (float)N;
|
||||
|
||||
// publish
|
||||
Publish(sample.timestamp_sample, x, y, z);
|
||||
}
|
||||
}
|
||||
|
||||
void PX4Gyroscope::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z)
|
||||
{
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
|
||||
sensor_gyro_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.error_count = _error_count;
|
||||
report.x = x * _scale;
|
||||
report.y = y * _scale;
|
||||
report.z = z * _scale;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
|
|
@ -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 ×tamp_sample, float x, float y, float z);
|
||||
void update(const hrt_abstime ×tamp_sample, float x, float y, float z)
|
||||
{
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
|
||||
void updateFIFO(sensor_gyro_fifo_s &sample);
|
||||
sensor_gyro_s report;
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.error_count = _error_count;
|
||||
report.x = x * _scale;
|
||||
report.y = y * _scale;
|
||||
report.z = z * _scale;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
void updateFIFO(sensor_gyro_fifo_s &sample)
|
||||
{
|
||||
// publish fifo
|
||||
sample.device_id = _device_id;
|
||||
sample.temperature = _temperature;
|
||||
sample.error_count = _error_count;
|
||||
sample.scale = _scale;
|
||||
|
||||
for (int i = 0; i < sample.samples; i++) {
|
||||
rotate_3i(_rotation, sample.x[i], sample.y[i], sample.z[i]);
|
||||
}
|
||||
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(sample);
|
||||
}
|
||||
private:
|
||||
void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z);
|
||||
|
||||
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
|
||||
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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; }
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
};
|
||||
|
|
|
@ -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,7 +92,7 @@ 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 -
|
||||
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
|
||||
|
|
|
@ -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;
|
||||
|
@ -178,6 +180,7 @@ private:
|
|||
VehicleGPSPosition *_vehicle_gps_position{nullptr};
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
// reset sample interval accumulator
|
||||
_timestamp_interval_last = 0;
|
||||
_sample_rate_determined = true;
|
||||
}
|
||||
|
||||
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);
|
||||
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));
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
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();
|
||||
|
||||
if (_fifo_available) {
|
||||
|
||||
// dynamic notch filter update
|
||||
if (_sample_rate_determined) {
|
||||
sensor_gyro_fft_s sensor_gyro_fft;
|
||||
|
||||
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++) {
|
||||
|
||||
float *peak_frequencies = nullptr;
|
||||
|
||||
switch (axis) {
|
||||
case 0:
|
||||
peak_frequencies = sensor_gyro_fft.peak_frequencies_x;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
peak_frequencies = sensor_gyro_fft.peak_frequencies_y;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
peak_frequencies = sensor_gyro_fft.peak_frequencies_z;
|
||||
break;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(peak_frequencies[i]) && (peak_frequencies[i] > 10.f)
|
||||
&& fabsf(_dynamic_notch_filter[i][axis].getNotchFreq() - peak_frequencies[i]) > 0.1f) {
|
||||
|
||||
_dynamic_notch_filter[i][axis].setParameters(_filter_sample_rate,
|
||||
peak_frequencies[i], sensor_gyro_fft.resolution_hz);
|
||||
|
||||
} else {
|
||||
// disable
|
||||
_dynamic_notch_filter[i][axis].setParameters(_filter_sample_rate, 0, sensor_gyro_fft.resolution_hz);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// process all outstanding fifo messages
|
||||
sensor_gyro_fifo_s sensor_fifo_data;
|
||||
|
||||
while (_sensor_fifo_sub.update(&sensor_fifo_data)) {
|
||||
|
||||
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_prev) / 1e6f), 0.0002f, 0.02f);
|
||||
_timestamp_sample_prev = sensor_data.timestamp_sample;
|
||||
const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) / 1e6f), 0.0002f, 0.02f);
|
||||
_timestamp_sample_last = sensor_data.timestamp_sample;
|
||||
|
||||
// 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};
|
||||
// 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{};
|
||||
|
||||
// correct for in-run bias errors
|
||||
const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias;
|
||||
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];
|
||||
|
||||
// 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)
|
||||
} 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);
|
||||
|
||||
const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};
|
||||
|
||||
const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
|
||||
|
||||
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)};
|
||||
|
||||
|
||||
// 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;
|
||||
if (PX4_ISFINITE(gyro_rate_hz)) {
|
||||
_filter_sample_rate = gyro_rate_hz;
|
||||
PX4_DEBUG("using sample rate %.3f Hz", (double)_filter_sample_rate);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (publish) {
|
||||
ResetFilters(angular_velocity, angular_acceleration);
|
||||
}
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
// Apply general notch filter (IMU_GYRO_NF_FREQ)
|
||||
_notch_filter_velocity[axis].apply(&angular_velocity(axis), 1);
|
||||
|
||||
// Apply general low-pass filter (IMU_GYRO_CUTOFF)
|
||||
_lp_filter_velocity[axis].apply(&angular_velocity(axis), 1);
|
||||
|
||||
// Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
|
||||
float accel = (angular_velocity(axis) - _angular_velocity_last(axis)) / dt;
|
||||
angular_acceleration(axis) = _lp_filter_acceleration[axis].apply(accel);
|
||||
_angular_velocity_last(axis) = angular_velocity(axis);
|
||||
}
|
||||
|
||||
// Publish
|
||||
if (!_sensor_sub.updated() && (sensor_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) {
|
||||
Publish(sensor_data.timestamp_sample, angular_velocity, angular_acceleration);
|
||||
_last_publish = sensor_data.timestamp_sample;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::Publish(const hrt_abstime ×tamp_sample, const Vector3f &angular_velocity,
|
||||
const Vector3f &angular_acceleration)
|
||||
{
|
||||
// Publish vehicle_angular_acceleration
|
||||
vehicle_angular_acceleration_s v_angular_acceleration;
|
||||
v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample;
|
||||
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 = sensor_data.timestamp_sample;
|
||||
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 = v_angular_velocity.timestamp_sample;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
_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();
|
||||
}
|
||||
|
|
|
@ -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 ×tamp_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,
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -37,22 +37,13 @@
|
|||
|
||||
using matrix::Vector3f;
|
||||
|
||||
bool Integrator::put(const hrt_abstime ×tamp, const Vector3f &val)
|
||||
bool Integrator::put(const Vector3f &val, const float dt)
|
||||
{
|
||||
if ((_last_integration_time == 0) || (timestamp <= _last_integration_time)) {
|
||||
/* this is the first item in the integrator */
|
||||
_last_integration_time = timestamp;
|
||||
_last_reset_time = timestamp;
|
||||
_last_val = val;
|
||||
|
||||
return false;
|
||||
}
|
||||
_integral_dt += dt;
|
||||
|
||||
// Use trapezoidal integration to calculate the delta integral
|
||||
const float dt = static_cast<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
|
||||
|
|
|
@ -58,29 +58,14 @@ public:
|
|||
* @param val Item to put.
|
||||
* @return true if data was accepted and integrated.
|
||||
*/
|
||||
bool put(const uint64_t ×tamp, const matrix::Vector3f &val);
|
||||
|
||||
/**
|
||||
* Put an item into the integral.
|
||||
*
|
||||
* @param timestamp Timestamp of the current value.
|
||||
* @param val Item to put.
|
||||
* @param integral Current integral in case the integrator did reset, else the value will not be modified
|
||||
* @param integral_dt Get the dt in us of the current integration (only if reset).
|
||||
* @return true if putting the item triggered an integral reset and the integral should be
|
||||
* published.
|
||||
*/
|
||||
bool put(const uint64_t ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
|
||||
{
|
||||
return put(timestamp, val) && reset(integral, integral_dt);
|
||||
}
|
||||
bool put(const matrix::Vector3f &val, const float dt);
|
||||
|
||||
/**
|
||||
* Set reset interval during runtime. This won't reset the integrator.
|
||||
*
|
||||
* @param reset_interval New reset time interval for the integrator.
|
||||
*/
|
||||
void set_reset_interval(uint32_t reset_interval) { _reset_interval_min = reset_interval; }
|
||||
void set_reset_interval(uint32_t reset_interval) { _reset_interval_min = reset_interval * 1e-6f; }
|
||||
|
||||
/**
|
||||
* Set required samples for reset. This won't reset the integrator.
|
||||
|
@ -95,7 +80,7 @@ public:
|
|||
*
|
||||
* @return true if integrator has sufficient data (minimum interval & samples satisfied) to reset.
|
||||
*/
|
||||
bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_last_integration_time >= (_last_reset_time + _reset_interval_min)); }
|
||||
bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); }
|
||||
|
||||
/* Reset integrator and return current integral & integration time
|
||||
*
|
||||
|
@ -105,19 +90,18 @@ public:
|
|||
bool reset(matrix::Vector3f &integral, uint32_t &integral_dt);
|
||||
|
||||
private:
|
||||
uint64_t _last_integration_time{0}; /**< timestamp of the last integration step */
|
||||
uint64_t _last_reset_time{0}; /**< last auto-announcement of integral value */
|
||||
|
||||
matrix::Vector3f _alpha{0.f, 0.f, 0.f}; /**< integrated value before coning corrections are applied */
|
||||
matrix::Vector3f _last_alpha{0.f, 0.f, 0.f}; /**< previous value of _alpha */
|
||||
matrix::Vector3f _beta{0.f, 0.f, 0.f}; /**< accumulated coning corrections */
|
||||
matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */
|
||||
matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */
|
||||
|
||||
uint32_t _reset_interval_min{1}; /**< the interval after which the content will be published and the integrator reset */
|
||||
float _reset_interval_min{0.001}; /**< the interval after which the content will be published and the integrator reset */
|
||||
|
||||
uint8_t _integrated_samples{0};
|
||||
uint8_t _reset_samples_min{1};
|
||||
|
||||
float _integral_dt{0};
|
||||
|
||||
const bool _coning_comp_on{false}; /**< true to turn on coning corrections */
|
||||
};
|
||||
|
|
|
@ -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,38 +292,42 @@ 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 (dt > 0.f && dt < 1.f) {
|
||||
_accel_integrator.put(accel_raw, dt);
|
||||
}
|
||||
|
||||
if (clip_y > 0) {
|
||||
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y;
|
||||
}
|
||||
// if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
|
||||
|
||||
if (clip_z > 0) {
|
||||
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z;
|
||||
}
|
||||
// // 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]}};
|
||||
|
||||
publish_status = true;
|
||||
}
|
||||
// // 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
|
||||
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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")};
|
||||
|
|
|
@ -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(¶m_update);
|
||||
|
||||
const auto imu_integ_rate_prev = _param_imu_integ_rate.get();
|
||||
|
||||
updateParams();
|
||||
|
||||
_accel_calibration.ParametersUpdate();
|
||||
_gyro_calibration.ParametersUpdate();
|
||||
|
||||
// constrain IMU integration time 1-10 milliseconds (100-1000 Hz)
|
||||
int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(),
|
||||
100, math::max(_param_imu_gyro_ratemax.get(), 1000));
|
||||
|
||||
if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
|
||||
PX4_WARN("IMU_INTEG_RATE updated %d -> %d", _param_imu_integ_rate.get(), imu_integration_rate_hz);
|
||||
_param_imu_integ_rate.set(imu_integration_rate_hz);
|
||||
_param_imu_integ_rate.commit_no_notification();
|
||||
}
|
||||
|
||||
_imu_integration_interval_us = 1000000 / imu_integration_rate_hz;
|
||||
|
||||
if (_param_imu_integ_rate.get() != imu_integ_rate_prev) {
|
||||
// force update
|
||||
UpdateIntegratorConfiguration();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool VehicleImuFifo::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime ×tamp_sample,
|
||||
uint8_t samples)
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
// conservative maximum time between samples to reject large gaps and reset averaging
|
||||
float max_interval_us = 10000; // 100 Hz
|
||||
float min_interval_us = 50; // 20,000 Hz
|
||||
|
||||
if (intavg.update_interval > 0) {
|
||||
// if available use previously calculated interval as bounds
|
||||
max_interval_us = 1.5f * intavg.update_interval;
|
||||
min_interval_us = 0.5f * intavg.update_interval;
|
||||
}
|
||||
|
||||
const float interval_us = (timestamp_sample - intavg.timestamp_sample_last);
|
||||
|
||||
if ((intavg.timestamp_sample_last > 0) && (interval_us < max_interval_us) && (interval_us > min_interval_us)) {
|
||||
|
||||
intavg.interval_sum += interval_us;
|
||||
intavg.sample_count += samples;
|
||||
intavg.interval_count++;
|
||||
|
||||
// periodically calculate sensor update rate
|
||||
if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) {
|
||||
|
||||
const float interval_avg = intavg.interval_sum / intavg.interval_count;
|
||||
const float sample_interval_avg = intavg.interval_sum / intavg.sample_count;
|
||||
|
||||
if (PX4_ISFINITE(interval_avg) && (interval_avg > 0.f)) {
|
||||
// update if interval has changed by more than 0.5%
|
||||
if ((fabsf(intavg.update_interval - interval_avg) / intavg.update_interval) > 0.005f) {
|
||||
|
||||
intavg.update_interval = interval_avg;
|
||||
intavg.sample_interval = sample_interval_avg;
|
||||
updated = true;
|
||||
}
|
||||
}
|
||||
|
||||
// reset sample interval accumulator
|
||||
intavg.interval_sum = 0.f;
|
||||
intavg.sample_count = 0;
|
||||
intavg.interval_count = 0.f;
|
||||
}
|
||||
|
||||
} else {
|
||||
// reset
|
||||
intavg.interval_sum = 0.f;
|
||||
intavg.sample_count = 0;
|
||||
intavg.interval_count = 0.f;
|
||||
}
|
||||
|
||||
intavg.timestamp_sample_last = timestamp_sample;
|
||||
|
||||
return updated;
|
||||
}
|
||||
|
||||
void VehicleImuFifo::Run()
|
||||
{
|
||||
// backup schedule
|
||||
ScheduleDelayed(10_ms);
|
||||
|
||||
ParametersUpdate();
|
||||
|
||||
if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
bool sensor_data_gap = false;
|
||||
bool update_integrator_config = false;
|
||||
|
||||
// integrate queued gyro
|
||||
sensor_gyro_fifo_s sensor_gyro_fifo;
|
||||
|
||||
while (_sensor_gyro_sub.update(&sensor_gyro_fifo)) {
|
||||
perf_count_interval(_gyro_update_perf, sensor_gyro_fifo.timestamp_sample);
|
||||
|
||||
if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) {
|
||||
sensor_data_gap = true;
|
||||
perf_count(_gyro_generation_gap_perf);
|
||||
|
||||
_gyro_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging
|
||||
|
||||
} else {
|
||||
// collect sample interval average for filters
|
||||
if (!_intervals_configured
|
||||
&& UpdateIntervalAverage(_gyro_interval, sensor_gyro_fifo.timestamp_sample, sensor_gyro_fifo.samples)) {
|
||||
update_integrator_config = true;
|
||||
_publish_status = true;
|
||||
_status.gyro_rate_hz = 1e6f / _gyro_interval.sample_interval;
|
||||
}
|
||||
}
|
||||
|
||||
_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
|
||||
|
||||
_gyro_calibration.set_device_id(sensor_gyro_fifo.device_id);
|
||||
|
||||
if (sensor_gyro_fifo.error_count != _status.gyro_error_count) {
|
||||
_publish_status = true;
|
||||
_status.gyro_error_count = sensor_gyro_fifo.error_count;
|
||||
}
|
||||
|
||||
_last_timestamp_sample_gyro = sensor_gyro_fifo.timestamp_sample;
|
||||
|
||||
const float dt_s = sensor_gyro_fifo.dt * 1e-6f;
|
||||
const int N = sensor_gyro_fifo.samples;
|
||||
|
||||
for (int n = 0; n < N; n++) {
|
||||
const Vector3f gyro_raw{
|
||||
sensor_gyro_fifo.scale *sensor_gyro_fifo.x[n],
|
||||
sensor_gyro_fifo.scale *sensor_gyro_fifo.y[n],
|
||||
sensor_gyro_fifo.scale *sensor_gyro_fifo.z[n]
|
||||
};
|
||||
|
||||
_gyro_integrator.put(gyro_raw, dt_s);
|
||||
|
||||
_gyro_sum += gyro_raw;
|
||||
_gyro_sum_count++;
|
||||
}
|
||||
|
||||
_gyro_temperature += sensor_gyro_fifo.temperature;
|
||||
_gyro_temperature_count++;
|
||||
|
||||
// break if interval is configured and we haven't fallen behind
|
||||
if (_intervals_configured && _gyro_integrator.integral_ready()
|
||||
&& (hrt_elapsed_time(&sensor_gyro_fifo.timestamp) < _imu_integration_interval_us) && !sensor_data_gap) {
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// update accel, stopping once caught up to the last gyro sample
|
||||
sensor_accel_fifo_s sensor_accel_fifo;
|
||||
|
||||
while (_sensor_accel_sub.update(&sensor_accel_fifo)) {
|
||||
perf_count_interval(_accel_update_perf, sensor_accel_fifo.timestamp_sample);
|
||||
|
||||
if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) {
|
||||
sensor_data_gap = true;
|
||||
perf_count(_accel_generation_gap_perf);
|
||||
|
||||
_accel_interval.timestamp_sample_last = 0; // invalidate any ongoing publication rate averaging
|
||||
|
||||
} else {
|
||||
// collect sample interval average for filters
|
||||
if (!_intervals_configured
|
||||
&& UpdateIntervalAverage(_accel_interval, sensor_accel_fifo.timestamp_sample, sensor_accel_fifo.samples)) {
|
||||
update_integrator_config = true;
|
||||
_publish_status = true;
|
||||
_status.accel_rate_hz = 1e6f / _accel_interval.sample_interval;
|
||||
}
|
||||
}
|
||||
|
||||
_accel_last_generation = _sensor_accel_sub.get_last_generation();
|
||||
|
||||
_accel_calibration.set_device_id(sensor_accel_fifo.device_id);
|
||||
|
||||
if (sensor_accel_fifo.error_count != _status.accel_error_count) {
|
||||
_publish_status = true;
|
||||
_status.accel_error_count = sensor_accel_fifo.error_count;
|
||||
}
|
||||
|
||||
_last_timestamp_sample_accel = sensor_accel_fifo.timestamp_sample;
|
||||
|
||||
const float dt_s = sensor_accel_fifo.dt * 1e-6f;
|
||||
const int N = sensor_accel_fifo.samples;
|
||||
|
||||
// clipping
|
||||
Vector3f clip_count {
|
||||
(float)clipping(sensor_accel_fifo.x, UINT16_MAX - 1, N),
|
||||
(float)clipping(sensor_accel_fifo.y, UINT16_MAX - 1, N),
|
||||
(float)clipping(sensor_accel_fifo.z, UINT16_MAX - 1, N),
|
||||
};
|
||||
|
||||
// rotate sensor clip counts into vehicle body frame
|
||||
const Vector3f clipping{_accel_calibration.rotation() *clip_count};
|
||||
|
||||
// round to get reasonble clip counts per axis (after board rotation)
|
||||
if (fabsf(clipping(0)) > 0.f) {
|
||||
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X;
|
||||
}
|
||||
|
||||
if (fabsf(clipping(1)) > 0.f) {
|
||||
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y;
|
||||
}
|
||||
|
||||
if (fabsf(clipping(2)) > 0.f) {
|
||||
_delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z;
|
||||
}
|
||||
|
||||
|
||||
for (int n = 0; n < N; n++) {
|
||||
const Vector3f accel_raw{
|
||||
sensor_accel_fifo.scale *sensor_accel_fifo.x[n],
|
||||
sensor_accel_fifo.scale *sensor_accel_fifo.y[n],
|
||||
sensor_accel_fifo.scale *sensor_accel_fifo.z[n]
|
||||
};
|
||||
|
||||
_accel_integrator.put(accel_raw, dt_s);
|
||||
|
||||
_accel_sum += accel_raw;
|
||||
_accel_sum_count++;
|
||||
|
||||
// break once caught up to gyro
|
||||
if (!sensor_data_gap && _intervals_configured) {
|
||||
// && (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval))) {
|
||||
|
||||
if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) {
|
||||
Publish();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_accel_temperature += sensor_accel_fifo.temperature;
|
||||
_accel_temperature_count++;
|
||||
}
|
||||
|
||||
if (sensor_data_gap) {
|
||||
_consecutive_data_gap++;
|
||||
|
||||
// if there's consistently a gap in data start monitoring publication interval again
|
||||
if (_consecutive_data_gap > 10) {
|
||||
_intervals_configured = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
_consecutive_data_gap = 0;
|
||||
}
|
||||
|
||||
// reconfigure integrators if calculated sensor intervals have changed
|
||||
if (update_integrator_config) {
|
||||
UpdateIntegratorConfiguration();
|
||||
}
|
||||
|
||||
Publish();
|
||||
}
|
||||
|
||||
void VehicleImuFifo::Publish()
|
||||
{
|
||||
// publish if both accel & gyro integrators are ready
|
||||
if (_accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) {
|
||||
|
||||
uint32_t accel_integral_dt;
|
||||
uint32_t gyro_integral_dt;
|
||||
Vector3f delta_angle;
|
||||
Vector3f delta_velocity;
|
||||
|
||||
if (_accel_integrator.reset(delta_velocity, accel_integral_dt)
|
||||
&& _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
|
||||
|
||||
if (_accel_calibration.enabled() && _gyro_calibration.enabled()) {
|
||||
|
||||
// delta angle: apply offsets, scale, and board rotation
|
||||
_gyro_calibration.SensorCorrectionsUpdate();
|
||||
const float gyro_dt_inv = 1.e6f / gyro_integral_dt;
|
||||
const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv};
|
||||
|
||||
// delta velocity: apply offsets, scale, and board rotation
|
||||
_accel_calibration.SensorCorrectionsUpdate();
|
||||
const float accel_dt_inv = 1.e6f / accel_integral_dt;
|
||||
Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv};
|
||||
|
||||
UpdateAccelVibrationMetrics(delta_velocity_corrected);
|
||||
UpdateGyroVibrationMetrics(delta_angle_corrected);
|
||||
|
||||
// vehicle_imu_status
|
||||
// publish before vehicle_imu so that error counts are available synchronously if needed
|
||||
if (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
|
||||
_status.accel_device_id = _accel_calibration.device_id();
|
||||
_status.gyro_device_id = _gyro_calibration.device_id();
|
||||
|
||||
// mean accel
|
||||
const Vector3f accel_mean{_accel_calibration.Correct(_accel_sum / _accel_sum_count)};
|
||||
accel_mean.copyTo(_status.mean_accel);
|
||||
_status.temperature_accel = _accel_temperature / _accel_temperature_count;
|
||||
_accel_sum.zero();
|
||||
_accel_sum_count = 0;
|
||||
_accel_temperature = 0;
|
||||
_accel_temperature_count = 0;
|
||||
|
||||
// mean gyro
|
||||
const Vector3f gyro_mean{_gyro_calibration.Correct(_gyro_sum / _gyro_sum_count)};
|
||||
gyro_mean.copyTo(_status.mean_gyro);
|
||||
_status.temperature_gyro = _gyro_temperature / _gyro_temperature_count;
|
||||
_gyro_sum.zero();
|
||||
_gyro_sum_count = 0;
|
||||
_gyro_temperature = 0;
|
||||
_gyro_temperature_count = 0;
|
||||
|
||||
_status.timestamp = hrt_absolute_time();
|
||||
_vehicle_imu_status_pub.publish(_status);
|
||||
|
||||
_publish_status = false;
|
||||
}
|
||||
|
||||
|
||||
// publish vehicle_imu
|
||||
vehicle_imu_s imu;
|
||||
imu.timestamp_sample = _last_timestamp_sample_gyro;
|
||||
imu.accel_device_id = _accel_calibration.device_id();
|
||||
imu.gyro_device_id = _gyro_calibration.device_id();
|
||||
delta_angle_corrected.copyTo(imu.delta_angle);
|
||||
delta_velocity_corrected.copyTo(imu.delta_velocity);
|
||||
imu.delta_angle_dt = gyro_integral_dt;
|
||||
imu.delta_velocity_dt = accel_integral_dt;
|
||||
imu.delta_velocity_clipping = _delta_velocity_clipping;
|
||||
imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count();
|
||||
imu.timestamp = hrt_absolute_time();
|
||||
_vehicle_imu_pub.publish(imu);
|
||||
}
|
||||
|
||||
// reset clip counts
|
||||
_delta_velocity_clipping = 0;
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleImuFifo::UpdateIntegratorConfiguration()
|
||||
{
|
||||
if ((_accel_interval.update_interval > 0) && (_gyro_interval.update_interval > 0)) {
|
||||
|
||||
const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
|
||||
|
||||
// determine number of sensor samples that will get closest to the desired integration interval
|
||||
const uint8_t accel_pub_integral_samples = math::max(1.f,
|
||||
roundf(configured_interval_us / _accel_interval.update_interval));
|
||||
const uint8_t gyro_pub_integral_samples = math::max(1.f,
|
||||
roundf(configured_interval_us / _gyro_interval.update_interval));
|
||||
|
||||
const uint8_t accel_sample_integral_samples = math::max(1.f,
|
||||
roundf(configured_interval_us / _accel_interval.sample_interval));
|
||||
const uint8_t gyro_sample_integral_samples = math::max(1.f,
|
||||
roundf(configured_interval_us / _gyro_interval.sample_interval));
|
||||
|
||||
// let the gyro set the configuration and scheduling
|
||||
// accel integrator will be forced to reset when gyro integrator is ready
|
||||
_gyro_integrator.set_reset_samples(gyro_sample_integral_samples);
|
||||
_accel_integrator.set_reset_samples(accel_sample_integral_samples);
|
||||
|
||||
// relaxed minimum integration time required
|
||||
_accel_integrator.set_reset_interval(roundf((accel_sample_integral_samples - 0.5f) * _accel_interval.sample_interval));
|
||||
_gyro_integrator.set_reset_interval(roundf((gyro_sample_integral_samples - 0.5f) * _gyro_interval.sample_interval));
|
||||
|
||||
// gyro: find largest integer multiple of gyro_integral_samples
|
||||
for (int n = sensor_gyro_fifo_s::ORB_QUEUE_LENGTH; n > 0; n--) {
|
||||
if (gyro_pub_integral_samples % n == 0) {
|
||||
_sensor_gyro_sub.set_required_updates(n);
|
||||
|
||||
// run when there are enough new gyro samples, unregister accel
|
||||
_sensor_accel_sub.unregisterCallback();
|
||||
|
||||
_intervals_configured = true; // stop monitoring topic publication rates
|
||||
|
||||
PX4_INFO("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f (%.1f), gyro interval: %.1f (%.1f) sub samples: %d",
|
||||
_accel_calibration.device_id(), _gyro_calibration.device_id(),
|
||||
accel_pub_integral_samples, gyro_pub_integral_samples,
|
||||
(double)_accel_interval.update_interval, (double)_accel_interval.sample_interval,
|
||||
(double)_gyro_interval.update_interval, (double)_gyro_interval.sample_interval,
|
||||
n);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleImuFifo::UpdateAccelVibrationMetrics(const Vector3f &delta_velocity)
|
||||
{
|
||||
// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
||||
const Vector3f delta_velocity_diff = delta_velocity - _delta_velocity_prev;
|
||||
_status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric + 0.01f * delta_velocity_diff.norm();
|
||||
|
||||
_delta_velocity_prev = delta_velocity;
|
||||
}
|
||||
|
||||
void VehicleImuFifo::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
|
||||
{
|
||||
// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
||||
const Vector3f delta_angle_diff = delta_angle - _delta_angle_prev;
|
||||
_status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric + 0.01f * delta_angle_diff.norm();
|
||||
|
||||
// Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
|
||||
const Vector3f coning_metric = delta_angle % _delta_angle_prev;
|
||||
_status.gyro_coning_vibration = 0.99f * _status.gyro_coning_vibration + 0.01f * coning_metric.norm();
|
||||
|
||||
_delta_angle_prev = delta_angle;
|
||||
}
|
||||
|
||||
void VehicleImuFifo::PrintStatus()
|
||||
{
|
||||
if (_accel_calibration.device_id() == _gyro_calibration.device_id()) {
|
||||
PX4_INFO("%d - IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _vehicle_imu_pub.get_instance(),
|
||||
_accel_calibration.device_id(),
|
||||
(double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
|
||||
|
||||
} else {
|
||||
PX4_INFO("%d - Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _vehicle_imu_pub.get_instance(),
|
||||
_accel_calibration.device_id(), (double)_accel_interval.update_interval, _gyro_calibration.device_id(),
|
||||
(double)_gyro_interval.update_interval);
|
||||
}
|
||||
|
||||
perf_print_counter(_accel_generation_gap_perf);
|
||||
perf_print_counter(_gyro_generation_gap_perf);
|
||||
perf_print_counter(_accel_update_perf);
|
||||
perf_print_counter(_gyro_update_perf);
|
||||
|
||||
_accel_calibration.PrintStatus();
|
||||
_gyro_calibration.PrintStatus();
|
||||
}
|
||||
|
||||
} // namespace sensors
|
|
@ -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 ×tamp_sample, uint8_t samples);
|
||||
void UpdateIntegratorConfiguration();
|
||||
void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
|
||||
void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);
|
||||
|
||||
uORB::PublicationMulti<vehicle_imu_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
|
Loading…
Reference in New Issue