forked from Archive/PX4-Autopilot
sensors/vehicle_angular_velocity: unify filtering for both FIFO and regular use cases
This commit is contained in:
parent
8c173b2e7f
commit
ae010ea55c
|
@ -146,8 +146,11 @@ bool VehicleAngularVelocity::UpdateSampleRate()
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, const Vector3f &angular_acceleration)
|
void VehicleAngularVelocity::ResetFilters()
|
||||||
{
|
{
|
||||||
|
const Vector3f angular_velocity{GetResetAngularVelocity()};
|
||||||
|
const Vector3f angular_acceleration{GetResetAngularAcceleration()};
|
||||||
|
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
// angular velocity low pass
|
// angular velocity low pass
|
||||||
_lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get());
|
_lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get());
|
||||||
|
@ -161,15 +164,17 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons
|
||||||
// angular acceleration low pass
|
// angular acceleration low pass
|
||||||
_lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get());
|
_lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get());
|
||||||
_lp_filter_acceleration[axis].reset(angular_acceleration(axis));
|
_lp_filter_acceleration[axis].reset(angular_acceleration(axis));
|
||||||
|
|
||||||
// dynamic notch filters, first disable, then force update (if available)
|
|
||||||
DisableDynamicNotchEscRpm();
|
|
||||||
DisableDynamicNotchFFT();
|
|
||||||
|
|
||||||
UpdateDynamicNotchEscRpm(true);
|
|
||||||
UpdateDynamicNotchFFT(true);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// dynamic notch filters, first disable, then force update (if available)
|
||||||
|
DisableDynamicNotchEscRpm();
|
||||||
|
DisableDynamicNotchFFT();
|
||||||
|
|
||||||
|
UpdateDynamicNotchEscRpm(true);
|
||||||
|
UpdateDynamicNotchFFT(true);
|
||||||
|
|
||||||
|
_angular_velocity_prev = angular_velocity;
|
||||||
|
|
||||||
_reset_filters = false;
|
_reset_filters = false;
|
||||||
perf_count(_filter_reset_perf);
|
perf_count(_filter_reset_perf);
|
||||||
}
|
}
|
||||||
|
@ -314,6 +319,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
|
||||||
if (_dynamic_notch_filter_esc_rpm_perf == nullptr) {
|
if (_dynamic_notch_filter_esc_rpm_perf == nullptr) {
|
||||||
_dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter");
|
_dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
DisableDynamicNotchEscRpm();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) {
|
if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) {
|
||||||
|
@ -324,6 +332,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
|
||||||
if (_dynamic_notch_filter_fft_perf == nullptr) {
|
if (_dynamic_notch_filter_fft_perf == nullptr) {
|
||||||
_dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter");
|
_dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
DisableDynamicNotchFFT();
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // !CONSTRAINED_FLASH
|
#endif // !CONSTRAINED_FLASH
|
||||||
|
@ -332,11 +343,27 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
|
||||||
|
|
||||||
Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
|
Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const
|
||||||
{
|
{
|
||||||
if (_fifo_available && (_last_scale > 0.f)) {
|
if ((_last_publish != 0) && (_last_scale > 0.f)
|
||||||
|
&& PX4_ISFINITE(_angular_velocity(0))
|
||||||
|
&& PX4_ISFINITE(_angular_velocity(1))
|
||||||
|
&& PX4_ISFINITE(_angular_velocity(2))) {
|
||||||
|
// angular velocity filtering is performed on raw unscaled data
|
||||||
|
// start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection)
|
||||||
return _calibration.Uncorrect(_angular_velocity + _bias) / _last_scale;
|
return _calibration.Uncorrect(_angular_velocity + _bias) / _last_scale;
|
||||||
|
}
|
||||||
|
|
||||||
} else if (!_fifo_available) {
|
return Vector3f{0.f, 0.f, 0.f};
|
||||||
return _angular_velocity;
|
}
|
||||||
|
|
||||||
|
Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const
|
||||||
|
{
|
||||||
|
if ((_last_publish != 0) && (_last_scale > 0.f)
|
||||||
|
&& PX4_ISFINITE(_angular_acceleration(0))
|
||||||
|
&& PX4_ISFINITE(_angular_acceleration(1))
|
||||||
|
&& PX4_ISFINITE(_angular_acceleration(2))) {
|
||||||
|
// angular acceleration filtering is performed on raw unscaled data
|
||||||
|
// start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection)
|
||||||
|
return _calibration.rotation().I() * _angular_acceleration / _last_scale;
|
||||||
}
|
}
|
||||||
|
|
||||||
return Vector3f{0.f, 0.f, 0.f};
|
return Vector3f{0.f, 0.f, 0.f};
|
||||||
|
@ -490,6 +517,73 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
|
||||||
#endif // !CONSTRAINED_FLASH
|
#endif // !CONSTRAINED_FLASH
|
||||||
}
|
}
|
||||||
|
|
||||||
|
float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int N)
|
||||||
|
{
|
||||||
|
#if !defined(CONSTRAINED_FLASH)
|
||||||
|
|
||||||
|
// Apply dynamic notch filter from ESC RPM
|
||||||
|
if (_dynamic_notch_esc_rpm_available) {
|
||||||
|
perf_begin(_dynamic_notch_filter_esc_rpm_perf);
|
||||||
|
|
||||||
|
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
|
||||||
|
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
|
||||||
|
if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
|
||||||
|
dnf[harmonic][axis].applyDF1(data, N);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
perf_end(_dynamic_notch_filter_esc_rpm_perf);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply dynamic notch filter from FFT
|
||||||
|
if (_dynamic_notch_fft_available) {
|
||||||
|
perf_begin(_dynamic_notch_filter_fft_perf);
|
||||||
|
|
||||||
|
for (auto &dnf : _dynamic_notch_filter_fft) {
|
||||||
|
if (dnf[axis].getNotchFreq() > 0.f) {
|
||||||
|
dnf[axis].applyDF1(data, N);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
perf_end(_dynamic_notch_filter_fft_perf);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // !CONSTRAINED_FLASH
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// return last filtered sample
|
||||||
|
return data[N - 1];
|
||||||
|
}
|
||||||
|
|
||||||
|
float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float data[], int N, float dt_s)
|
||||||
|
{
|
||||||
|
if (N > 0) {
|
||||||
|
// angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
|
||||||
|
float delta_velocity_filtered;
|
||||||
|
|
||||||
|
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];
|
||||||
|
}
|
||||||
|
|
||||||
|
return delta_velocity_filtered / dt_s;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0.f;
|
||||||
|
}
|
||||||
|
|
||||||
void VehicleAngularVelocity::Run()
|
void VehicleAngularVelocity::Run()
|
||||||
{
|
{
|
||||||
// backup schedule
|
// backup schedule
|
||||||
|
@ -507,30 +601,26 @@ void VehicleAngularVelocity::Run()
|
||||||
sensor_gyro_fifo_s sensor_fifo_data;
|
sensor_gyro_fifo_s sensor_fifo_data;
|
||||||
|
|
||||||
while (_sensor_fifo_sub.update(&sensor_fifo_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]);
|
const float dt_s = sensor_fifo_data.dt * 1e-6f;
|
||||||
|
_timestamp_sample_last = sensor_fifo_data.timestamp_sample;
|
||||||
|
|
||||||
if ((sensor_fifo_data.samples > 0) && (sensor_fifo_data.samples <= FIFO_SIZE_MAX)) {
|
if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) {
|
||||||
_timestamp_sample_last = sensor_fifo_data.timestamp_sample;
|
if (UpdateSampleRate()) {
|
||||||
|
// in FIFO mode the unscaled raw data is filtered
|
||||||
|
_last_scale = sensor_fifo_data.scale;
|
||||||
|
|
||||||
const int N = sensor_fifo_data.samples;
|
ResetFilters();
|
||||||
const float dt_s = sensor_fifo_data.dt * 1e-6f;
|
|
||||||
|
|
||||||
if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) {
|
|
||||||
if (UpdateSampleRate()) {
|
|
||||||
// in FIFO mode the unscaled raw data is filtered
|
|
||||||
_last_scale = sensor_fifo_data.scale;
|
|
||||||
|
|
||||||
_angular_velocity_prev = GetResetAngularVelocity();
|
|
||||||
Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration / _last_scale};
|
|
||||||
|
|
||||||
ResetFilters(_angular_velocity_prev, angular_acceleration);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (_reset_filters) {
|
|
||||||
continue; // not safe to run until filters configured
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (_reset_filters) {
|
||||||
|
continue; // not safe to run until filters configured
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const int N = sensor_fifo_data.samples;
|
||||||
|
static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
|
||||||
|
|
||||||
|
if ((N > 0) && (N <= FIFO_SIZE_MAX)) {
|
||||||
UpdateDynamicNotchEscRpm();
|
UpdateDynamicNotchEscRpm();
|
||||||
UpdateDynamicNotchFFT();
|
UpdateDynamicNotchFFT();
|
||||||
|
|
||||||
|
@ -547,74 +637,15 @@ void VehicleAngularVelocity::Run()
|
||||||
data[n] = raw_data_array[axis][n];
|
data[n] = raw_data_array[axis][n];
|
||||||
}
|
}
|
||||||
|
|
||||||
#if !defined(CONSTRAINED_FLASH)
|
|
||||||
|
|
||||||
// Apply dynamic notch filter from ESC RPM
|
|
||||||
if (_dynamic_notch_esc_rpm_available) {
|
|
||||||
perf_begin(_dynamic_notch_filter_esc_rpm_perf);
|
|
||||||
|
|
||||||
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
|
|
||||||
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
|
|
||||||
if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
|
|
||||||
dnf[harmonic][axis].applyDF1(data, N);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
perf_end(_dynamic_notch_filter_esc_rpm_perf);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Apply dynamic notch filter from FFT
|
|
||||||
if (_dynamic_notch_fft_available) {
|
|
||||||
perf_begin(_dynamic_notch_filter_fft_perf);
|
|
||||||
|
|
||||||
for (auto &dnf : _dynamic_notch_filter_fft) {
|
|
||||||
if (dnf[axis].getNotchFreq() > 0.f) {
|
|
||||||
dnf[axis].applyDF1(data, N);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
perf_end(_dynamic_notch_filter_fft_perf);
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // !CONSTRAINED_FLASH
|
|
||||||
|
|
||||||
// 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
|
// save last filtered sample
|
||||||
angular_velocity_unscaled(axis) = data[N - 1];
|
angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, N);
|
||||||
|
angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, data, N, dt_s);
|
||||||
|
|
||||||
// angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
|
|
||||||
float delta_velocity_filtered;
|
|
||||||
|
|
||||||
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];
|
|
||||||
}
|
|
||||||
|
|
||||||
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
|
|
||||||
_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
|
|
||||||
_angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * sensor_fifo_data.scale;
|
|
||||||
|
|
||||||
// Publish
|
// Publish
|
||||||
if (!_sensor_fifo_sub.updated()) {
|
if (!_sensor_fifo_sub.updated()) {
|
||||||
Publish(sensor_fifo_data.timestamp_sample);
|
CalibrateAndPublish(sensor_fifo_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled,
|
||||||
|
sensor_fifo_data.scale);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -629,8 +660,9 @@ void VehicleAngularVelocity::Run()
|
||||||
|
|
||||||
if (_reset_filters) {
|
if (_reset_filters) {
|
||||||
if (UpdateSampleRate()) {
|
if (UpdateSampleRate()) {
|
||||||
_angular_velocity_prev = _angular_velocity;
|
// non-FIFO sensor data is already scaled
|
||||||
ResetFilters(_angular_velocity, _angular_acceleration);
|
_last_scale = 1.f;
|
||||||
|
ResetFilters();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_reset_filters) {
|
if (_reset_filters) {
|
||||||
|
@ -641,70 +673,39 @@ void VehicleAngularVelocity::Run()
|
||||||
UpdateDynamicNotchEscRpm();
|
UpdateDynamicNotchEscRpm();
|
||||||
UpdateDynamicNotchFFT();
|
UpdateDynamicNotchFFT();
|
||||||
|
|
||||||
// Apply calibration, rotation, and correct for in-run bias errors
|
Vector3f angular_velocity_unscaled;
|
||||||
Vector3f angular_velocity{_calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias};
|
Vector3f angular_acceleration_unscaled;
|
||||||
|
|
||||||
|
float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
|
||||||
|
|
||||||
for (int axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
#if !defined(CONSTRAINED_FLASH)
|
// copy sensor sample to float array for filtering
|
||||||
|
float data[1] {raw_data_array[axis]};
|
||||||
|
|
||||||
// Apply dynamic notch filter from ESC RPM
|
// save last filtered sample
|
||||||
if (_dynamic_notch_esc_rpm_available) {
|
angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, 1);
|
||||||
perf_begin(_dynamic_notch_filter_esc_rpm_perf);
|
angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, data, 1, dt_s);
|
||||||
|
|
||||||
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
|
|
||||||
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
|
|
||||||
if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
|
|
||||||
dnf[harmonic][axis].applyDF1(&angular_velocity(axis), 1);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
perf_end(_dynamic_notch_filter_esc_rpm_perf);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Apply dynamic notch filter from FFT
|
|
||||||
if (_dynamic_notch_fft_available) {
|
|
||||||
perf_begin(_dynamic_notch_filter_fft_perf);
|
|
||||||
|
|
||||||
for (auto &dnf : _dynamic_notch_filter_fft) {
|
|
||||||
if (dnf[axis].getNotchFreq() > 0.f) {
|
|
||||||
dnf[axis].applyDF1(&angular_velocity(axis), 1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
perf_end(_dynamic_notch_filter_fft_perf);
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // !CONSTRAINED_FLASH
|
|
||||||
|
|
||||||
// Apply general notch filter (IMU_GYRO_NF_FREQ)
|
|
||||||
_notch_filter_velocity[axis].apply(&angular_velocity(axis), 1);
|
|
||||||
|
|
||||||
// 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
|
// Publish
|
||||||
if (!_sensor_sub.updated()) {
|
if (!_sensor_sub.updated()) {
|
||||||
Publish(sensor_data.timestamp_sample);
|
CalibrateAndPublish(sensor_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void VehicleAngularVelocity::Publish(const hrt_abstime ×tamp_sample)
|
void VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sample,
|
||||||
|
const Vector3f &angular_velocity_unscaled, const Vector3f &angular_acceleration_unscaled, float scale)
|
||||||
{
|
{
|
||||||
|
// Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias
|
||||||
|
_angular_velocity = _calibration.Correct(angular_velocity_unscaled * scale) - _bias;
|
||||||
|
|
||||||
|
// Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation
|
||||||
|
_angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * scale;
|
||||||
|
|
||||||
if (timestamp_sample >= _last_publish + _publish_interval_min_us) {
|
if (timestamp_sample >= _last_publish + _publish_interval_min_us) {
|
||||||
|
|
||||||
// Publish vehicle_angular_acceleration
|
// Publish vehicle_angular_acceleration
|
||||||
vehicle_angular_acceleration_s v_angular_acceleration;
|
vehicle_angular_acceleration_s v_angular_acceleration;
|
||||||
v_angular_acceleration.timestamp_sample = timestamp_sample;
|
v_angular_acceleration.timestamp_sample = timestamp_sample;
|
||||||
|
|
|
@ -75,11 +75,17 @@ public:
|
||||||
private:
|
private:
|
||||||
void Run() override;
|
void Run() override;
|
||||||
|
|
||||||
|
void CalibrateAndPublish(const hrt_abstime ×tamp_sample, const matrix::Vector3f &angular_velocity,
|
||||||
|
const matrix::Vector3f &angular_acceleration, float scale = 1.f);
|
||||||
|
|
||||||
|
float FilterAngularVelocity(int axis, float data[], int N);
|
||||||
|
float FilterAngularAcceleration(int axis, float data[], int N, float dt_s);
|
||||||
|
|
||||||
void DisableDynamicNotchEscRpm();
|
void DisableDynamicNotchEscRpm();
|
||||||
void DisableDynamicNotchFFT();
|
void DisableDynamicNotchFFT();
|
||||||
void ParametersUpdate(bool force = false);
|
void ParametersUpdate(bool force = false);
|
||||||
void Publish(const hrt_abstime ×tamp_sample);
|
|
||||||
void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration);
|
void ResetFilters();
|
||||||
void SensorBiasUpdate(bool force = false);
|
void SensorBiasUpdate(bool force = false);
|
||||||
bool SensorSelectionUpdate(bool force = false);
|
bool SensorSelectionUpdate(bool force = false);
|
||||||
void UpdateDynamicNotchEscRpm(bool force = false);
|
void UpdateDynamicNotchEscRpm(bool force = false);
|
||||||
|
@ -88,6 +94,7 @@ private:
|
||||||
|
|
||||||
// scaled appropriately for current FIFO mode
|
// scaled appropriately for current FIFO mode
|
||||||
matrix::Vector3f GetResetAngularVelocity() const;
|
matrix::Vector3f GetResetAngularVelocity() const;
|
||||||
|
matrix::Vector3f GetResetAngularAcceleration() const;
|
||||||
|
|
||||||
static constexpr int MAX_SENSOR_COUNT = 4;
|
static constexpr int MAX_SENSOR_COUNT = 4;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue