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

sensors/vehicle_angular_velocity: use full raw FIFO data (sensor_gyro_fifo) if available

Browse Source
This commit is contained in:
Daniel Agar
2021-02-25 10:06:17 -05:00
committed by GitHub
parent 8f625e5744
commit 6482120d9a
10 changed files with 323 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
boards/nxp/fmurt1062-v1/default.cmake
View File

@@ -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

2
msg/sensor_gyro_fifo.msg
View File

@@ -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

2
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@@ -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};

21
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@@ -33,8 +33,6 @@
#include "LowPassFilter2p.hpp"
#include <px4_platform_common/defines.h>
#include <math.h>
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);

20
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@@ -38,6 +38,8 @@
#pragma once
#include <px4_platform_common/defines.h>
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; }

30
src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp
View File

@@ -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;
}
};

10
src/lib/mathlib/math/filter/NotchFilterArray.hpp
View File

@@ -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)) {

351
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@@ -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<vehicle_imu_status_s> 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_s> 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_s> 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<enum Rotation>(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 &timestamp_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();
}

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

@@ -36,8 +36,9 @@
#include <lib/sensor_calibration/Gyroscope.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/mathlib/math/filter/NotchFilter.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/NotchFilterArray.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@@ -49,6 +50,7 @@
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
@@ -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 &timestamp_sample);
static constexpr int MAX_SENSOR_COUNT = 4;
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::Publication<vehicle_angular_velocity_s> _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<matrix::Vector3f> _notch_filter_velocity{};
math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::NotchFilterArray<float> _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<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
)
};

2
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
View File

@@ -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);