mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
PX4Accelerometer/PX4Gyroscope update FIFO case to trapezoidal integration
This commit is contained in:
Daniel Agar
parent
697dbfb9f8
commit
009ba638f5
@ -300,6 +300,11 @@ void ICM20602::Run()
|
||||
{
|
||||
perf_count(_interval_perf);
|
||||
|
||||
// use timestamp from the data ready interrupt if available,
|
||||
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
|
||||
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
|
||||
hrt_absolute_time();
|
||||
|
||||
// read FIFO count
|
||||
uint8_t fifo_count_buf[3] {};
|
||||
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
|
||||
@ -351,15 +356,11 @@ void ICM20602::Run()
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
|
||||
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
|
||||
const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt);
|
||||
|
||||
PX4Accelerometer::FIFOSample accel{};
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
|
||||
|
||||
PX4Gyroscope::FIFOSample gyro{};
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
|
||||
|
||||
@ -276,6 +276,11 @@ void ICM20608G::Run()
|
||||
{
|
||||
perf_count(_interval_perf);
|
||||
|
||||
// use timestamp from the data ready interrupt if available,
|
||||
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
|
||||
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
|
||||
hrt_absolute_time();
|
||||
|
||||
// read FIFO count
|
||||
uint8_t fifo_count_buf[3] {};
|
||||
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
|
||||
@ -327,15 +332,11 @@ void ICM20608G::Run()
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
|
||||
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
|
||||
const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt);
|
||||
|
||||
PX4Accelerometer::FIFOSample accel{};
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
|
||||
|
||||
PX4Gyroscope::FIFOSample gyro{};
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
|
||||
|
||||
@ -40,6 +40,8 @@ using namespace ST_ISM330DLC;
|
||||
|
||||
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
|
||||
|
||||
static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval
|
||||
|
||||
ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
|
||||
SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
|
||||
@ -53,8 +55,8 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
|
||||
_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);
|
||||
_px4_gyro.set_sample_rate(ST_ISM330DLC::G_ODR);
|
||||
|
||||
_px4_accel.set_update_rate(1000000 / _fifo_interval);
|
||||
_px4_gyro.set_update_rate(1000000 / _fifo_interval);
|
||||
_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
|
||||
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
|
||||
}
|
||||
|
||||
ISM330DLC::~ISM330DLC()
|
||||
@ -227,8 +229,7 @@ void ISM330DLC::Start()
|
||||
RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR |
|
||||
INT1_CTRL_BIT::INT1_FTH);
|
||||
#else
|
||||
|
||||
ScheduleOnInterval(_fifo_interval, _fifo_interval);
|
||||
ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -248,8 +249,12 @@ void ISM330DLC::Run()
|
||||
{
|
||||
perf_count(_interval_perf);
|
||||
|
||||
// use timestamp from the data ready interrupt if available,
|
||||
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
|
||||
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
|
||||
hrt_absolute_time();
|
||||
|
||||
// Number of unread words (16-bit axes) stored in FIFO.
|
||||
const hrt_abstime timestamp_fifo_level = hrt_absolute_time();
|
||||
const uint8_t fifo_words = RegisterRead(Register::FIFO_STATUS1);
|
||||
|
||||
// check for FIFO status
|
||||
@ -311,19 +316,15 @@ void ISM330DLC::Run()
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
static constexpr uint32_t gyro_dt = 1000000 / ST_ISM330DLC::G_ODR;
|
||||
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
|
||||
const hrt_abstime timestamp_sample = timestamp_fifo_level - ((samples - 1) * gyro_dt);
|
||||
|
||||
PX4Accelerometer::FIFOSample accel{};
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = samples;
|
||||
accel.dt = gyro_dt;
|
||||
accel.dt = 1000000 / ST_ISM330DLC::LA_ODR;
|
||||
|
||||
PX4Gyroscope::FIFOSample gyro{};
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
gyro.dt = gyro_dt;
|
||||
gyro.dt = 1000000 / ST_ISM330DLC::G_ODR;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
@ -366,6 +367,8 @@ void ISM330DLC::PrintInfo()
|
||||
perf_print_counter(_fifo_empty_perf);
|
||||
perf_print_counter(_fifo_overflow_perf);
|
||||
perf_print_counter(_fifo_reset_perf);
|
||||
perf_print_counter(_drdy_count_perf);
|
||||
perf_print_counter(_drdy_interval_perf);
|
||||
|
||||
_px4_accel.print_status();
|
||||
_px4_gyro.print_status();
|
||||
|
||||
@ -84,8 +84,6 @@ private:
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
static constexpr uint32_t _fifo_interval{1000}; // 1000 us sample interval
|
||||
|
||||
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")};
|
||||
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};
|
||||
|
||||
@ -53,7 +53,7 @@ static constexpr uint8_t Bit7 = (1 << 7);
|
||||
namespace ST_ISM330DLC
|
||||
{
|
||||
|
||||
static constexpr uint8_t DIR_READ = 0x80;
|
||||
static constexpr uint8_t DIR_READ = 0x80;
|
||||
|
||||
static constexpr uint8_t ISM330DLC_WHO_AM_I = 0b01101010; // Who I am ID
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-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
|
||||
@ -39,6 +39,30 @@
|
||||
using namespace time_literals;
|
||||
using matrix::Vector3f;
|
||||
|
||||
static inline 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 inline unsigned 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, uint8_t priority, enum Rotation rotation) :
|
||||
CDev(nullptr),
|
||||
ModuleParams(nullptr),
|
||||
@ -47,7 +71,8 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
|
||||
_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
|
||||
_sensor_status_pub{ORB_ID(sensor_accel_status), priority},
|
||||
_device_id{device_id},
|
||||
_rotation{rotation}
|
||||
_rotation{rotation},
|
||||
_rotation_dcm{get_rot_matrix(rotation)}
|
||||
{
|
||||
_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
|
||||
|
||||
@ -119,10 +144,8 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
|
||||
const Vector3f raw{x, y, z};
|
||||
|
||||
// Clipping (check unscaled raw values)
|
||||
const float clip_limit = (_range / _scale) * 0.95f;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (fabsf(raw(i)) > clip_limit) {
|
||||
if (fabsf(raw(i)) > _clip_limit) {
|
||||
_clipping[i]++;
|
||||
_integrator_clipping++;
|
||||
}
|
||||
@ -139,10 +162,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
|
||||
Vector3f delta_velocity;
|
||||
uint32_t integral_dt = 0;
|
||||
|
||||
if (_integrator_samples == 0) {
|
||||
_integrator_timestamp_sample = timestamp_sample;
|
||||
}
|
||||
|
||||
_integrator_samples++;
|
||||
|
||||
if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
|
||||
@ -165,7 +184,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
|
||||
// fill sensor_accel_integrated and publish
|
||||
sensor_accel_integrated_s report{};
|
||||
|
||||
report.timestamp_sample = _integrator_timestamp_sample;
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.error_count = _error_count;
|
||||
report.device_id = _device_id;
|
||||
delta_velocity.copyTo(report.delta_velocity);
|
||||
@ -189,10 +208,13 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
|
||||
|
||||
void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
{
|
||||
const uint8_t N = sample.samples;
|
||||
const float dt = sample.dt;
|
||||
|
||||
// filtered data (control)
|
||||
float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
|
||||
float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
|
||||
float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
|
||||
float x_filtered = _filterArrayX.apply(sample.x, N);
|
||||
float y_filtered = _filterArrayY.apply(sample.y, N);
|
||||
float z_filtered = _filterArrayZ.apply(sample.z, N);
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
|
||||
@ -200,65 +222,41 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
const Vector3f raw{x_filtered, y_filtered, z_filtered};
|
||||
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
|
||||
const Vector3f val_calibrated{((raw * _scale) - _calibration_offset).emult(_calibration_scale)};
|
||||
|
||||
|
||||
// clipping
|
||||
const int16_t clip_limit = (_range / _scale) * 0.95f;
|
||||
unsigned clip_count_x = clipping(sample.x, _clip_limit, N);
|
||||
unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
|
||||
unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
|
||||
|
||||
// x clipping
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
if (abs(sample.x[n]) > clip_limit) {
|
||||
_clipping[0]++;
|
||||
_integrator_clipping++;
|
||||
}
|
||||
}
|
||||
|
||||
// y clipping
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
if (abs(sample.y[n]) > clip_limit) {
|
||||
_clipping[1]++;
|
||||
_integrator_clipping++;
|
||||
}
|
||||
}
|
||||
|
||||
// z clipping
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
if (abs(sample.z[n]) > clip_limit) {
|
||||
_clipping[2]++;
|
||||
_integrator_clipping++;
|
||||
}
|
||||
}
|
||||
_clipping[0] += clip_count_x;
|
||||
_clipping[1] += clip_count_y;
|
||||
_clipping[2] += clip_count_z;
|
||||
|
||||
_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
|
||||
|
||||
// integrated data (INS)
|
||||
{
|
||||
// reset integrator if previous sample was too long ago
|
||||
if ((sample.timestamp_sample > _timestamp_sample_prev)
|
||||
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
|
||||
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
|
||||
|
||||
ResetIntegrator();
|
||||
}
|
||||
|
||||
if (_integrator_samples == 0) {
|
||||
_integrator_timestamp_sample = sample.timestamp_sample;
|
||||
}
|
||||
|
||||
// integrate
|
||||
_integrator_samples += 1;
|
||||
_integrator_fifo_samples += sample.samples;
|
||||
_integrator_fifo_samples += N;
|
||||
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
_integrator_accum[0] += sample.x[n];
|
||||
}
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
|
||||
_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
|
||||
_integration_raw(2) += (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];
|
||||
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
_integrator_accum[1] += sample.y[n];
|
||||
}
|
||||
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
_integrator_accum[2] += sample.z[n];
|
||||
}
|
||||
|
||||
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
|
||||
|
||||
@ -266,7 +264,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
{
|
||||
sensor_accel_s report{};
|
||||
|
||||
report.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.x = val_calibrated(0);
|
||||
@ -277,31 +275,25 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
// Apply rotation and scale
|
||||
// integrated in microseconds, convert to seconds
|
||||
const Vector3f delta_velocity_uncalibrated{_rotation_dcm *_integration_raw * _scale};
|
||||
|
||||
const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
|
||||
// scale calibration offset to number of samples
|
||||
const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
|
||||
|
||||
// average integrated values to apply calibration
|
||||
float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
|
||||
float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
|
||||
float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
|
||||
|
||||
const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
|
||||
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
Vector3f delta_velocity{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))};
|
||||
delta_velocity *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
|
||||
// Apply calibration and scale to seconds
|
||||
Vector3f delta_velocity{((delta_velocity_uncalibrated - offset).emult(_calibration_scale))};
|
||||
delta_velocity *= 1e-6f * dt;
|
||||
|
||||
// fill sensor_accel_integrated and publish
|
||||
sensor_accel_integrated_s report{};
|
||||
|
||||
report.timestamp_sample = _integrator_timestamp_sample;
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.error_count = _error_count;
|
||||
report.device_id = _device_id;
|
||||
delta_velocity.copyTo(report.delta_velocity);
|
||||
report.dt = integrator_dt_us;
|
||||
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
|
||||
report.samples = _integrator_fifo_samples;
|
||||
report.clip_count = _integrator_clipping;
|
||||
|
||||
@ -323,13 +315,13 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
|
||||
|
||||
fifo.device_id = _device_id;
|
||||
fifo.timestamp_sample = sample.timestamp_sample;
|
||||
fifo.dt = sample.dt;
|
||||
fifo.dt = dt;
|
||||
fifo.scale = _scale;
|
||||
fifo.samples = sample.samples;
|
||||
fifo.samples = N;
|
||||
|
||||
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
|
||||
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
|
||||
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
|
||||
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
|
||||
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
|
||||
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N);
|
||||
|
||||
fifo.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(fifo);
|
||||
@ -366,12 +358,9 @@ void PX4Accelerometer::ResetIntegrator()
|
||||
{
|
||||
_integrator_samples = 0;
|
||||
_integrator_fifo_samples = 0;
|
||||
_integrator_accum[0] = 0;
|
||||
_integrator_accum[1] = 0;
|
||||
_integrator_accum[2] = 0;
|
||||
_integration_raw.zero();
|
||||
_integrator_clipping = 0;
|
||||
|
||||
_integrator_timestamp_sample = 0;
|
||||
_timestamp_sample_prev = 0;
|
||||
}
|
||||
|
||||
@ -384,6 +373,12 @@ void PX4Accelerometer::ConfigureFilter(float cutoff_freq)
|
||||
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
|
||||
}
|
||||
|
||||
void PX4Accelerometer::UpdateClipLimit()
|
||||
{
|
||||
// 95% of potential max
|
||||
_clip_limit = (_range / _scale) * 0.95f;
|
||||
}
|
||||
|
||||
void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
|
||||
{
|
||||
// Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
||||
|
||||
@ -61,9 +61,9 @@ public:
|
||||
void set_device_id(uint32_t device_id) { _device_id = device_id; }
|
||||
void set_device_type(uint8_t devtype);
|
||||
void set_error_count(uint64_t error_count) { _error_count += error_count; }
|
||||
void set_range(float range) { _range = range; }
|
||||
void set_range(float range) { _range = range; UpdateClipLimit(); }
|
||||
void set_sample_rate(uint16_t rate);
|
||||
void set_scale(float scale) { _scale = scale; }
|
||||
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
|
||||
void set_temperature(float temperature) { _temperature = temperature; }
|
||||
void set_update_rate(uint16_t rate);
|
||||
|
||||
@ -91,6 +91,7 @@ private:
|
||||
void ConfigureFilter(float cutoff_freq);
|
||||
void PublishStatus();
|
||||
void ResetIntegrator();
|
||||
void UpdateClipLimit();
|
||||
void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);
|
||||
|
||||
uORB::PublicationMulti<sensor_accel_s> _sensor_pub;
|
||||
@ -100,28 +101,31 @@ private:
|
||||
|
||||
math::LowPassFilter2pVector3f _filter{1000, 100};
|
||||
|
||||
hrt_abstime _status_last_publish{0};
|
||||
|
||||
math::LowPassFilter2pArray _filterArrayX{4000, 100};
|
||||
math::LowPassFilter2pArray _filterArrayY{4000, 100};
|
||||
math::LowPassFilter2pArray _filterArrayZ{4000, 100};
|
||||
|
||||
hrt_abstime _status_last_publish{0};
|
||||
|
||||
Integrator _integrator{4000, false};
|
||||
|
||||
matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f};
|
||||
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
|
||||
matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f};
|
||||
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
|
||||
|
||||
matrix::Vector3f _delta_velocity_prev{0.0f, 0.0f, 0.0f}; // delta velocity from the previous IMU measurement
|
||||
float _vibration_metric{0.0f}; // high frequency vibration level in the IMU delta velocity data (m/s)
|
||||
matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement
|
||||
float _vibration_metric{0.f}; // high frequency vibration level in the IMU delta velocity data (m/s)
|
||||
|
||||
int _class_device_instance{-1};
|
||||
|
||||
uint32_t _device_id{0};
|
||||
const enum Rotation _rotation;
|
||||
const matrix::Dcmf _rotation_dcm;
|
||||
|
||||
float _range{16.0f * CONSTANTS_ONE_G};
|
||||
float _scale{1.0f};
|
||||
float _temperature{0.0f};
|
||||
float _range{16 * CONSTANTS_ONE_G};
|
||||
float _scale{1.f};
|
||||
float _temperature{0.f};
|
||||
|
||||
int16_t _clip_limit{(int16_t)(_range / _scale)};
|
||||
|
||||
uint64_t _error_count{0};
|
||||
|
||||
@ -131,9 +135,9 @@ private:
|
||||
uint16_t _update_rate{1000};
|
||||
|
||||
// integrator
|
||||
hrt_abstime _integrator_timestamp_sample{0};
|
||||
hrt_abstime _timestamp_sample_prev{0};
|
||||
float _integrator_accum[3] {};
|
||||
matrix::Vector3f _integration_raw{};
|
||||
int16_t _last_sample[3] {};
|
||||
uint8_t _integrator_reset_samples{4};
|
||||
uint8_t _integrator_samples{0};
|
||||
uint8_t _integrator_fifo_samples{0};
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-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
|
||||
@ -39,6 +39,30 @@
|
||||
using namespace time_literals;
|
||||
using matrix::Vector3f;
|
||||
|
||||
static inline 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 inline unsigned 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;
|
||||
}
|
||||
|
||||
PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
|
||||
CDev(nullptr),
|
||||
ModuleParams(nullptr),
|
||||
@ -47,7 +71,8 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
|
||||
_sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority},
|
||||
_sensor_status_pub{ORB_ID(sensor_gyro_status), priority},
|
||||
_device_id{device_id},
|
||||
_rotation{rotation}
|
||||
_rotation{rotation},
|
||||
_rotation_dcm{get_rot_matrix(rotation)}
|
||||
{
|
||||
_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
|
||||
|
||||
@ -120,10 +145,8 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
|
||||
const Vector3f raw{x, y, z};
|
||||
|
||||
// Clipping (check unscaled raw values)
|
||||
const float clip_limit = (_range / _scale) * 0.95f;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (fabsf(raw(i)) > clip_limit) {
|
||||
if (fabsf(raw(i)) > _clip_limit) {
|
||||
_clipping[i]++;
|
||||
_integrator_clipping++;
|
||||
}
|
||||
@ -156,10 +179,6 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
|
||||
Vector3f delta_angle;
|
||||
uint32_t integral_dt = 0;
|
||||
|
||||
if (_integrator_samples == 0) {
|
||||
_integrator_timestamp_sample = timestamp_sample;
|
||||
}
|
||||
|
||||
_integrator_samples++;
|
||||
|
||||
if (_integrator.put(timestamp_sample, val_calibrated, delta_angle, integral_dt)) {
|
||||
@ -167,7 +186,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
|
||||
// fill sensor_gyro_integrated and publish
|
||||
sensor_gyro_integrated_s report{};
|
||||
|
||||
report.timestamp_sample = _integrator_timestamp_sample;
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.error_count = _error_count;
|
||||
report.device_id = _device_id;
|
||||
delta_angle.copyTo(report.delta_angle);
|
||||
@ -191,18 +210,19 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
|
||||
|
||||
void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
{
|
||||
const uint8_t N = sample.samples;
|
||||
const float dt = sample.dt;
|
||||
|
||||
// filtered data (control)
|
||||
float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
|
||||
float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
|
||||
float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
|
||||
float x_filtered = _filterArrayX.apply(sample.x, N);
|
||||
float y_filtered = _filterArrayY.apply(sample.y, N);
|
||||
float z_filtered = _filterArrayZ.apply(sample.z, N);
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
|
||||
|
||||
const Vector3f raw{x_filtered, y_filtered, z_filtered};
|
||||
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
const Vector3f val_calibrated{(raw * _scale) - _calibration_offset};
|
||||
// Apply range scale and the calibration offset
|
||||
const Vector3f val_calibrated{(Vector3f{x_filtered, y_filtered, z_filtered} * _scale) - _calibration_offset};
|
||||
|
||||
|
||||
// publish control data (filtered) immediately
|
||||
@ -220,7 +240,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
if (publish_control) {
|
||||
sensor_gyro_s report{};
|
||||
|
||||
report.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.x = val_calibrated(0);
|
||||
@ -236,88 +256,59 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
|
||||
|
||||
// clipping
|
||||
const int16_t clip_limit = (_range / _scale) * 0.95f;
|
||||
unsigned clip_count_x = clipping(sample.x, _clip_limit, N);
|
||||
unsigned clip_count_y = clipping(sample.y, _clip_limit, N);
|
||||
unsigned clip_count_z = clipping(sample.z, _clip_limit, N);
|
||||
|
||||
// x clipping
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
if (abs(sample.x[n]) > clip_limit) {
|
||||
_clipping[0]++;
|
||||
_integrator_clipping++;
|
||||
}
|
||||
}
|
||||
|
||||
// y clipping
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
if (abs(sample.y[n]) > clip_limit) {
|
||||
_clipping[1]++;
|
||||
_integrator_clipping++;
|
||||
}
|
||||
}
|
||||
|
||||
// z clipping
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
if (abs(sample.z[n]) > clip_limit) {
|
||||
_clipping[2]++;
|
||||
_integrator_clipping++;
|
||||
}
|
||||
}
|
||||
_clipping[0] += clip_count_x;
|
||||
_clipping[1] += clip_count_y;
|
||||
_clipping[2] += clip_count_z;
|
||||
|
||||
_integrator_clipping += clip_count_x + clip_count_y + clip_count_z;
|
||||
|
||||
// integrated data (INS)
|
||||
{
|
||||
// reset integrator if previous sample was too long ago
|
||||
if ((sample.timestamp_sample > _timestamp_sample_prev)
|
||||
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
|
||||
&& ((sample.timestamp_sample - _timestamp_sample_prev) > (N * dt * 2.0f))) {
|
||||
|
||||
ResetIntegrator();
|
||||
}
|
||||
|
||||
if (_integrator_samples == 0) {
|
||||
_integrator_timestamp_sample = sample.timestamp_sample;
|
||||
}
|
||||
|
||||
// integrate
|
||||
_integrator_samples += 1;
|
||||
_integrator_fifo_samples += sample.samples;
|
||||
_integrator_fifo_samples += N;
|
||||
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
_integrator_accum[0] += sample.x[n];
|
||||
}
|
||||
// trapezoidal integration (equally spaced, scaled by dt later)
|
||||
_integration_raw(0) += (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1));
|
||||
_integration_raw(1) += (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1));
|
||||
_integration_raw(2) += (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];
|
||||
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
_integrator_accum[1] += sample.y[n];
|
||||
}
|
||||
|
||||
for (int n = 0; n < sample.samples; n++) {
|
||||
_integrator_accum[2] += sample.z[n];
|
||||
}
|
||||
|
||||
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
|
||||
|
||||
const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
|
||||
// Apply rotation and scale
|
||||
// integrated in microseconds, convert to seconds
|
||||
const Vector3f delta_angle_uncalibrated{_rotation_dcm *_integration_raw * _scale};
|
||||
|
||||
// average integrated values to apply calibration
|
||||
float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
|
||||
float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
|
||||
float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
|
||||
// scale calibration offset to number of samples
|
||||
const Vector3f offset{_calibration_offset * _integrator_fifo_samples};
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
|
||||
|
||||
const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
|
||||
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
Vector3f delta_angle{(raw_int * _scale) - _calibration_offset};
|
||||
delta_angle *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore
|
||||
// Apply calibration and scale to seconds
|
||||
Vector3f delta_angle{delta_angle_uncalibrated - offset};
|
||||
delta_angle *= 1e-6f * dt;
|
||||
|
||||
// fill sensor_gyro_integrated and publish
|
||||
sensor_gyro_integrated_s report{};
|
||||
|
||||
report.timestamp_sample = _integrator_timestamp_sample;
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.error_count = _error_count;
|
||||
report.device_id = _device_id;
|
||||
delta_angle.copyTo(report.delta_angle);
|
||||
report.dt = integrator_dt_us;
|
||||
report.dt = _integrator_fifo_samples * dt; // time span in microseconds
|
||||
report.samples = _integrator_fifo_samples;
|
||||
report.clip_count = _integrator_clipping;
|
||||
|
||||
@ -339,13 +330,13 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
|
||||
fifo.device_id = _device_id;
|
||||
fifo.timestamp_sample = sample.timestamp_sample;
|
||||
fifo.dt = sample.dt;
|
||||
fifo.dt = dt;
|
||||
fifo.scale = _scale;
|
||||
fifo.samples = sample.samples;
|
||||
fifo.samples = N;
|
||||
|
||||
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
|
||||
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
|
||||
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
|
||||
memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
|
||||
memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
|
||||
memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N);
|
||||
|
||||
fifo.timestamp = hrt_absolute_time();
|
||||
_sensor_fifo_pub.publish(fifo);
|
||||
@ -383,12 +374,9 @@ void PX4Gyroscope::ResetIntegrator()
|
||||
{
|
||||
_integrator_samples = 0;
|
||||
_integrator_fifo_samples = 0;
|
||||
_integrator_accum[0] = 0;
|
||||
_integrator_accum[1] = 0;
|
||||
_integrator_accum[2] = 0;
|
||||
_integration_raw.zero();
|
||||
_integrator_clipping = 0;
|
||||
|
||||
_integrator_timestamp_sample = 0;
|
||||
_timestamp_sample_prev = 0;
|
||||
}
|
||||
|
||||
@ -406,6 +394,13 @@ void PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
|
||||
_notch_filter.setParameters(_sample_rate, notch_freq, bandwidth);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::UpdateClipLimit()
|
||||
{
|
||||
// 95% of potential max
|
||||
_clip_limit = (_range / _scale) * 0.95f;
|
||||
}
|
||||
|
||||
|
||||
void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
|
||||
{
|
||||
// Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
||||
|
||||
@ -61,9 +61,9 @@ public:
|
||||
void set_device_id(uint32_t device_id) { _device_id = device_id; }
|
||||
void set_device_type(uint8_t devtype);
|
||||
void set_error_count(uint64_t error_count) { _error_count += error_count; }
|
||||
void set_range(float range) { _range = range; }
|
||||
void set_range(float range) { _range = range; UpdateClipLimit(); }
|
||||
void set_sample_rate(uint16_t rate);
|
||||
void set_scale(float scale) { _scale = scale; }
|
||||
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
|
||||
void set_temperature(float temperature) { _temperature = temperature; }
|
||||
void set_update_rate(uint16_t rate);
|
||||
|
||||
@ -92,6 +92,7 @@ private:
|
||||
void ConfigureNotchFilter(float notch_freq, float bandwidth);
|
||||
void PublishStatus();
|
||||
void ResetIntegrator();
|
||||
void UpdateClipLimit();
|
||||
void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
|
||||
|
||||
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub;
|
||||
@ -111,20 +112,23 @@ private:
|
||||
|
||||
Integrator _integrator{4000, true};
|
||||
|
||||
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
|
||||
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
|
||||
|
||||
matrix::Vector3f _delta_angle_prev{0.0f, 0.0f, 0.0f}; // delta angle from the previous IMU measurement
|
||||
float _vibration_metric{0.0f}; // high frequency vibration level in the IMU delta angle data (rad)
|
||||
float _coning_vibration{0.0f}; // Level of coning vibration in the IMU delta angles (rad^2)
|
||||
matrix::Vector3f _delta_angle_prev{0.f, 0.f, 0.f}; // delta angle from the previous IMU measurement
|
||||
float _vibration_metric{0.f}; // high frequency vibration level in the IMU delta angle data (rad)
|
||||
float _coning_vibration{0.f}; // Level of coning vibration in the IMU delta angles (rad^2)
|
||||
|
||||
int _class_device_instance{-1};
|
||||
|
||||
uint32_t _device_id{0};
|
||||
const enum Rotation _rotation;
|
||||
const matrix::Dcmf _rotation_dcm;
|
||||
|
||||
float _range{math::radians(2000.0f)};
|
||||
float _scale{1.0f};
|
||||
float _temperature{0.0f};
|
||||
float _range{math::radians(2000.f)};
|
||||
float _scale{1.f};
|
||||
float _temperature{0.f};
|
||||
|
||||
int16_t _clip_limit{(int16_t)(_range / _scale)};
|
||||
|
||||
uint64_t _error_count{0};
|
||||
|
||||
@ -134,9 +138,9 @@ private:
|
||||
uint16_t _update_rate{1000};
|
||||
|
||||
// integrator
|
||||
hrt_abstime _integrator_timestamp_sample{0};
|
||||
hrt_abstime _timestamp_sample_prev{0};
|
||||
float _integrator_accum[3] {};
|
||||
matrix::Vector3f _integration_raw{};
|
||||
int16_t _last_sample[3] {};
|
||||
uint8_t _integrator_reset_samples{4};
|
||||
uint8_t _integrator_samples{0};
|
||||
uint8_t _integrator_fifo_samples{0};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user