diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index c2ef0af236..dc496571f6 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -42,7 +42,7 @@ px4_add_board( pwm_out_sim pwm_out rc_input - roboclaw + #roboclaw safety_button telemetry # all available telemetry drivers tone_alarm diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg index 7f717a5d92..cba63efe8d 100644 --- a/msg/sensor_gyro_fifo.msg +++ b/msg/sensor_gyro_fifo.msg @@ -13,3 +13,5 @@ int16[32] y # angular velocity in the FRD board frame Y-axis in ra int16[32] z # angular velocity in the FRD board frame Z-axis in rad/s uint8 rotation # Direction the sensor faces (see Rotation enum) + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 4d467819f4..1a96c2368d 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -48,7 +48,7 @@ struct wq_config_t { namespace wq_configurations { -static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1664, 0}; // PX4 inner loop highest priority +static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1888, 0}; // PX4 inner loop highest priority static constexpr wq_config_t ctrl_alloc{"wq:ctrl_alloc", 9500, 0}; // PX4 control allocation, same priority as rate_ctrl static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1}; diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 55e8cdc513..4a30440451 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -33,8 +33,6 @@ #include "LowPassFilter2p.hpp" -#include - #include namespace math @@ -72,25 +70,6 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) _a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c; } -float LowPassFilter2p::apply(float sample) -{ - // do the filtering - float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; - - if (!PX4_ISFINITE(delay_element_0)) { - // don't allow bad values to propagate via the filter - delay_element_0 = sample; - } - - const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; - - _delay_element_2 = _delay_element_1; - _delay_element_1 = delay_element_0; - - // return the value. Should be no need to check limits - return output; -} - float LowPassFilter2p::reset(float sample) { const float dval = sample / (_b0 + _b1 + _b2); diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 7d6c77caae..74d0f9a513 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -38,6 +38,8 @@ #pragma once +#include + namespace math { class __EXPORT LowPassFilter2p @@ -58,7 +60,23 @@ public: * * @return retrieve the filtered result */ - float apply(float sample); + inline float apply(float sample) + { + // Direct Form II implementation + float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; + + if (!PX4_ISFINITE(delay_element_0)) { + // don't allow bad values to propagate via the filter + delay_element_0 = sample; + } + + const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + + _delay_element_2 = _delay_element_1; + _delay_element_1 = delay_element_0; + + return output; + } // Return the cutoff frequency float get_cutoff_freq() const { return _cutoff_freq; } diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp index 023a4abe90..28846fcd0a 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp @@ -51,35 +51,23 @@ public: { } - /** - * Add a new raw value to the filter - * - * @return retrieve the filtered result - */ - inline float apply(const int16_t samples[], uint8_t num_samples) + // Filter array of samples in place using the Direct form II. + inline void apply(float samples[], uint8_t num_samples) { - float output = 0.0f; - for (int n = 0; n < num_samples; n++) { - // do the filtering - float delay_element_0 = samples[n] - _delay_element_1 * _a1 - _delay_element_2 * _a2; + // Direct Form II implementation + float delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2}; - if (n == num_samples - 1) { - output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + // don't allow bad values to propagate via the filter + if (!PX4_ISFINITE(delay_element_0)) { + delay_element_0 = samples[n]; } + samples[n] = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + _delay_element_2 = _delay_element_1; _delay_element_1 = delay_element_0; } - - // don't allow bad values to propagate via the filter - if (!PX4_ISFINITE(output)) { - reset(samples[num_samples - 1]); - output = samples[num_samples - 1]; - } - - // return the value. Should be no need to check limits - return output; } }; diff --git a/src/lib/mathlib/math/filter/NotchFilterArray.hpp b/src/lib/mathlib/math/filter/NotchFilterArray.hpp index ffe86c0d05..eff13b868c 100644 --- a/src/lib/mathlib/math/filter/NotchFilterArray.hpp +++ b/src/lib/mathlib/math/filter/NotchFilterArray.hpp @@ -64,11 +64,7 @@ public: NotchFilterArray() = default; ~NotchFilterArray() = default; - /** - * Add new raw values to the filter using the Direct form II. - * - * @return retrieve the filtered result - */ + // Filter array of samples in place using the Direct form II. inline void apply(T samples[], uint8_t num_samples) { for (int n = 0; n < num_samples; n++) { @@ -96,8 +92,8 @@ public: { for (int n = 0; n < num_samples; n++) { // Direct Form II implementation - const T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - - _a2 * _delay_element_output_2; + T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - + _a2 * _delay_element_output_2; // don't allow bad values to propagate via the filter if (!isFinite(output)) { diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 2dc0a836bc..a2778ca36e 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,7 +46,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - CheckAndUpdateFilters(); } VehicleAngularVelocity::~VehicleAngularVelocity() @@ -81,46 +80,80 @@ void VehicleAngularVelocity::Stop() Deinit(); } -void VehicleAngularVelocity::CheckAndUpdateFilters() +bool VehicleAngularVelocity::UpdateSampleRate() { - bool sample_rate_changed = false; + float sample_rate_hz = NAN; + float publish_rate_hz = NAN; - // get sample rate from vehicle_imu_status publication for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; - const float sample_rate_hz = imu_status.get().gyro_rate_hz; - - if ((imu_status.get().gyro_device_id != 0) && (imu_status.get().gyro_device_id == _calibration.device_id()) - && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) { - // check if sample rate error is greater than 1% - if ((fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { - PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz); - _filter_sample_rate = sample_rate_hz; - sample_rate_changed = true; - break; - } + if (imu_status.get().gyro_device_id == _selected_sensor_device_id) { + sample_rate_hz = imu_status.get().gyro_raw_rate_hz; + publish_rate_hz = imu_status.get().gyro_rate_hz; + break; } } - // update software low pass filters - if (sample_rate_changed || (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.1f)) { - _lp_filter_velocity.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get()); - _lp_filter_velocity.reset(_angular_velocity_prev); + // calculate sensor update rate + if (PX4_ISFINITE(sample_rate_hz) && PX4_ISFINITE(publish_rate_hz)) { + // check if sample rate error is greater than 1% + if ((fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f) { + PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz); + _reset_filters = true; + _filter_sample_rate_hz = sample_rate_hz; + + if (_param_imu_gyro_ratemax.get() > 0.f) { + // determine number of sensor samples that will get closest to the desired rate + const float configured_interval_us = 1e6f / _param_imu_gyro_ratemax.get(); + const float publish_interval_us = 1e6f / publish_rate_hz; + + const uint8_t samples = roundf(configured_interval_us / publish_interval_us); + + if (_fifo_available) { + _sensor_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH)); + + } else { + _sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH)); + } + + // publish interval (constrained 100 Hz - 8 kHz) + _publish_interval_min_us = math::constrain((int)roundf(configured_interval_us - (publish_interval_us / 2.f)), 125, + 10000); + + } else { + _sensor_sub.set_required_updates(1); + _sensor_fifo_sub.set_required_updates(1); + _publish_interval_min_us = 0; + } + } + + if (_filter_sample_rate_hz > 0.f) { + return true; + } } - if (sample_rate_changed - || (fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.1f) - || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.1f) - ) { - _notch_filter_velocity.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get()); - _notch_filter_velocity.reset(_angular_velocity_prev); + return false; +} + +void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, const Vector3f &angular_acceleration) +{ + for (int axis = 0; axis < 3; axis++) { + // angular velocity low pass + _lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get()); + _lp_filter_velocity[axis].reset(angular_velocity(axis)); + + // angular velocity notch + _notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(), + _param_imu_gyro_nf_bw.get()); + _notch_filter_velocity[axis].reset(angular_velocity(axis)); + + // angular acceleration low pass + _lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()); + _lp_filter_acceleration[axis].reset(angular_acceleration(axis)); } - if (sample_rate_changed || (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.1f)) { - _lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get()); - _lp_filter_acceleration.reset(_angular_acceleration_prev); - } + _reset_filters = false; } void VehicleAngularVelocity::SensorBiasUpdate(bool force) @@ -137,40 +170,63 @@ void VehicleAngularVelocity::SensorBiasUpdate(bool force) if (_estimator_sensor_bias_sub.updated() || force) { estimator_sensor_bias_s bias; - if (_estimator_sensor_bias_sub.copy(&bias)) { - if (bias.gyro_device_id == _calibration.device_id()) { - _bias = Vector3f{bias.gyro_bias}; + if (_estimator_sensor_bias_sub.copy(&bias) && (bias.gyro_device_id == _selected_sensor_device_id)) { + _bias = Vector3f{bias.gyro_bias}; - } else { - _bias.zero(); - } + } else { + _bias.zero(); } } } bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) { - if (_sensor_selection_sub.updated() || (_calibration.device_id() == 0) || force) { + if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) { sensor_selection_s sensor_selection{}; _sensor_selection_sub.copy(&sensor_selection); - if ((sensor_selection.gyro_device_id != 0) && (_calibration.device_id() != sensor_selection.gyro_device_id)) { + if (_selected_sensor_device_id != sensor_selection.gyro_device_id) { + + // see if the selected sensor publishes sensor_gyro_fifo + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; + + if ((sensor_gyro_fifo_sub.get().device_id != 0) + && (sensor_gyro_fifo_sub.get().device_id == sensor_selection.gyro_device_id)) { + if (_sensor_fifo_sub.ChangeInstance(i) && _sensor_fifo_sub.registerCallback()) { + // make sure non-FIFO sub is unregistered + _sensor_sub.unregisterCallback(); + + // record selected sensor + _selected_sensor_device_id = sensor_selection.gyro_device_id; + _calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id); + + _reset_filters = true; + _bias.zero(); + _fifo_available = true; + + return true; + } + } + } + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - const uint32_t device_id = sensor_gyro_sub.get().device_id; - - if ((device_id != 0) && (device_id == sensor_selection.gyro_device_id)) { - + if ((sensor_gyro_sub.get().device_id != 0) + && (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id)) { if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { - PX4_DEBUG("selected sensor changed %d -> %d", _calibration.device_id(), device_id); + // make sure FIFO sub is unregistered + _sensor_fifo_sub.unregisterCallback(); + + // record selected sensor + _calibration.set_device_id(sensor_gyro_sub.get().device_id); + _selected_sensor_device_id = sensor_selection.gyro_device_id; // clear bias and corrections + _reset_filters = true; _bias.zero(); - - _calibration.set_device_id(device_id); - - CheckAndUpdateFilters(); + _fifo_available = false; return true; } @@ -178,7 +234,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) } PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.gyro_device_id); - _calibration.set_device_id(0); + _selected_sensor_device_id = 0; } } @@ -197,7 +253,31 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) _calibration.ParametersUpdate(); - CheckAndUpdateFilters(); + // gyro low pass cutoff frequency changed + for (auto &lp : _lp_filter_velocity) { + if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) { + _reset_filters = true; + break; + } + } + + // gyro notch filter frequency or bandwidth changed + for (auto &nf : _notch_filter_velocity) { + if ((fabsf(nf.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f) + || (fabsf(nf.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)) { + + _reset_filters = true; + break; + } + } + + // gyro derivative low pass cutoff changed + for (auto &lp : _lp_filter_acceleration) { + if (fabsf(lp.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) { + _reset_filters = true; + break; + } + } } } @@ -207,81 +287,162 @@ void VehicleAngularVelocity::Run() ScheduleDelayed(10_ms); // update corrections first to set _selected_sensor - bool selection_updated = SensorSelectionUpdate(); + const bool selection_updated = SensorSelectionUpdate(); _calibration.SensorCorrectionsUpdate(selection_updated); SensorBiasUpdate(selection_updated); ParametersUpdate(); - // process all outstanding messages - sensor_gyro_s sensor_data; + if (_fifo_available) { + // process all outstanding fifo messages + sensor_gyro_fifo_s sensor_fifo_data; - while (_sensor_sub.update(&sensor_data)) { + while (_sensor_fifo_sub.update(&sensor_fifo_data)) { + static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f, 0.02f); - _timestamp_sample_prev = sensor_data.timestamp_sample; + if ((sensor_fifo_data.samples > 0) && (sensor_fifo_data.samples <= FIFO_SIZE_MAX)) { + const int N = sensor_fifo_data.samples; + const float dt_s = sensor_fifo_data.dt * 1e-6f; + const enum Rotation fifo_rotation = static_cast(sensor_fifo_data.rotation); - // 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}; + if (_reset_filters || (fabsf(sensor_fifo_data.scale - _fifo_last_scale) > FLT_EPSILON)) { + if (UpdateSampleRate()) { + // in FIFO mode the unscaled raw data is filtered + _angular_velocity_prev = _angular_velocity / sensor_fifo_data.scale; + ResetFilters(_angular_velocity_prev, _angular_acceleration / sensor_fifo_data.scale); - // correct for in-run bias errors - const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias; + _fifo_last_scale = sensor_fifo_data.scale; + } - // Gyro filtering: - // - Apply general notch filter (IMU_GYRO_NF_FREQ) - // - Apply general low-pass filter (IMU_GYRO_CUTOFF) - // - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + if (_reset_filters) { + continue; // not safe to run until filters configured + } + } - const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)}; + Vector3f angular_velocity_unscaled; + Vector3f angular_acceleration_unscaled; - const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)}; + int16_t *raw_data_array[] {sensor_fifo_data.x, sensor_fifo_data.y, sensor_fifo_data.z}; - const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt; - _angular_velocity_prev = angular_velocity; - _angular_acceleration_prev = angular_acceleration_raw; - const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)}; + for (int axis = 0; axis < 3; axis++) { + // copy raw int16 sensor samples to float array for filtering + float data[FIFO_SIZE_MAX]; + + for (int n = 0; n < N; n++) { + data[n] = raw_data_array[axis][n]; + } + + // Apply general notch filter (IMU_GYRO_NF_FREQ) + if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) { + _notch_filter_velocity[axis].apply(data, N); + } + + // Apply general low-pass filter (IMU_GYRO_CUTOFF) + _lp_filter_velocity[axis].apply(data, N); + + // save last filtered sample + angular_velocity_unscaled(axis) = data[N - 1]; - // publish once all new samples are processed - if (!_sensor_sub.updated()) { - bool publish = true; + // angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + float delta_velocity_filtered; - if (_param_imu_gyro_rate_max.get() > 0) { - const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); + for (int n = 0; n < N; n++) { + const float delta_velocity = (data[n] - _angular_velocity_prev(axis)); + delta_velocity_filtered = _lp_filter_acceleration[axis].apply(delta_velocity); + _angular_velocity_prev(axis) = data[n]; + } - if (hrt_elapsed_time(&_last_publish) < interval) { - publish = false; + angular_acceleration_unscaled(axis) = delta_velocity_filtered / dt_s; + } + + // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias + rotate_3f(fifo_rotation, angular_velocity_unscaled(0), angular_velocity_unscaled(1), angular_velocity_unscaled(2)); + _angular_velocity = _calibration.Correct(angular_velocity_unscaled * sensor_fifo_data.scale) - _bias; + + // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation + rotate_3f(fifo_rotation, angular_acceleration_unscaled(0), angular_acceleration_unscaled(1), + angular_acceleration_unscaled(2)); + _angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * sensor_fifo_data.scale; + + // Publish + if (!_sensor_fifo_sub.updated() && (sensor_fifo_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) { + Publish(sensor_fifo_data.timestamp_sample); + } + } + } + + } else { + // process all outstanding messages + sensor_gyro_s sensor_data; + + while (_sensor_sub.update(&sensor_data)) { + const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) / 1e6f), 0.0002f, 0.02f); + _timestamp_sample_last = sensor_data.timestamp_sample; + + if (_reset_filters) { + if (UpdateSampleRate()) { + _angular_velocity_prev = _angular_velocity; + ResetFilters(_angular_velocity, _angular_acceleration); + } + + if (_reset_filters) { + continue; // not safe to run until filters configured } } - 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); + // Apply calibration, rotation, and correct for in-run bias errors + Vector3f angular_velocity{_calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias}; - // 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); + for (int axis = 0; axis < 3; axis++) { + // Apply general notch filter (IMU_GYRO_NF_FREQ) + _notch_filter_velocity[axis].apply(&angular_velocity(axis), 1); - _last_publish = v_angular_velocity.timestamp_sample; - return; + // Apply general low-pass filter (IMU_GYRO_CUTOFF) + _lp_filter_velocity[axis].apply(&angular_velocity(axis), 1); + + // Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + const float accel = (angular_velocity(axis) - _angular_velocity_prev(axis)) / dt_s; + _angular_acceleration(axis) = _lp_filter_acceleration[axis].apply(accel); + _angular_velocity_prev(axis) = angular_velocity(axis); + } + + _angular_velocity = angular_velocity; + + // Publish + if (!_sensor_sub.updated() && (sensor_data.timestamp_sample - _last_publish >= _publish_interval_min_us)) { + Publish(sensor_data.timestamp_sample); } } } } +void VehicleAngularVelocity::Publish(const hrt_abstime ×tamp_sample) +{ + // Publish vehicle_angular_acceleration + vehicle_angular_acceleration_s v_angular_acceleration; + v_angular_acceleration.timestamp_sample = timestamp_sample; + _angular_acceleration.copyTo(v_angular_acceleration.xyz); + v_angular_acceleration.timestamp = hrt_absolute_time(); + _vehicle_angular_acceleration_pub.publish(v_angular_acceleration); + + // Publish vehicle_angular_velocity + vehicle_angular_velocity_s v_angular_velocity; + v_angular_velocity.timestamp_sample = timestamp_sample; + _angular_velocity.copyTo(v_angular_velocity.xyz); + v_angular_velocity.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_pub.publish(v_angular_velocity); + + // shift last publish time forward, but don't let it get further behind than the interval + _last_publish = math::constrain(_last_publish + _publish_interval_min_us, + timestamp_sample - _publish_interval_min_us, timestamp_sample); +} + void VehicleAngularVelocity::PrintStatus() { - PX4_INFO("selected sensor: %d, rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]", - _calibration.device_id(), (double)_filter_sample_rate, - (double)_bias(0), (double)_bias(1), (double)_bias(2)); + PX4_INFO("selected sensor: %d, rate: %.1f Hz %s", + _selected_sensor_device_id, (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : ""); + PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2)); _calibration.PrintStatus(); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 82180faafb..d0b84735ad 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -36,8 +36,9 @@ #include #include #include -#include -#include +#include +#include +#include #include #include #include @@ -49,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -72,15 +74,19 @@ public: private: void Run() override; - void CheckAndUpdateFilters(); + void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration); + bool UpdateSampleRate(); + void ParametersUpdate(bool force = false); void SensorBiasUpdate(bool force = false); bool SensorSelectionUpdate(bool force = false); + void Publish(const hrt_abstime ×tamp_sample); + static constexpr int MAX_SENSOR_COUNT = 4; uORB::Publication _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)}; - uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; + uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; @@ -89,32 +95,44 @@ private: uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)}; + uORB::SubscriptionCallbackWorkItem _sensor_fifo_sub{this, ORB_ID(sensor_gyro_fifo)}; calibration::Gyroscope _calibration{}; matrix::Vector3f _bias{}; - matrix::Vector3f _angular_acceleration_prev{}; - matrix::Vector3f _angular_velocity_prev{}; - hrt_abstime _timestamp_sample_prev{0}; + matrix::Vector3f _angular_velocity{}; + matrix::Vector3f _angular_acceleration{}; + matrix::Vector3f _angular_velocity_prev{}; + hrt_abstime _timestamp_sample_last{0}; + + hrt_abstime _publish_interval_min_us{0}; hrt_abstime _last_publish{0}; + + float _filter_sample_rate_hz{0.f}; + static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */ - float _filter_sample_rate{kInitialRateHz}; // angular velocity filters - math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.f}; - math::NotchFilter _notch_filter_velocity{}; + math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}}; + math::NotchFilterArray _notch_filter_velocity[3] {}; // angular acceleration filter - math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 30.f}; + math::LowPassFilter2p _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}}; + + uint32_t _selected_sensor_device_id{0}; + + float _fifo_last_scale{0}; + + bool _reset_filters{true}; + bool _fifo_available{false}; DEFINE_PARAMETERS( (ParamFloat) _param_imu_gyro_cutoff, (ParamFloat) _param_imu_gyro_nf_freq, (ParamFloat) _param_imu_gyro_nf_bw, - (ParamInt) _param_imu_gyro_rate_max, - + (ParamInt) _param_imu_gyro_ratemax, (ParamFloat) _param_imu_dgyro_cutoff ) }; 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 ea6f422f72..aee724ce14 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -120,4 +120,4 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); +PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);