yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

sensors: compute and publish vehicle_angular_acceleration

Browse Source 
- introduces parameter IMU_DGYRO_CUTOFF to configure the angular acceleration low pass filter
 - the angular acceleration is computed by differentiating angular velocity after the notch filter (IMU_GYRO_NF_FREQ & IMU_GYRO_NF_BW) is applied

Co-authored-by: Julien Lecoeur <jlecoeur@users.noreply.github.com>
This commit is contained in:
Daniel Agar
2020-01-27 16:44:01 -05:00
committed by GitHub
parent 497ab07daf
commit 1237402fa4
7 changed files with 98 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@@ -51,6 +51,7 @@
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
@@ -68,7 +69,7 @@ public:
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 @@ private:
(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff,
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
@@ -89,6 +92,7 @@ private:
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
)
uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
@@ -110,12 +114,22 @@ private:
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<matrix::Vector3f> _notch_filter{};
// angular velocity filters
math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.0f};
math::NotchFilter<matrix::Vector3f> _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};