diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 17c898e094..d657edf63b 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -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(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; diff --git a/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp index 8cc8cf6f21..807999d936 100644 --- a/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp @@ -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(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; diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp index 6c58bce735..7db5315d6a 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp @@ -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(); diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp index e3c91d7674..937cf784a2 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp @@ -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")}; diff --git a/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp b/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp index b6c32c0a45..25705b39cc 100644 --- a/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp +++ b/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp @@ -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 diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index ec3246565e..6002b84dac 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -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) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index f4e036ea98..574a422c65 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -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_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}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index b52f7b0895..2eaaaee0fd 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -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) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index e1b28e9ada..1d73466785 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -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_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};