diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 8bd94b73cc8d..e92eff278c7b 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -135,6 +135,7 @@ set(msg_files ulog_stream_ack.msg vehicle_acceleration.msg vehicle_air_data.msg + vehicle_angular_acceleration.msg vehicle_angular_velocity.msg vehicle_attitude.msg vehicle_attitude_setpoint.msg diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index e0e6d2deca86..69cea00748cc 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -277,6 +277,8 @@ rtps: id: 123 - msg: vehicle_imu id: 124 + - msg: vehicle_angular_acceleration + id: 125 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/vehicle_angular_acceleration.msg b/msg/vehicle_angular_acceleration.msg new file mode 100644 index 000000000000..442dd8e68062 --- /dev/null +++ b/msg/vehicle_angular_acceleration.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # angular acceleration about X, Y, Z body axis in rad/s^2, diff --git a/msg/vehicle_angular_velocity.msg b/msg/vehicle_angular_velocity.msg index 3f1b0a7720d5..61e89eee4319 100644 --- a/msg/vehicle_angular_velocity.msg +++ b/msg/vehicle_angular_velocity.msg @@ -1,7 +1,6 @@ -uint64 timestamp # time since system start (microseconds) - -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) float32[3] xyz # Bias corrected angular velocity about X, Y, Z body axis in rad/s diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 71100c4bb3e8..8be549343210 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -43,8 +43,10 @@ VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _lowpass_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get()); - _notch_filter.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get()); + _lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get()); + _notch_filter_velocity.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get()); + + _lp_filter_acceleration.set_cutoff_frequency(kInitialRateHz, _param_imu_dgyro_cutoff.get()); } VehicleAngularVelocity::~VehicleAngularVelocity() @@ -81,7 +83,7 @@ void VehicleAngularVelocity::Stop() _sensor_selection_sub.unregisterCallback(); } -void VehicleAngularVelocity::CheckFilters(const Vector3f &rates) +void VehicleAngularVelocity::CheckFilters() { if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) { _filter_check_last = hrt_absolute_time(); @@ -104,20 +106,27 @@ void VehicleAngularVelocity::CheckFilters(const Vector3f &rates) } const bool sample_rate_updated = (_sample_rate_incorrect_count > 50); - const bool lowpass_updated = (fabsf(_lowpass_filter.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f); - const bool notch_updated = ((fabsf(_notch_filter.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f) - || (fabsf(_notch_filter.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)); - if (sample_rate_updated || lowpass_updated || notch_updated) { + const bool lp_velocity_updated = (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f); + const bool notch_updated = ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f) + || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)); + + const bool lp_acceleration_updated = (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > + 0.01f); + + if (sample_rate_updated || lp_velocity_updated || notch_updated || lp_acceleration_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 - _lowpass_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get()); - _lowpass_filter.reset(rates); + _lp_filter_velocity.set_cutoff_frequency(_filter_sample_rate, _param_imu_gyro_cutoff.get()); + _lp_filter_velocity.reset(_angular_velocity_prev); - _notch_filter.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get()); - _notch_filter.reset(rates); + _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); + + _lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get()); + _lp_filter_acceleration.reset(_angular_acceleration_prev); // reset state _sample_rate_incorrect_count = 0; @@ -273,22 +282,35 @@ void VehicleAngularVelocity::Run() perf_count_interval(_interval_perf, sensor_data.timestamp_sample); } + // 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; + + // 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 rates{(val - _offset).emult(_scale)}; + Vector3f angular_velocity_raw{(val - _offset).emult(_scale)}; // rotate corrected measurements from sensor to body frame - rates = _board_rotation * rates; + angular_velocity_raw = _board_rotation * angular_velocity_raw; // correct for in-run bias errors - rates -= _bias; + angular_velocity_raw -= _bias; - // Filter: apply notch and then low-pass - CheckFilters(rates); - const Vector3f rates_filtered = _lowpass_filter.apply(_notch_filter.apply(rates)); + // Differentiate angular velocity (after notch filter) + const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)}; + const Vector3f angular_acceleration_raw = (angular_velocity_notched - _angular_velocity_prev) / dt; + _angular_velocity_prev = angular_velocity_notched; + _angular_acceleration_prev = angular_acceleration_raw; + + CheckFilters(); + + // Filter: apply low-pass + 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; @@ -301,15 +323,21 @@ void VehicleAngularVelocity::Run() } if (publish) { - vehicle_angular_velocity_s out; - - out.timestamp_sample = sensor_data.timestamp_sample; - rates_filtered.copyTo(out.xyz); - out.timestamp = hrt_absolute_time(); - - _vehicle_angular_velocity_pub.publish(out); - - _last_publish = out.timestamp_sample; + // 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); + + _last_publish = v_angular_velocity.timestamp_sample; } } } @@ -323,10 +351,10 @@ void VehicleAngularVelocity::PrintStatus() 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)_lowpass_filter.get_cutoff_freq()); + PX4_INFO("low-pass filter cutoff: %.3f Hz", (double)_lp_filter_velocity.get_cutoff_freq()); - if (_notch_filter.getNotchFreq() > 0.0f) { - PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter.getNotchFreq(), - (double)_notch_filter.getBandwidth()); + if (_notch_filter_velocity.getNotchFreq() > 0.0f) { + PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter_velocity.getNotchFreq(), + (double)_notch_filter_velocity.getBandwidth()); } } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 0c96a291e4b4..8bf01e63ebd3 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -51,6 +51,7 @@ #include #include #include +#include #include class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem @@ -68,7 +69,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem private: void Run() override; - void CheckFilters(const matrix::Vector3f &rates); + void CheckFilters(); void ParametersUpdate(bool force = false); void SensorBiasUpdate(bool force = false); void SensorCorrectionsUpdate(bool force = false); @@ -82,6 +83,8 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem (ParamFloat) _param_imu_gyro_nf_bw, (ParamInt) _param_imu_gyro_rate_max, + (ParamFloat) _param_imu_dgyro_cutoff, + (ParamInt) _param_sens_board_rot, (ParamFloat) _param_sens_board_x_off, @@ -89,6 +92,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem (ParamFloat) _param_sens_board_z_off ) + 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)}; @@ -110,12 +114,22 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem matrix::Vector3f _offset{0.f, 0.f, 0.f}; matrix::Vector3f _scale{1.f, 1.f, 1.f}; + matrix::Vector3f _angular_acceleration_prev{0.f, 0.f, 0.f}; + matrix::Vector3f _angular_velocity_prev{0.f, 0.f, 0.f}; + hrt_abstime _timestamp_sample_prev{0}; + 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 _lowpass_filter{kInitialRateHz, 30}; - math::NotchFilter _notch_filter{}; + + // angular velocity filters + math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.0f}; + math::NotchFilter _notch_filter_velocity{}; + + // angular acceleration filter + math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 10.0f}; + float _filter_sample_rate{kInitialRateHz}; int _sample_rate_incorrect_count{0}; 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 25eb5aab580b..68e2b01ed5c5 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -94,3 +94,18 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f); * @group Sensors */ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0); + +/** +* Cutoff frequency for angular acceleration +* +* The cutoff frequency for the 2nd order butterworth filter used on +* the time derivative of the measured angular velocity. +* Set to 0 to disable the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);