Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

sensors: move accel filtering to sensors/vehicle_acceleration #14033

Merged
merged 2 commits into from
Jan 29, 2020
Merged

sensors: move accel filtering to sensors/vehicle_acceleration #14033

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
81e5cfa
sensors: move accel filtering to sensors/vehicle_acceleration
dagar Jan 26, 2020
a335a6b
Merge branch 'master' into pr-imu_control_filter_queued
dagar Jan 29, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions msg/sensor_accel.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,5 @@ float32 y # acceleration in the NED Y board axis in m/s^2
float32 z # acceleration in the NED Z board axis in m/s^2

float32 temperature # temperature in degrees celsius

uint8 ORB_QUEUE_LENGTH = 4
1 change: 0 additions & 1 deletion msg/sensor_accel_status.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@ uint8 rotation
uint32[3] clipping # clipping per axis

uint16 measure_rate
uint16 sample_rate

float32 full_scale_range

Expand Down
2 changes: 2 additions & 0 deletions msg/sensor_gyro.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,5 @@ float32 y # angular velocity in the NED Y board axis in rad/s
float32 z # angular velocity in the NED Z board axis in rad/s

float32 temperature # temperature in degrees celsius

uint8 ORB_QUEUE_LENGTH = 4
2 changes: 0 additions & 2 deletions src/drivers/imu/adis16448/ADIS16448.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,8 +258,6 @@ ADIS16448::set_sample_rate(uint16_t desired_sample_rate_hz)
return false;
}

_px4_accel.set_sample_rate(desired_sample_rate_hz);

return true;
}

Expand Down
1 change: 0 additions & 1 deletion src/drivers/imu/adis16477/ADIS16477.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,6 @@ ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) :
#endif // GPIO_SPI1_RESET_ADIS16477

_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16477);
_px4_accel.set_sample_rate(ADIS16477_DEFAULT_RATE);
_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);
Expand Down
1 change: 0 additions & 1 deletion src/drivers/imu/adis16497/ADIS16497.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,6 @@ ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
#endif // GPIO_SPI1_RESET_ADIS16497

_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16497);
_px4_accel.set_sample_rate(ADIS16497_DEFAULT_RATE);

_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497);
}
Expand Down
3 changes: 0 additions & 3 deletions src/drivers/imu/bmi160/bmi160.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,10 +64,7 @@ BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) :
_duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates"))
{
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_BMI160);
_px4_accel.set_sample_rate(BMI160_ACCEL_DEFAULT_RATE);

_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_BMI160);
_px4_accel.set_sample_rate(BMI160_GYRO_DEFAULT_RATE);
}

BMI160::~BMI160()
Expand Down
2 changes: 0 additions & 2 deletions src/drivers/imu/invensense/icm20602/ICM20602.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,6 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602);
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602);

_px4_accel.set_sample_rate(ACCEL_RATE);

_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
}
Expand Down
2 changes: 0 additions & 2 deletions src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,6 @@ ICM20608G::ICM20608G(int bus, uint32_t device, enum Rotation rotation) :
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20608);
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20608);

_px4_accel.set_sample_rate(ACCEL_RATE);

_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
}
Expand Down
1 change: 0 additions & 1 deletion src/drivers/imu/lsm303d/LSM303D.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -363,7 +363,6 @@ LSM303D::accel_set_samplerate(unsigned frequency)
}

_call_accel_interval = 1000000 / accel_samplerate;
_px4_accel.set_sample_rate(accel_samplerate);

modify_reg(ADDR_CTRL_REG1, clearbits, setbits);

Expand Down
2 changes: 0 additions & 2 deletions src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,6 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
_px4_accel.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
_px4_gyro.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);

_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);

_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
}
Expand Down
2 changes: 0 additions & 2 deletions src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,6 @@ LSM9DS1::LSM9DS1(int bus, uint32_t device, enum Rotation rotation) :
_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);
_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);

_px4_accel.set_sample_rate(ST_LSM9DS1::LA_ODR);

_px4_accel.set_update_rate(1000000 / _fifo_interval);
_px4_gyro.set_update_rate(1000000 / _fifo_interval);
}
Expand Down
100 changes: 35 additions & 65 deletions src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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},
Expand All @@ -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()
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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{};

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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};
Expand Down Expand Up @@ -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];
Expand All @@ -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
Expand All @@ -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));
Expand Down
20 changes: 3 additions & 17 deletions src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -62,7 +60,6 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
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);
Expand All @@ -88,25 +85,18 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams

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};
Expand All @@ -131,7 +121,6 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams

uint32_t _clipping[3] {};

uint16_t _sample_rate{1000};
uint16_t _update_rate{1000};

// integrator
Expand All @@ -143,7 +132,4 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
uint8_t _integrator_fifo_samples{0};
uint8_t _integrator_clipping{0};

DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
)
};
3 changes: 2 additions & 1 deletion src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -88,7 +89,7 @@ class PX4Gyroscope : public cdev::CDev
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;
Expand Down
14 changes: 0 additions & 14 deletions src/modules/sensors/sensor_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -212,17 +212,3 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
* @group Sensors
*/
PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);

/**
* Driver level cutoff frequency for accel
*
* The cutoff frequency for the 2nd order butterworth filter on the accel 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_ACCEL_CUTOFF, 30.0f);
Loading