mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
sensors: move gyro filtering to sensors/vehicle_angular_velocity
- gyro filtering (low-pass and notch) only performed on primary gyro in `sensors/vehicle_angular_velocity` instead of every gyro in `PX4Gyroscope` - sample rate is calculated from actual updates (the fixed value was slightly wrong in many cases, and very wrong in a few) - In the FIFO case the array is now averaged and published in `sensor_gyro` for filtering downstream. I'll update this in the future to use the full FIFO array (if available), but right now it should be fine.
This commit is contained in:
Daniel Agar
GitHub
parent
0d2e5f1c50
commit
24f0c2d72a
@ -11,7 +11,6 @@ uint8 rotation
|
||||
uint32[3] clipping # clipping per axis
|
||||
|
||||
uint16 measure_rate
|
||||
uint16 sample_rate
|
||||
|
||||
float32 full_scale_range
|
||||
|
||||
|
||||
@ -50,13 +50,13 @@ namespace wq_configurations
|
||||
{
|
||||
static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority
|
||||
|
||||
static constexpr wq_config_t SPI0{"wq:SPI0", 1600, -1};
|
||||
static constexpr wq_config_t SPI1{"wq:SPI1", 1600, -2};
|
||||
static constexpr wq_config_t SPI2{"wq:SPI2", 1616, -3};
|
||||
static constexpr wq_config_t SPI3{"wq:SPI3", 1600, -4};
|
||||
static constexpr wq_config_t SPI4{"wq:SPI4", 1600, -5};
|
||||
static constexpr wq_config_t SPI5{"wq:SPI5", 1600, -6};
|
||||
static constexpr wq_config_t SPI6{"wq:SPI6", 1600, -7};
|
||||
static constexpr wq_config_t SPI0{"wq:SPI0", 1900, -1};
|
||||
static constexpr wq_config_t SPI1{"wq:SPI1", 1900, -2};
|
||||
static constexpr wq_config_t SPI2{"wq:SPI2", 1900, -3};
|
||||
static constexpr wq_config_t SPI3{"wq:SPI3", 1900, -4};
|
||||
static constexpr wq_config_t SPI4{"wq:SPI4", 1900, -5};
|
||||
static constexpr wq_config_t SPI5{"wq:SPI5", 1900, -6};
|
||||
static constexpr wq_config_t SPI6{"wq:SPI6", 1900, -7};
|
||||
|
||||
static constexpr wq_config_t I2C0{"wq:I2C0", 1400, -8};
|
||||
static constexpr wq_config_t I2C1{"wq:I2C1", 1400, -9};
|
||||
|
||||
@ -259,7 +259,6 @@ ADIS16448::set_sample_rate(uint16_t desired_sample_rate_hz)
|
||||
}
|
||||
|
||||
_px4_accel.set_sample_rate(desired_sample_rate_hz);
|
||||
_px4_gyro.set_sample_rate(desired_sample_rate_hz);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -71,7 +71,6 @@ ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) :
|
||||
_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
|
||||
|
||||
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16477);
|
||||
_px4_gyro.set_sample_rate(ADIS16477_DEFAULT_RATE);
|
||||
_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
|
||||
}
|
||||
|
||||
|
||||
@ -87,7 +87,6 @@ ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
|
||||
_px4_accel.set_sample_rate(ADIS16497_DEFAULT_RATE);
|
||||
|
||||
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497);
|
||||
_px4_gyro.set_sample_rate(ADIS16497_DEFAULT_RATE);
|
||||
}
|
||||
|
||||
ADIS16497::~ADIS16497()
|
||||
|
||||
@ -429,8 +429,6 @@ FXAS21002C::set_samplerate(unsigned frequency)
|
||||
modify_reg(FXAS21002C_CTRL_REG1, CTRL_REG1_DR_MASK, bits);
|
||||
set_standby(_current_rate, false);
|
||||
|
||||
_px4_gyro.set_sample_rate(_current_rate);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
@ -59,7 +59,6 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
|
||||
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602);
|
||||
|
||||
_px4_accel.set_sample_rate(ACCEL_RATE);
|
||||
_px4_gyro.set_sample_rate(GYRO_RATE);
|
||||
|
||||
_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
|
||||
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
|
||||
|
||||
@ -59,7 +59,6 @@ ICM20608G::ICM20608G(int bus, uint32_t device, enum Rotation rotation) :
|
||||
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20608);
|
||||
|
||||
_px4_accel.set_sample_rate(ACCEL_RATE);
|
||||
_px4_gyro.set_sample_rate(GYRO_RATE);
|
||||
|
||||
_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
|
||||
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
|
||||
|
||||
@ -649,13 +649,6 @@ MPU6000::stop()
|
||||
memset(_last_accel, 0, sizeof(_last_accel));
|
||||
}
|
||||
|
||||
void
|
||||
MPU6000::Run()
|
||||
{
|
||||
/* make another measurement */
|
||||
measure();
|
||||
}
|
||||
|
||||
void
|
||||
MPU6000::check_registers(void)
|
||||
{
|
||||
@ -717,17 +710,16 @@ MPU6000::check_registers(void)
|
||||
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
|
||||
}
|
||||
|
||||
int
|
||||
MPU6000::measure()
|
||||
void MPU6000::Run()
|
||||
{
|
||||
if (_in_factory_test) {
|
||||
// don't publish any data while in factory test mode
|
||||
return OK;
|
||||
return;
|
||||
}
|
||||
|
||||
if (hrt_absolute_time() < _reset_wait) {
|
||||
// we're waiting for a reset to complete
|
||||
return OK;
|
||||
return;
|
||||
}
|
||||
|
||||
struct MPUReport mpu_report;
|
||||
@ -756,7 +748,8 @@ MPU6000::measure()
|
||||
if (sizeof(mpu_report) != _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED),
|
||||
(uint8_t *)&mpu_report, sizeof(mpu_report))) {
|
||||
|
||||
return -EIO;
|
||||
perf_end(_sample_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
check_registers();
|
||||
@ -774,9 +767,11 @@ MPU6000::measure()
|
||||
perf_end(_sample_perf);
|
||||
perf_count(_duplicates);
|
||||
_got_duplicate = true;
|
||||
return OK;
|
||||
return;
|
||||
}
|
||||
|
||||
perf_end(_sample_perf);
|
||||
|
||||
memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6);
|
||||
_got_duplicate = false;
|
||||
|
||||
@ -804,20 +799,19 @@ MPU6000::measure()
|
||||
|
||||
// all zero data - probably a SPI bus error
|
||||
perf_count(_bad_transfers);
|
||||
perf_end(_sample_perf);
|
||||
|
||||
// note that we don't call reset() here as a reset()
|
||||
// costs 20ms with interrupts disabled. That means if
|
||||
// the mpu6k does go bad it would cause a FMU failure,
|
||||
// regardless of whether another sensor is available,
|
||||
return -EIO;
|
||||
return;
|
||||
}
|
||||
|
||||
if (_register_wait != 0) {
|
||||
// we are waiting for some good transfers before using
|
||||
// the sensor again, don't return any data yet
|
||||
_register_wait--;
|
||||
return OK;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@ -876,10 +870,6 @@ MPU6000::measure()
|
||||
|
||||
_px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
|
||||
_px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
|
||||
|
||||
/* stop measuring */
|
||||
perf_end(_sample_perf);
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@ -394,11 +394,6 @@ private:
|
||||
*/
|
||||
bool is_mpu_device() { return _device_type == MPU_DEVICE_TYPE_MPU6000; }
|
||||
|
||||
/**
|
||||
* Fetch measurements from the sensor and update the report buffers.
|
||||
*/
|
||||
int measure();
|
||||
|
||||
/**
|
||||
* Read a register from the MPU6000
|
||||
*
|
||||
|
||||
@ -53,7 +53,6 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
|
||||
_px4_gyro.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
|
||||
|
||||
_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);
|
||||
_px4_gyro.set_sample_rate(ST_ISM330DLC::G_ODR);
|
||||
|
||||
_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
|
||||
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
|
||||
|
||||
@ -49,7 +49,6 @@ LSM9DS1::LSM9DS1(int bus, uint32_t device, enum Rotation rotation) :
|
||||
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);
|
||||
|
||||
_px4_accel.set_sample_rate(ST_LSM9DS1::LA_ODR);
|
||||
_px4_gyro.set_sample_rate(ST_LSM9DS1::G_ODR);
|
||||
|
||||
_px4_accel.set_update_rate(1000000 / _fifo_interval);
|
||||
_px4_gyro.set_update_rate(1000000 / _fifo_interval);
|
||||
|
||||
@ -65,7 +65,6 @@ static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, u
|
||||
|
||||
PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
|
||||
CDev(nullptr),
|
||||
ModuleParams(nullptr),
|
||||
_sensor_pub{ORB_ID(sensor_gyro), priority},
|
||||
_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
|
||||
_sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority},
|
||||
@ -75,11 +74,6 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
|
||||
_rotation_dcm{get_rot_matrix(rotation)}
|
||||
{
|
||||
_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
|
||||
|
||||
// set software low pass filter for controllers
|
||||
updateParams();
|
||||
ConfigureFilter(_param_imu_gyro_cutoff.get());
|
||||
ConfigureNotchFilter(_param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
|
||||
}
|
||||
|
||||
PX4Gyroscope::~PX4Gyroscope()
|
||||
@ -123,14 +117,6 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
|
||||
_device_id = device_id.devid;
|
||||
}
|
||||
|
||||
void PX4Gyroscope::set_sample_rate(uint16_t rate)
|
||||
{
|
||||
_sample_rate = rate;
|
||||
|
||||
ConfigureFilter(_filter.get_cutoff_freq());
|
||||
ConfigureNotchFilter(_notch_filter.getNotchFreq(), _notch_filter.getBandwidth());
|
||||
}
|
||||
|
||||
void PX4Gyroscope::set_update_rate(uint16_t rate)
|
||||
{
|
||||
const uint32_t update_interval = 1000000 / rate;
|
||||
@ -155,21 +141,16 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
|
||||
// Apply range scale and the calibrating offset/scale
|
||||
const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
|
||||
|
||||
// Filtered values: apply notch and then low-pass
|
||||
Vector3f val_filtered{_notch_filter.apply(val_calibrated)};
|
||||
val_filtered = _filter.apply(val_filtered);
|
||||
|
||||
|
||||
// publish control data (filtered) immediately
|
||||
// publish raw data immediately
|
||||
{
|
||||
sensor_gyro_s report{};
|
||||
|
||||
report.timestamp_sample = timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.x = val_filtered(0);
|
||||
report.y = val_filtered(1);
|
||||
report.z = val_filtered(2);
|
||||
report.x = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
@ -213,45 +194,30 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
|
||||
const uint8_t N = sample.samples;
|
||||
const float dt = sample.dt;
|
||||
|
||||
// filtered data (control)
|
||||
float x_filtered = _filterArrayX.apply(sample.x, N);
|
||||
float y_filtered = _filterArrayY.apply(sample.y, N);
|
||||
float z_filtered = _filterArrayZ.apply(sample.z, N);
|
||||
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
|
||||
|
||||
// Apply range scale and the calibration offset
|
||||
const Vector3f val_calibrated{(Vector3f{x_filtered, y_filtered, z_filtered} * _scale) - _calibration_offset};
|
||||
|
||||
|
||||
// publish control data (filtered) immediately
|
||||
// publish raw data immediately
|
||||
{
|
||||
bool publish_control = true;
|
||||
// average
|
||||
float x = (float)sum(sample.x, N) / (float)N;
|
||||
float y = (float)sum(sample.y, N) / (float)N;
|
||||
float z = (float)sum(sample.z, N) / (float)N;
|
||||
|
||||
if (_param_imu_gyro_rate_max.get() > 0) {
|
||||
const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
|
||||
// Apply rotation (before scaling)
|
||||
rotate_3f(_rotation, x, y, z);
|
||||
|
||||
if (hrt_elapsed_time(&_control_last_publish) < interval) {
|
||||
publish_control = false;
|
||||
}
|
||||
}
|
||||
// Apply range scale and the calibration offset
|
||||
const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset};
|
||||
|
||||
if (publish_control) {
|
||||
sensor_gyro_s report{};
|
||||
sensor_gyro_s report{};
|
||||
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.x = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.timestamp_sample = sample.timestamp_sample;
|
||||
report.device_id = _device_id;
|
||||
report.temperature = _temperature;
|
||||
report.x = val_calibrated(0);
|
||||
report.y = val_calibrated(1);
|
||||
report.z = val_calibrated(2);
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_pub.publish(report);
|
||||
|
||||
_control_last_publish = report.timestamp_sample;
|
||||
}
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
|
||||
@ -356,7 +322,6 @@ void PX4Gyroscope::PublishStatus()
|
||||
status.full_scale_range = _range;
|
||||
status.rotation = _rotation;
|
||||
status.measure_rate = _update_rate;
|
||||
status.sample_rate = _sample_rate;
|
||||
status.temperature = _temperature;
|
||||
status.vibration_metric = _vibration_metric;
|
||||
status.coning_vibration = _coning_vibration;
|
||||
@ -380,20 +345,6 @@ void PX4Gyroscope::ResetIntegrator()
|
||||
_timestamp_sample_prev = 0;
|
||||
}
|
||||
|
||||
void PX4Gyroscope::ConfigureFilter(float cutoff_freq)
|
||||
{
|
||||
_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
|
||||
|
||||
_filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq);
|
||||
_filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq);
|
||||
_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
|
||||
{
|
||||
_notch_filter.setParameters(_sample_rate, notch_freq, bandwidth);
|
||||
}
|
||||
|
||||
void PX4Gyroscope::UpdateClipLimit()
|
||||
{
|
||||
// 95% of potential max
|
||||
@ -417,10 +368,6 @@ void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
|
||||
void PX4Gyroscope::print_status()
|
||||
{
|
||||
PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
|
||||
PX4_INFO("sample rate: %d Hz", _sample_rate);
|
||||
PX4_INFO("filter cutoff: %.3f Hz", (double)_filter.get_cutoff_freq());
|
||||
PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter.getNotchFreq(),
|
||||
(double)_notch_filter.getBandwidth());
|
||||
|
||||
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
|
||||
(double)_calibration_offset(2));
|
||||
|
||||
@ -38,17 +38,13 @@
|
||||
#include <lib/cdev/CDev.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/drivers/device/integrator.h>
|
||||
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
|
||||
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
|
||||
#include <lib/mathlib/math/filter/NotchFilter.hpp>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/sensor_gyro_fifo.h>
|
||||
#include <uORB/topics/sensor_gyro_integrated.h>
|
||||
#include <uORB/topics/sensor_gyro_status.h>
|
||||
|
||||
class PX4Gyroscope : public cdev::CDev, public ModuleParams
|
||||
class PX4Gyroscope : public cdev::CDev
|
||||
{
|
||||
public:
|
||||
PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
|
||||
@ -62,7 +58,6 @@ public:
|
||||
void set_device_type(uint8_t devtype);
|
||||
void set_error_count(uint64_t error_count) { _error_count += error_count; }
|
||||
void set_range(float range) { _range = range; UpdateClipLimit(); }
|
||||
void set_sample_rate(uint16_t rate);
|
||||
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
|
||||
void set_temperature(float temperature) { _temperature = temperature; }
|
||||
void set_update_rate(uint16_t rate);
|
||||
@ -88,8 +83,6 @@ public:
|
||||
|
||||
private:
|
||||
|
||||
void ConfigureFilter(float cutoff_freq);
|
||||
void ConfigureNotchFilter(float notch_freq, float bandwidth);
|
||||
void PublishStatus();
|
||||
void ResetIntegrator();
|
||||
void UpdateClipLimit();
|
||||
@ -100,16 +93,8 @@ private:
|
||||
uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
|
||||
uORB::PublicationMulti<sensor_gyro_status_s> _sensor_status_pub;
|
||||
|
||||
math::LowPassFilter2pVector3f _filter{1000, 100};
|
||||
math::NotchFilter<matrix::Vector3f> _notch_filter{};
|
||||
|
||||
hrt_abstime _control_last_publish{0};
|
||||
hrt_abstime _status_last_publish{0};
|
||||
|
||||
math::LowPassFilter2pArray _filterArrayX{8000, 100};
|
||||
math::LowPassFilter2pArray _filterArrayY{8000, 100};
|
||||
math::LowPassFilter2pArray _filterArrayZ{8000, 100};
|
||||
|
||||
Integrator _integrator{4000, true};
|
||||
|
||||
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
|
||||
@ -134,7 +119,6 @@ private:
|
||||
|
||||
uint32_t _clipping[3] {};
|
||||
|
||||
uint16_t _sample_rate{1000};
|
||||
uint16_t _update_rate{1000};
|
||||
|
||||
// integrator
|
||||
@ -146,10 +130,4 @@ private:
|
||||
uint8_t _integrator_fifo_samples{0};
|
||||
uint8_t _integrator_clipping{0};
|
||||
|
||||
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
|
||||
)
|
||||
};
|
||||
|
||||
@ -38,6 +38,8 @@
|
||||
|
||||
#include "LowPassFilter2p.hpp"
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
|
||||
@ -207,47 +207,9 @@ perf_count(perf_counter_t handle)
|
||||
((struct perf_ctr_count *)handle)->event_count++;
|
||||
break;
|
||||
|
||||
case PC_INTERVAL: {
|
||||
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
switch (pci->event_count) {
|
||||
case 0:
|
||||
pci->time_first = now;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
pci->time_least = (uint32_t)(now - pci->time_last);
|
||||
pci->time_most = (uint32_t)(now - pci->time_last);
|
||||
pci->mean = pci->time_least / 1e6f;
|
||||
pci->M2 = 0;
|
||||
break;
|
||||
|
||||
default: {
|
||||
hrt_abstime interval = now - pci->time_last;
|
||||
|
||||
if ((uint32_t)interval < pci->time_least) {
|
||||
pci->time_least = (uint32_t)interval;
|
||||
}
|
||||
|
||||
if ((uint32_t)interval > pci->time_most) {
|
||||
pci->time_most = (uint32_t)interval;
|
||||
}
|
||||
|
||||
// maintain mean and variance of interval in seconds
|
||||
// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
|
||||
float dt = interval / 1e6f;
|
||||
float delta_intvl = dt - pci->mean;
|
||||
pci->mean += delta_intvl / pci->event_count;
|
||||
pci->M2 += delta_intvl * (dt - pci->mean);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
pci->time_last = now;
|
||||
pci->event_count++;
|
||||
break;
|
||||
}
|
||||
case PC_INTERVAL:
|
||||
perf_count_interval(handle, hrt_absolute_time());
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
@ -357,6 +319,60 @@ perf_set_elapsed(perf_counter_t handle, int64_t elapsed)
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
perf_count_interval(perf_counter_t handle, hrt_abstime now)
|
||||
{
|
||||
if (handle == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (handle->type) {
|
||||
case PC_INTERVAL: {
|
||||
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
|
||||
|
||||
switch (pci->event_count) {
|
||||
case 0:
|
||||
pci->time_first = now;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
pci->time_least = (uint32_t)(now - pci->time_last);
|
||||
pci->time_most = (uint32_t)(now - pci->time_last);
|
||||
pci->mean = pci->time_least / 1e6f;
|
||||
pci->M2 = 0;
|
||||
break;
|
||||
|
||||
default: {
|
||||
hrt_abstime interval = now - pci->time_last;
|
||||
|
||||
if ((uint32_t)interval < pci->time_least) {
|
||||
pci->time_least = (uint32_t)interval;
|
||||
}
|
||||
|
||||
if ((uint32_t)interval > pci->time_most) {
|
||||
pci->time_most = (uint32_t)interval;
|
||||
}
|
||||
|
||||
// maintain mean and variance of interval in seconds
|
||||
// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
|
||||
float dt = interval / 1e6f;
|
||||
float delta_intvl = dt - pci->mean;
|
||||
pci->mean += delta_intvl / pci->event_count;
|
||||
pci->M2 += delta_intvl * (dt - pci->mean);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
pci->time_last = now;
|
||||
pci->event_count++;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
perf_set_count(perf_counter_t handle, uint64_t count)
|
||||
{
|
||||
@ -396,8 +412,6 @@ perf_cancel(perf_counter_t handle)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void
|
||||
perf_reset(perf_counter_t handle)
|
||||
{
|
||||
@ -571,6 +585,31 @@ perf_event_count(perf_counter_t handle)
|
||||
return 0;
|
||||
}
|
||||
|
||||
float
|
||||
perf_mean(perf_counter_t handle)
|
||||
{
|
||||
if (handle == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
switch (handle->type) {
|
||||
case PC_ELAPSED: {
|
||||
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
|
||||
return pce->mean;
|
||||
}
|
||||
|
||||
case PC_INTERVAL: {
|
||||
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
|
||||
return pci->mean;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
void
|
||||
perf_iterate_all(perf_callback cb, void *user)
|
||||
{
|
||||
|
||||
@ -132,6 +132,18 @@ __EXPORT extern void perf_end(perf_counter_t handle);
|
||||
*/
|
||||
__EXPORT extern void perf_set_elapsed(perf_counter_t handle, int64_t elapsed);
|
||||
|
||||
/**
|
||||
* Register a measurement
|
||||
*
|
||||
* This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
|
||||
* If a call is made without a corresponding perf_begin call. It sets the
|
||||
* value provided as argument as a new measurement.
|
||||
*
|
||||
* @param handle The handle returned from perf_alloc.
|
||||
* @param time The time for the interval.
|
||||
*/
|
||||
__EXPORT extern void perf_count_interval(perf_counter_t handle, uint64_t time);
|
||||
|
||||
/**
|
||||
* Set a counter
|
||||
*
|
||||
@ -228,6 +240,14 @@ __EXPORT extern void perf_reset_all(void);
|
||||
*/
|
||||
__EXPORT extern uint64_t perf_event_count(perf_counter_t handle);
|
||||
|
||||
/**
|
||||
* Return current mean
|
||||
*
|
||||
* @param handle The handle returned from perf_alloc.
|
||||
* @param return mean
|
||||
*/
|
||||
__EXPORT extern float perf_mean(perf_counter_t handle);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
#endif
|
||||
|
||||
@ -213,70 +213,6 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
|
||||
|
||||
/**
|
||||
* Driver level notch frequency for gyro
|
||||
*
|
||||
* The center frequency for the 2nd order notch filter on the gyro driver.
|
||||
* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency.
|
||||
* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
|
||||
* See "IMU_GYRO_NF_BW" to set the bandwidth of the filter.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @unit Hz
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
|
||||
|
||||
/**
|
||||
* Driver level notch bandwidth for gyro
|
||||
*
|
||||
* The frequency width of the stop band for the 2nd order notch filter on the gyro driver.
|
||||
* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency.
|
||||
*
|
||||
* @min 0
|
||||
* @max 100
|
||||
* @unit Hz
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
|
||||
|
||||
/**
|
||||
* Driver level cutoff frequency for gyro
|
||||
*
|
||||
* The cutoff frequency for the 2nd order butterworth filter on the gyro driver.
|
||||
* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @unit Hz
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
|
||||
|
||||
/**
|
||||
* Gyro control data maximum publication rate
|
||||
*
|
||||
* This is the maximum rate the gyro control data (sensor_gyro) will be allowed to publish at.
|
||||
* Set to 0 to disable and publish at the native sensor sample rate.
|
||||
*
|
||||
* @min 0
|
||||
* @max 2000
|
||||
* @value 0 0 (no limit)
|
||||
* @value 50 50 Hz
|
||||
* @value 250 250 Hz
|
||||
* @value 400 400 Hz
|
||||
* @value 1000 1000 Hz
|
||||
* @value 2000 2000 Hz
|
||||
* @unit Hz
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
|
||||
|
||||
/**
|
||||
* Driver level cutoff frequency for accel
|
||||
*
|
||||
|
||||
@ -43,11 +43,15 @@ 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());
|
||||
}
|
||||
|
||||
VehicleAngularVelocity::~VehicleAngularVelocity()
|
||||
{
|
||||
Stop();
|
||||
|
||||
perf_free(_interval_perf);
|
||||
}
|
||||
|
||||
bool VehicleAngularVelocity::Start()
|
||||
@ -77,6 +81,50 @@ void VehicleAngularVelocity::Stop()
|
||||
_sensor_selection_sub.unregisterCallback();
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::CheckFilters(const Vector3f &rates)
|
||||
{
|
||||
if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
|
||||
_filter_check_last = hrt_absolute_time();
|
||||
|
||||
// calculate sensor update rate
|
||||
const float sample_interval_avg = perf_mean(_interval_perf);
|
||||
|
||||
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
|
||||
|
||||
const float update_rate_hz = 1.0f / sample_interval_avg;
|
||||
|
||||
if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
|
||||
_update_rate_hz = update_rate_hz;
|
||||
|
||||
// check if sample rate error is greater than 1%
|
||||
if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
|
||||
++_sample_rate_incorrect_count;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
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);
|
||||
|
||||
_notch_filter.setParameters(_filter_sample_rate, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
|
||||
_notch_filter.reset(rates);
|
||||
|
||||
// reset state
|
||||
_sample_rate_incorrect_count = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void VehicleAngularVelocity::SensorBiasUpdate(bool force)
|
||||
{
|
||||
if (_estimator_sensor_bias_sub.updated() || force) {
|
||||
@ -165,6 +213,9 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
|
||||
// force corrections reselection
|
||||
_corrections_selected_instance = -1;
|
||||
|
||||
// reset sample rate monitor
|
||||
_sample_rate_incorrect_count = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@ -205,15 +256,23 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
|
||||
void VehicleAngularVelocity::Run()
|
||||
{
|
||||
// update corrections first to set _selected_sensor
|
||||
bool sensor_select_update = SensorSelectionUpdate();
|
||||
SensorCorrectionsUpdate(sensor_select_update);
|
||||
SensorBiasUpdate(sensor_select_update);
|
||||
bool selection_updated = SensorSelectionUpdate();
|
||||
|
||||
SensorCorrectionsUpdate(selection_updated);
|
||||
SensorBiasUpdate(selection_updated);
|
||||
ParametersUpdate();
|
||||
|
||||
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
|
||||
bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
|
||||
|
||||
if (sensor_updated || selection_updated) {
|
||||
sensor_gyro_s sensor_data;
|
||||
|
||||
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
|
||||
|
||||
if (sensor_updated) {
|
||||
perf_count_interval(_interval_perf, 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};
|
||||
|
||||
@ -226,14 +285,32 @@ void VehicleAngularVelocity::Run()
|
||||
// correct for in-run bias errors
|
||||
rates -= _bias;
|
||||
|
||||
// publish
|
||||
vehicle_angular_velocity_s out;
|
||||
// Filter: apply notch and then low-pass
|
||||
CheckFilters(rates);
|
||||
const Vector3f rates_filtered = _lowpass_filter.apply(_notch_filter.apply(rates));
|
||||
|
||||
out.timestamp_sample = sensor_data.timestamp_sample;
|
||||
rates.copyTo(out.xyz);
|
||||
out.timestamp = hrt_absolute_time();
|
||||
|
||||
_vehicle_angular_velocity_pub.publish(out);
|
||||
bool publish = true;
|
||||
|
||||
if (_param_imu_gyro_rate_max.get() > 0) {
|
||||
const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
|
||||
|
||||
if (hrt_elapsed_time(&_last_publish) < interval) {
|
||||
publish = false;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -244,4 +321,12 @@ void VehicleAngularVelocity::PrintStatus()
|
||||
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
|
||||
PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
|
||||
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());
|
||||
|
||||
if (_notch_filter.getNotchFreq() > 0.0f) {
|
||||
PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter.getNotchFreq(),
|
||||
(double)_notch_filter.getBandwidth());
|
||||
}
|
||||
}
|
||||
|
||||
@ -36,6 +36,9 @@
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/mathlib/math/Limits.hpp>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
|
||||
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
|
||||
#include <lib/mathlib/math/filter/NotchFilter.hpp>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
@ -65,6 +68,7 @@ public:
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
void CheckFilters(const matrix::Vector3f &rates);
|
||||
void ParametersUpdate(bool force = false);
|
||||
void SensorBiasUpdate(bool force = false);
|
||||
void SensorCorrectionsUpdate(bool force = false);
|
||||
@ -73,6 +77,11 @@ private:
|
||||
static constexpr int MAX_SENSOR_COUNT = 3;
|
||||
|
||||
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::SENS_BOARD_ROT>) _param_sens_board_rot,
|
||||
|
||||
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
|
||||
@ -93,12 +102,23 @@ private:
|
||||
{this, ORB_ID(sensor_gyro), 2}
|
||||
};
|
||||
|
||||
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
|
||||
|
||||
matrix::Dcmf _board_rotation;
|
||||
|
||||
matrix::Vector3f _bias{0.f, 0.f, 0.f};
|
||||
matrix::Vector3f _offset{0.f, 0.f, 0.f};
|
||||
matrix::Vector3f _scale{1.f, 1.f, 1.f};
|
||||
|
||||
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{};
|
||||
float _filter_sample_rate{kInitialRateHz};
|
||||
int _sample_rate_incorrect_count{0};
|
||||
|
||||
uint32_t _selected_sensor_device_id{0};
|
||||
uint8_t _selected_sensor_sub_index{0};
|
||||
int8_t _corrections_selected_instance{-1};
|
||||
|
||||
@ -0,0 +1,96 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Driver level notch frequency for gyro
|
||||
*
|
||||
* The center frequency for the 2nd order notch filter on the gyro driver.
|
||||
* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency.
|
||||
* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
|
||||
* See "IMU_GYRO_NF_BW" to set the bandwidth of the filter.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @unit Hz
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
|
||||
|
||||
/**
|
||||
* Driver level notch bandwidth for gyro
|
||||
*
|
||||
* The frequency width of the stop band for the 2nd order notch filter on the gyro driver.
|
||||
* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency.
|
||||
*
|
||||
* @min 0
|
||||
* @max 100
|
||||
* @unit Hz
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
|
||||
|
||||
/**
|
||||
* Driver level cutoff frequency for gyro
|
||||
*
|
||||
* The cutoff frequency for the 2nd order butterworth filter on the gyro driver.
|
||||
* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @unit Hz
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
|
||||
|
||||
/**
|
||||
* Gyro control data maximum publication rate
|
||||
*
|
||||
* This is the maximum rate the gyro control data (sensor_gyro) will be allowed to publish at.
|
||||
* Set to 0 to disable and publish at the native sensor sample rate.
|
||||
*
|
||||
* @min 0
|
||||
* @max 2000
|
||||
* @value 0 0 (no limit)
|
||||
* @value 50 50 Hz
|
||||
* @value 250 250 Hz
|
||||
* @value 400 400 Hz
|
||||
* @value 1000 1000 Hz
|
||||
* @value 2000 2000 Hz
|
||||
* @unit Hz
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
|
||||
@ -104,7 +104,6 @@ private:
|
||||
Simulator() : ModuleParams(nullptr)
|
||||
{
|
||||
_px4_accel.set_sample_rate(250);
|
||||
_px4_gyro.set_sample_rate(250);
|
||||
}
|
||||
|
||||
~Simulator()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user