fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 15:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: move accel filtering to sensors/vehicle_acceleration

Browse Source 
I've added a queue depth of 4 for sensor_accel and sensor_gyro. This is initially added because it's not always possible for the `vehicle_acceleration` to keep up with every publication of the primary accelerator as it runs in the same thread as ekf2, various controllers, etc. 

Later this mechanism will be used in a few areas
 - rate limit `vehicle_angular_velocity` and `vehicle_acceleration` without missing any raw data
 - move IMU integration to `vehicle_imu` and out of the actual driver threads, eliminating the need for sensor_accel_integrated and sensor_gyro_integrated
 - integrate raw gyro synchronized with optical flow measurements
This commit is contained in:
Daniel Agar
2020-01-29 16:13:38 -05:00
committed by GitHub
parent 22c422d017
commit 2410b31662
25 changed files with 336 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+35 -65
View File
@@ -65,7 +65,6 @@ static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, u
PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
CDev(nullptr),
ModuleParams(nullptr),
_sensor_pub{ORB_ID(sensor_accel), priority},
_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
@@ -75,10 +74,6 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
_rotation_dcm{get_rot_matrix(rotation)}
{
_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
// set software low pass filter for controllers
updateParams();
ConfigureFilter(_param_imu_accel_cutoff.get());
}
PX4Accelerometer::~PX4Accelerometer()
@@ -123,13 +118,6 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
_device_id = device_id.devid;
}
void PX4Accelerometer::set_sample_rate(uint16_t rate)
{
_sample_rate = rate;
ConfigureFilter(_filter.get_cutoff_freq());
}
void PX4Accelerometer::set_update_rate(uint16_t rate)
{
const uint32_t update_interval = 1000000 / rate;
@@ -154,9 +142,20 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
// Filtered values
const Vector3f val_filtered{_filter.apply(val_calibrated)};
// publish raw data immediately
{
sensor_accel_s report{};
report.timestamp_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);
}
// Integrated values
Vector3f delta_velocity;
@@ -166,21 +165,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl
if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) {
// publish control data (filtered)
{
sensor_accel_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.timestamp = hrt_absolute_time();
_sensor_pub.publish(report);
}
// fill sensor_accel_integrated and publish
sensor_accel_integrated_s report{};
@@ -211,18 +195,31 @@ void PX4Accelerometer::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);
// publish raw data immediately
{
// 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;
// Apply rotation (before scaling)
rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
const Vector3f raw{x_filtered, y_filtered, z_filtered};
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)};
// Apply range scale and the calibrating offset/scale
const Vector3f val_calibrated{((raw * _scale) - _calibration_offset).emult(_calibration_scale)};
sensor_accel_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();
_sensor_pub.publish(report);
}
// clipping
@@ -260,21 +257,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
// publish control data (filtered)
{
sensor_accel_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();
_sensor_pub.publish(report);
}
// Apply rotation and scale
// integrated in microseconds, convert to seconds
const Vector3f delta_velocity_uncalibrated{_rotation_dcm *_integration_raw * _scale};
@@ -341,7 +323,6 @@ void PX4Accelerometer::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.clipping[0] = _clipping[0];
@@ -364,15 +345,6 @@ void PX4Accelerometer::ResetIntegrator()
_timestamp_sample_prev = 0;
}
void PX4Accelerometer::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 PX4Accelerometer::UpdateClipLimit()
{
// 95% of potential max
@@ -391,8 +363,6 @@ void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
void PX4Accelerometer::print_status()
{
PX4_INFO(ACCEL_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("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
(double)_calibration_scale(2));
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+3 -17
View File
@@ -39,16 +39,14 @@
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/integrator.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/PublicationQueuedMulti.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_accel_status.h>
class PX4Accelerometer : public cdev::CDev, public ModuleParams
class PX4Accelerometer : public cdev::CDev
{
public:
PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
@@ -62,7 +60,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,25 +85,18 @@ public:
private:
void ConfigureFilter(float cutoff_freq);
void PublishStatus();
void ResetIntegrator();
void UpdateClipLimit();
void UpdateVibrationMetrics(const matrix::Vector3f &delta_velocity);
uORB::PublicationMulti<sensor_accel_s> _sensor_pub;
uORB::PublicationQueuedMulti<sensor_accel_s> _sensor_pub;
uORB::PublicationMulti<sensor_accel_fifo_s> _sensor_fifo_pub;
uORB::PublicationMulti<sensor_accel_integrated_s> _sensor_integrated_pub;
uORB::PublicationMulti<sensor_accel_status_s> _sensor_status_pub;
math::LowPassFilter2pVector3f _filter{1000, 100};
hrt_abstime _status_last_publish{0};
math::LowPassFilter2pArray _filterArrayX{4000, 100};
math::LowPassFilter2pArray _filterArrayY{4000, 100};
math::LowPassFilter2pArray _filterArrayZ{4000, 100};
Integrator _integrator{4000, false};
matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f};
@@ -131,7 +121,6 @@ private:
uint32_t _clipping[3] {};
uint16_t _sample_rate{1000};
uint16_t _update_rate{1000};
// integrator
@@ -143,7 +132,4 @@ private:
uint8_t _integrator_fifo_samples{0};
uint8_t _integrator_clipping{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
)
};
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+2 -1
View File
@@ -39,6 +39,7 @@
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/integrator.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/PublicationQueuedMulti.hpp>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/sensor_gyro_integrated.h>
@@ -88,7 +89,7 @@ private:
void UpdateClipLimit();
void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle);
uORB::PublicationMulti<sensor_gyro_s> _sensor_pub;
uORB::PublicationQueuedMulti<sensor_gyro_s> _sensor_pub;
uORB::PublicationMulti<sensor_gyro_fifo_s> _sensor_fifo_pub;
uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
uORB::PublicationMulti<sensor_gyro_status_s> _sensor_status_pub;