From 2410b3166252ec53f3a1c59c54b9b97a704580a5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 29 Jan 2020 16:13:38 -0500 Subject: [PATCH] sensors: move accel filtering to sensors/vehicle_acceleration I've added a queue depth of 4 for sensor_accel and sensor_gyro. This is initially added because it's not always possible for the `vehicle_acceleration` to keep up with every publication of the primary accelerator as it runs in the same thread as ekf2, various controllers, etc. Later this mechanism will be used in a few areas - rate limit `vehicle_angular_velocity` and `vehicle_acceleration` without missing any raw data - move IMU integration to `vehicle_imu` and out of the actual driver threads, eliminating the need for sensor_accel_integrated and sensor_gyro_integrated - integrate raw gyro synchronized with optical flow measurements --- msg/sensor_accel.msg | 2 + msg/sensor_accel_status.msg | 1 - msg/sensor_gyro.msg | 2 + src/drivers/imu/adis16448/ADIS16448.cpp | 2 - src/drivers/imu/adis16477/ADIS16477.cpp | 1 - src/drivers/imu/adis16497/ADIS16497.cpp | 1 - src/drivers/imu/bmi160/bmi160.cpp | 3 - .../imu/invensense/icm20602/ICM20602.cpp | 2 - .../imu/invensense/icm20608-g/ICM20608G.cpp | 2 - src/drivers/imu/lsm303d/LSM303D.cpp | 1 - src/drivers/imu/st/ism330dlc/ISM330DLC.cpp | 2 - src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp | 2 - .../accelerometer/PX4Accelerometer.cpp | 100 ++++++----------- .../accelerometer/PX4Accelerometer.hpp | 20 +--- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 3 +- src/modules/sensors/sensor_params.c | 14 --- .../VehicleAcceleration.cpp | 105 +++++++++++++++--- .../VehicleAcceleration.hpp | 20 +++- .../imu_accel_parameters.c | 46 ++++++++ .../VehicleAngularVelocity.cpp | 57 +++++----- .../VehicleAngularVelocity.hpp | 3 +- .../imu_gyro_parameters.c | 12 +- src/modules/simulator/simulator.h | 1 - src/modules/uORB/CMakeLists.txt | 1 + src/modules/uORB/PublicationQueuedMulti.hpp | 101 +++++++++++++++++ 25 files changed, 336 insertions(+), 168 deletions(-) create mode 100644 src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c create mode 100644 src/modules/uORB/PublicationQueuedMulti.hpp diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index 98ad1e9c9a..67f99870a2 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -8,3 +8,5 @@ float32 y # acceleration in the NED Y board axis in m/s^2 float32 z # acceleration in the NED Z board axis in m/s^2 float32 temperature # temperature in degrees celsius + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg index 3e94f1c659..ee6f292a08 100644 --- a/msg/sensor_accel_status.msg +++ b/msg/sensor_accel_status.msg @@ -11,7 +11,6 @@ uint8 rotation uint32[3] clipping # clipping per axis uint16 measure_rate -uint16 sample_rate float32 full_scale_range diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index ce6079fcdf..c3fd3207dd 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -8,3 +8,5 @@ float32 y # angular velocity in the NED Y board axis in rad/s float32 z # angular velocity in the NED Z board axis in rad/s float32 temperature # temperature in degrees celsius + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp index ab33410da3..0467fcf9b2 100644 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/adis16448/ADIS16448.cpp @@ -258,8 +258,6 @@ ADIS16448::set_sample_rate(uint16_t desired_sample_rate_hz) return false; } - _px4_accel.set_sample_rate(desired_sample_rate_hz); - return true; } diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 8f169a97da..686d7768e2 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -67,7 +67,6 @@ ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) : #endif // GPIO_SPI1_RESET_ADIS16477 _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16477); - _px4_accel.set_sample_rate(ADIS16477_DEFAULT_RATE); _px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16477); diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index 35db887007..bf089bb9f8 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/src/drivers/imu/adis16497/ADIS16497.cpp @@ -84,7 +84,6 @@ ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) : #endif // GPIO_SPI1_RESET_ADIS16497 _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16497); - _px4_accel.set_sample_rate(ADIS16497_DEFAULT_RATE); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497); } diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 6095d7dc1b..b702010b19 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -64,10 +64,7 @@ BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) : _duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates")) { _px4_accel.set_device_type(DRV_ACC_DEVTYPE_BMI160); - _px4_accel.set_sample_rate(BMI160_ACCEL_DEFAULT_RATE); - _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_BMI160); - _px4_accel.set_sample_rate(BMI160_GYRO_DEFAULT_RATE); } BMI160::~BMI160() diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 781272a748..b5022d8fd3 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -58,8 +58,6 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602); - _px4_accel.set_sample_rate(ACCEL_RATE); - _px4_accel.set_update_rate(1000000 / FIFO_INTERVAL); _px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL); } diff --git a/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp index 22ca1a86b9..00560f7f70 100644 --- a/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp @@ -58,8 +58,6 @@ ICM20608G::ICM20608G(int bus, uint32_t device, enum Rotation rotation) : _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20608); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20608); - _px4_accel.set_sample_rate(ACCEL_RATE); - _px4_accel.set_update_rate(1000000 / FIFO_INTERVAL); _px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL); } diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index 659242d280..7ee2bd85ae 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -363,7 +363,6 @@ LSM303D::accel_set_samplerate(unsigned frequency) } _call_accel_interval = 1000000 / accel_samplerate; - _px4_accel.set_sample_rate(accel_samplerate); modify_reg(ADDR_CTRL_REG1, clearbits, setbits); diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp index 3ef4074d32..62cef7de94 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp @@ -52,8 +52,6 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) : _px4_accel.set_device_type(DRV_DEVTYPE_ST_ISM330DLC); _px4_gyro.set_device_type(DRV_DEVTYPE_ST_ISM330DLC); - _px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR); - _px4_accel.set_update_rate(1000000 / FIFO_INTERVAL); _px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL); } diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index 155631bd54..869caba079 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -48,8 +48,6 @@ LSM9DS1::LSM9DS1(int bus, uint32_t device, enum Rotation rotation) : _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG); _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG); - _px4_accel.set_sample_rate(ST_LSM9DS1::LA_ODR); - _px4_accel.set_update_rate(1000000 / _fifo_interval); _px4_gyro.set_update_rate(1000000 / _fifo_interval); } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 6002b84dac..05de4474d4 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -65,7 +65,6 @@ static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, u PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) : CDev(nullptr), - ModuleParams(nullptr), _sensor_pub{ORB_ID(sensor_accel), priority}, _sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority}, _sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority}, @@ -75,10 +74,6 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro _rotation_dcm{get_rot_matrix(rotation)} { _class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - - // set software low pass filter for controllers - updateParams(); - ConfigureFilter(_param_imu_accel_cutoff.get()); } PX4Accelerometer::~PX4Accelerometer() @@ -123,13 +118,6 @@ void PX4Accelerometer::set_device_type(uint8_t devtype) _device_id = device_id.devid; } -void PX4Accelerometer::set_sample_rate(uint16_t rate) -{ - _sample_rate = rate; - - ConfigureFilter(_filter.get_cutoff_freq()); -} - void PX4Accelerometer::set_update_rate(uint16_t rate) { const uint32_t update_interval = 1000000 / rate; @@ -154,9 +142,20 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl // Apply range scale and the calibrating offset/scale const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))}; - // Filtered values - const Vector3f val_filtered{_filter.apply(val_calibrated)}; + // publish raw data immediately + { + sensor_accel_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = _device_id; + report.temperature = _temperature; + report.x = val_calibrated(0); + report.y = val_calibrated(1); + report.z = val_calibrated(2); + report.timestamp = hrt_absolute_time(); + + _sensor_pub.publish(report); + } // Integrated values Vector3f delta_velocity; @@ -166,21 +165,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) { - // publish control data (filtered) - { - sensor_accel_s report{}; - - report.timestamp_sample = timestamp_sample; - report.device_id = _device_id; - report.temperature = _temperature; - report.x = val_filtered(0); - report.y = val_filtered(1); - report.z = val_filtered(2); - report.timestamp = hrt_absolute_time(); - - _sensor_pub.publish(report); - } - // fill sensor_accel_integrated and publish sensor_accel_integrated_s report{}; @@ -211,18 +195,31 @@ 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, N); - float y_filtered = _filterArrayY.apply(sample.y, N); - float z_filtered = _filterArrayZ.apply(sample.z, N); + // publish raw data immediately + { + // average + float x = (float)sum(sample.x, N) / (float)N; + float y = (float)sum(sample.y, N) / (float)N; + float z = (float)sum(sample.z, N) / (float)N; - // Apply rotation (before scaling) - rotate_3f(_rotation, x_filtered, y_filtered, z_filtered); + // Apply rotation (before scaling) + rotate_3f(_rotation, x, y, z); - const Vector3f raw{x_filtered, y_filtered, z_filtered}; + // Apply range scale and the calibrating offset/scale + const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)}; - // Apply range scale and the calibrating offset/scale - const Vector3f val_calibrated{((raw * _scale) - _calibration_offset).emult(_calibration_scale)}; + sensor_accel_s report{}; + + report.timestamp_sample = sample.timestamp_sample; + report.device_id = _device_id; + report.temperature = _temperature; + report.x = val_calibrated(0); + report.y = val_calibrated(1); + report.z = val_calibrated(2); + report.timestamp = hrt_absolute_time(); + + _sensor_pub.publish(report); + } // clipping @@ -260,21 +257,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) { - // publish control data (filtered) - { - sensor_accel_s report{}; - - report.timestamp_sample = sample.timestamp_sample; - report.device_id = _device_id; - report.temperature = _temperature; - report.x = val_calibrated(0); - report.y = val_calibrated(1); - report.z = val_calibrated(2); - report.timestamp = hrt_absolute_time(); - - _sensor_pub.publish(report); - } - // Apply rotation and scale // integrated in microseconds, convert to seconds const Vector3f delta_velocity_uncalibrated{_rotation_dcm *_integration_raw * _scale}; @@ -341,7 +323,6 @@ void PX4Accelerometer::PublishStatus() status.full_scale_range = _range; status.rotation = _rotation; status.measure_rate = _update_rate; - status.sample_rate = _sample_rate; status.temperature = _temperature; status.vibration_metric = _vibration_metric; status.clipping[0] = _clipping[0]; @@ -364,15 +345,6 @@ void PX4Accelerometer::ResetIntegrator() _timestamp_sample_prev = 0; } -void PX4Accelerometer::ConfigureFilter(float cutoff_freq) -{ - _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); - - _filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq); - _filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq); - _filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq); -} - void PX4Accelerometer::UpdateClipLimit() { // 95% of potential max @@ -391,8 +363,6 @@ void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity) void PX4Accelerometer::print_status() { PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); - PX4_INFO("sample rate: %d Hz", _sample_rate); - PX4_INFO("filter cutoff: %.3f Hz", (double)_filter.get_cutoff_freq()); PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1), (double)_calibration_scale(2)); diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 574a422c65..ceb7ac9844 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -39,16 +39,14 @@ #include #include #include -#include -#include -#include #include +#include #include #include #include #include -class PX4Accelerometer : public cdev::CDev, public ModuleParams +class PX4Accelerometer : public cdev::CDev { public: PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); @@ -62,7 +60,6 @@ public: 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; UpdateClipLimit(); } - void set_sample_rate(uint16_t rate); void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } void set_temperature(float temperature) { _temperature = temperature; } void set_update_rate(uint16_t rate); @@ -88,25 +85,18 @@ public: private: - void ConfigureFilter(float cutoff_freq); void PublishStatus(); void ResetIntegrator(); void UpdateClipLimit(); void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity); - uORB::PublicationMulti _sensor_pub; + uORB::PublicationQueuedMulti _sensor_pub; uORB::PublicationMulti _sensor_fifo_pub; uORB::PublicationMulti _sensor_integrated_pub; uORB::PublicationMulti _sensor_status_pub; - 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}; - Integrator _integrator{4000, false}; matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f}; @@ -131,7 +121,6 @@ private: uint32_t _clipping[3] {}; - uint16_t _sample_rate{1000}; uint16_t _update_rate{1000}; // integrator @@ -143,7 +132,4 @@ private: uint8_t _integrator_fifo_samples{0}; uint8_t _integrator_clipping{0}; - DEFINE_PARAMETERS( - (ParamFloat) _param_imu_accel_cutoff - ) }; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 27762b5a06..36d0797614 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -88,7 +89,7 @@ private: void UpdateClipLimit(); void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle); - uORB::PublicationMulti _sensor_pub; + uORB::PublicationQueuedMulti _sensor_pub; uORB::PublicationMulti _sensor_fifo_pub; uORB::PublicationMulti _sensor_integrated_pub; uORB::PublicationMulti _sensor_status_pub; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c9a7eaf143..de08c78363 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -212,17 +212,3 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); * @group Sensors */ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1); - -/** -* Driver level cutoff frequency for accel -* -* The cutoff frequency for the 2nd order butterworth filter on the accel driver. -* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. -* -* @min 0 -* @max 1000 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f); diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 56d78c3afa..00c27a311d 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -43,11 +43,14 @@ VehicleAcceleration::VehicleAcceleration() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl) { + _lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get()); } VehicleAcceleration::~VehicleAcceleration() { Stop(); + + perf_free(_interval_perf); } bool VehicleAcceleration::Start() @@ -77,6 +80,45 @@ void VehicleAcceleration::Stop() _sensor_selection_sub.unregisterCallback(); } +void VehicleAcceleration::CheckFilters() +{ + if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) { + _filter_check_last = hrt_absolute_time(); + + // calculate sensor update rate + const float sample_interval_avg = perf_mean(_interval_perf); + + if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) { + + const float update_rate_hz = 1.0f / sample_interval_avg; + + if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) { + _update_rate_hz = update_rate_hz; + + // check if sample rate error is greater than 1% + if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { + ++_sample_rate_incorrect_count; + } + } + } + + const bool sample_rate_updated = (_sample_rate_incorrect_count > 50); + const bool lp_updated = (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f); + + if (sample_rate_updated || lp_updated) { + PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz); + _filter_sample_rate = _update_rate_hz; + + // update software low pass filters + _lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get()); + _lp_filter.reset(_acceleration_prev); + + // reset state + _sample_rate_incorrect_count = 0; + } + } +} + void VehicleAcceleration::SensorBiasUpdate(bool force) { if (_estimator_sensor_bias_sub.updated() || force) { @@ -165,6 +207,9 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force) // force corrections reselection _corrections_selected_instance = -1; + // reset sample rate monitor + _sample_rate_incorrect_count = 0; + return true; } } @@ -205,35 +250,56 @@ void VehicleAcceleration::ParametersUpdate(bool force) void VehicleAcceleration::Run() { // update corrections first to set _selected_sensor - bool sensor_select_update = SensorSelectionUpdate(); - SensorCorrectionsUpdate(sensor_select_update); - SensorBiasUpdate(sensor_select_update); + bool selection_updated = SensorSelectionUpdate(); + + SensorCorrectionsUpdate(selection_updated); + SensorBiasUpdate(selection_updated); ParametersUpdate(); - if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) { + bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated(); + + // process all outstanding messages + while (sensor_updated || selection_updated) { + selection_updated = false; + sensor_accel_s sensor_data; if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) { - // 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 offsets and scale - Vector3f accel{(val - _offset).emult(_scale)}; + if (sensor_updated) { + perf_count_interval(_interval_perf, sensor_data.timestamp_sample); + } - // rotate corrected measurements from sensor to body frame - accel = _board_rotation * accel; + CheckFilters(); - // correct for in-run bias errors - accel -= _bias; + // Filter: apply low-pass + const Vector3f accel_filtered = _lp_filter.apply(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}); - // publish - vehicle_acceleration_s out; + _acceleration_prev = accel_filtered; - out.timestamp_sample = sensor_data.timestamp_sample; - accel.copyTo(out.xyz); - out.timestamp = hrt_absolute_time(); + // publish once all new samples are processed + sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated(); - _vehicle_acceleration_pub.publish(out); + if (!sensor_updated) { + // apply offsets and scale + Vector3f accel{(accel_filtered - _offset).emult(_scale)}; + + // rotate corrected measurements from sensor to body frame + accel = _board_rotation * accel; + + // correct for in-run bias errors + accel -= _bias; + + // Publish vehicle_acceleration + vehicle_acceleration_s v_acceleration; + v_acceleration.timestamp_sample = sensor_data.timestamp_sample; + accel.copyTo(v_acceleration.xyz); + v_acceleration.timestamp = hrt_absolute_time(); + _vehicle_acceleration_pub.publish(v_acceleration); + + _last_publish = v_acceleration.timestamp_sample; + return; + } } } } @@ -244,4 +310,7 @@ void VehicleAcceleration::PrintStatus() PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2)); PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2)); PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2)); + + PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz); + PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter.get_cutoff_freq()); } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 394bb4b1a7..4575384cdb 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -65,6 +66,7 @@ public: private: void Run() override; + void CheckFilters(); void ParametersUpdate(bool force = false); void SensorBiasUpdate(bool force = false); void SensorCorrectionsUpdate(bool force = false); @@ -73,6 +75,8 @@ private: static constexpr int MAX_SENSOR_COUNT = 3; DEFINE_PARAMETERS( + (ParamFloat) _param_imu_accel_cutoff, + (ParamInt) _param_sens_board_rot, (ParamFloat) _param_sens_board_x_off, @@ -82,8 +86,8 @@ private: uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; @@ -93,12 +97,26 @@ private: {this, ORB_ID(sensor_accel), 2} }; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + matrix::Dcmf _board_rotation; matrix::Vector3f _bias{0.f, 0.f, 0.f}; matrix::Vector3f _offset{0.f, 0.f, 0.f}; matrix::Vector3f _scale{1.f, 1.f, 1.f}; + matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f}; + + hrt_abstime _last_publish{0}; + hrt_abstime _filter_check_last{0}; + static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */ + float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */ + + math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f}; + + float _filter_sample_rate{kInitialRateHz}; + int _sample_rate_incorrect_count{0}; + uint32_t _selected_sensor_device_id{0}; uint8_t _selected_sensor_sub_index{0}; int8_t _corrections_selected_instance{-1}; diff --git a/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c b/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c new file mode 100644 index 0000000000..c4fda14b94 --- /dev/null +++ b/src/modules/sensors/vehicle_acceleration/imu_accel_parameters.c @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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. + * + ****************************************************************************/ + +/** +* Low pass filter cutoff frequency for accel +* +* The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer. +* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 8be5493432..32dd9de0c5 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -273,7 +273,10 @@ void VehicleAngularVelocity::Run() bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated(); - if (sensor_updated || selection_updated) { + // process all outstanding messages + while (sensor_updated || selection_updated) { + selection_updated = false; + sensor_gyro_s sensor_data; if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) { @@ -286,12 +289,8 @@ void VehicleAngularVelocity::Run() const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f); _timestamp_sample_prev = 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 offsets and scale - Vector3f angular_velocity_raw{(val - _offset).emult(_scale)}; + Vector3f angular_velocity_raw{(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z} - _offset).emult(_scale)}; // rotate corrected measurements from sensor to body frame angular_velocity_raw = _board_rotation * angular_velocity_raw; @@ -312,32 +311,38 @@ void VehicleAngularVelocity::Run() const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)}; const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)}; - bool publish = true; + // publish once all new samples are processed + sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated(); - if (_param_imu_gyro_rate_max.get() > 0) { - const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); + if (!sensor_updated) { + bool publish = true; - if (hrt_elapsed_time(&_last_publish) < interval) { - publish = false; + 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 (publish) { - // Publish vehicle_angular_acceleration - vehicle_angular_acceleration_s v_angular_acceleration; - v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample; - angular_acceleration.copyTo(v_angular_acceleration.xyz); - v_angular_acceleration.timestamp = hrt_absolute_time(); - _vehicle_angular_acceleration_pub.publish(v_angular_acceleration); + if (publish) { + // Publish vehicle_angular_acceleration + vehicle_angular_acceleration_s v_angular_acceleration; + v_angular_acceleration.timestamp_sample = sensor_data.timestamp_sample; + angular_acceleration.copyTo(v_angular_acceleration.xyz); + v_angular_acceleration.timestamp = hrt_absolute_time(); + _vehicle_angular_acceleration_pub.publish(v_angular_acceleration); - // Publish vehicle_angular_velocity - vehicle_angular_velocity_s v_angular_velocity; - v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample; - angular_velocity.copyTo(v_angular_velocity.xyz); - v_angular_velocity.timestamp = hrt_absolute_time(); - _vehicle_angular_velocity_pub.publish(v_angular_velocity); + // Publish vehicle_angular_velocity + vehicle_angular_velocity_s v_angular_velocity; + v_angular_velocity.timestamp_sample = sensor_data.timestamp_sample; + angular_velocity.copyTo(v_angular_velocity.xyz); + v_angular_velocity.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_pub.publish(v_angular_velocity); - _last_publish = v_angular_velocity.timestamp_sample; + _last_publish = v_angular_velocity.timestamp_sample; + return; + } } } } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 8bf01e63eb..429b673f99 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -36,7 +36,6 @@ #include #include #include -#include #include #include #include @@ -95,8 +94,8 @@ private: uORB::Publication _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)}; uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index 68e2b01ed5..eb9a539356 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -32,9 +32,9 @@ ****************************************************************************/ /** -* Driver level notch frequency for gyro +* Notch filter frequency for gyro * -* The center frequency for the 2nd order notch filter on the gyro driver. +* The center frequency for the 2nd order notch filter on the primary gyro. * This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency. * This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. * See "IMU_GYRO_NF_BW" to set the bandwidth of the filter. @@ -48,9 +48,9 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f); /** -* Driver level notch bandwidth for gyro +* Notch filter bandwidth for gyro * -* The frequency width of the stop band for the 2nd order notch filter on the gyro driver. +* The frequency width of the stop band for the 2nd order notch filter on the primary gyro. * See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency. * * @min 0 @@ -62,9 +62,9 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f); PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f); /** -* Driver level cutoff frequency for gyro +* Low pass filter cutoff frequency for gyro * -* The cutoff frequency for the 2nd order butterworth filter on the gyro driver. +* The cutoff frequency for the 2nd order butterworth filter on the primary gyro. * This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. * * @min 0 diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index e9012751ad..f36bfaf8ff 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -103,7 +103,6 @@ public: private: Simulator() : ModuleParams(nullptr) { - _px4_accel.set_sample_rate(250); } ~Simulator() diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index a07767e8a7..7d293e0206 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -44,6 +44,7 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat Publication.hpp PublicationMulti.hpp PublicationQueued.hpp + PublicationQueuedMulti.hpp Subscription.cpp Subscription.hpp SubscriptionCallback.hpp diff --git a/src/modules/uORB/PublicationQueuedMulti.hpp b/src/modules/uORB/PublicationQueuedMulti.hpp new file mode 100644 index 0000000000..810277e6d7 --- /dev/null +++ b/src/modules/uORB/PublicationQueuedMulti.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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. + * + ****************************************************************************/ + +/** + * @file PublicationQueuedMulti.hpp + * + */ + +#pragma once + +#include +#include +#include + +namespace uORB +{ + +/** + * Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH) + */ +template +class PublicationQueuedMulti +{ +public: + + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. + */ + PublicationQueuedMulti(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : + _meta(meta), + _priority(priority) + {} + + ~PublicationQueuedMulti() + { + //orb_unadvertise(_handle); + } + + /** + * Publish the struct + * @param data The uORB message struct we are updating. + */ + bool publish(const T &data) + { + if (_handle != nullptr) { + return (orb_publish(_meta, _handle, &data) == PX4_OK); + + } else { + int instance = 0; + orb_advert_t handle = orb_advertise_multi_queue(_meta, &data, &instance, _priority, T::ORB_QUEUE_LENGTH); + + if (handle != nullptr) { + _handle = handle; + return true; + } + } + + return false; + } + +protected: + const orb_metadata *_meta; + + orb_advert_t _handle{nullptr}; + + const uint8_t _priority; +}; + +} // namespace uORB