Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: replaced INS_MPU6K_FILTER with INS_ACCEL_FILTER and INS_GYRO_FILTER

Browse Source 
this allows filtering to be set separately on accels and gyros where possible
This commit is contained in:
Andrew Tridgell 2015-03-12 11:58:36 +11:00
parent f3314791f2
commit 3d7d46b9b0
16 changed files with 168 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -6,6 +6,7 @@
#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_Notify.h>
#include <AP_Vehicle.h>
/*
enable TIMING_DEBUG to track down scheduling issues with the main
@ -22,6 +23,16 @@
extern const AP_HAL::HAL& hal;
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
#define DEFAULT_GYRO_FILTER 30
#define DEFAULT_ACCEL_FILTER 30
#elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
#define DEFAULT_GYRO_FILTER 10
#define DEFAULT_ACCEL_FILTER 10
#else
#define DEFAULT_GYRO_FILTER 20
#define DEFAULT_ACCEL_FILTER 20
#endif
#define SAMPLE_UNIT 1
@ -41,6 +52,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
ACCSCAL : 1
ACCOFFS : 2
MPU6K_FILTER: 4
ACC2SCAL : 5
ACC2OFFS : 6
ACC3SCAL : 8
@ -67,14 +79,6 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("GYROFFS", 3, AP_InertialSensor, _gyro_offset[0], 0),
// @Param: MPU6K_FILTER
// @DisplayName: MPU6000 filter frequency
// @Description: Filter frequency to ask the MPU6000 to apply to samples. This can be set to a lower value to try to cope with very high vibration levels in aircraft. The default value on ArduPlane, APMrover2 and ArduCopter is 20Hz. This option takes effect on the next reboot or gyro initialisation
// @Units: Hz
// @Values: 0:Default,5:5Hz,10:10Hz,20:20Hz,42:42Hz,98:98Hz
// @User: Advanced
AP_GROUPINFO("MPU6K_FILTER", 4, AP_InertialSensor, _mpu6000_filter, 0),
#if INS_MAX_INSTANCES > 1
// @Param: GYR2OFFS_X
// @DisplayName: Gyro2 offsets of X axis
@ -244,6 +248,22 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
AP_GROUPINFO("ACC3OFFS", 17, AP_InertialSensor, _accel_offset[2], 0),
#endif
// @Param: GYRO_FILTER
// @DisplayName: Gyro filter cutoff frequency
// @Description: Filter cutoff frequency for gyroscopes. This can be set to a lower value to try to cope with very high vibration levels in aircraft. This option takes effect on the next reboot. A value of zero means no filtering (not recommended!)
// @Units: Hz
// @Range: 0 127
// @User: Advanced
AP_GROUPINFO("GYRO_FILTER", 18, AP_InertialSensor, _gyro_filter_cutoff, DEFAULT_GYRO_FILTER),
// @Param: ACCEL_FILTER
// @DisplayName: Accel filter cutoff frequency
// @Description: Filter cutoff frequency for accelerometers. This can be set to a lower value to try to cope with very high vibration levels in aircraft. This option takes effect on the next reboot. A value of zero means no filtering (not recommended!)
// @Units: Hz
// @Range: 0 127
// @User: Advanced
AP_GROUPINFO("ACCEL_FILTER", 19, AP_InertialSensor, _accel_filter_cutoff, DEFAULT_ACCEL_FILTER),
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully

13
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -183,16 +183,6 @@ public:
_board_orientation = orientation;
}
// override default filter frequency
void set_default_filter(float filter_hz) {
if (!_mpu6000_filter.load()) {
_mpu6000_filter.set(filter_hz);
}
}
// get_filter - return filter in hz
uint8_t get_filter() const { return _mpu6000_filter.get(); }
// return the selected sample rate
Sample_rate get_sample_rate(void) const { return _sample_rate; }
@ -266,7 +256,8 @@ private:
AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
// filtering frequency (0 means default)
AP_Int8 _mpu6000_filter;
AP_Int8 _accel_filter_cutoff;
AP_Int8 _gyro_filter_cutoff;
// board orientation from AHRS
enum Rotation _board_orientation;

23
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -89,29 +89,6 @@ void AP_InertialSensor_Backend::_set_gyro_error_count(uint8_t instance, uint32_t
_imu._gyro_error_count[instance] = error_count;
}
/*
return the default filter frequency in Hz for the sample rate
This uses the sample_rate as a proxy for what type of vehicle it is
(ie. plane and rover run at 50Hz). Copters need a bit more filter
bandwidth
*/
uint8_t AP_InertialSensor_Backend::_default_filter(void) const
{
switch (_imu.get_sample_rate()) {
case AP_InertialSensor::RATE_50HZ:
// on Rover and plane use a lower filter rate
return 15;
case AP_InertialSensor::RATE_100HZ:
return 30;
case AP_InertialSensor::RATE_200HZ:
return 30;
case AP_InertialSensor::RATE_400HZ:
return 30;
}
return 30;
}
// return the requested sample rate in Hz
uint16_t AP_InertialSensor_Backend::get_sample_rate_hz(void) const
{

5
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -83,7 +83,10 @@ protected:
int16_t _product_id;
// return the default filter frequency in Hz for the sample rate
uint8_t _default_filter(void) const;
uint8_t _accel_filter_cutoff(void) const { return _imu._accel_filter_cutoff; }
// return the default filter frequency in Hz for the sample rate
uint8_t _gyro_filter_cutoff(void) const { return _imu._gyro_filter_cutoff; }
// return the requested sample rate in Hz
uint16_t get_sample_rate_hz(void) const;

13
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -91,8 +91,6 @@ AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor
bool AP_InertialSensor_Flymaple::_init_sensor(void)
{
_default_filter_hz = _default_filter();
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@ -142,7 +140,7 @@ bool AP_InertialSensor_Flymaple::_init_sensor(void)
hal.scheduler->delay(1);
// Set up the filter desired
_set_filter_frequency(_imu.get_filter());
_set_filter_frequency(_accel_filter_cutoff());
// give back i2c semaphore
i2c_sem->give();
@ -160,9 +158,6 @@ bool AP_InertialSensor_Flymaple::_init_sensor(void)
*/
void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
{
if (filter_hz == 0)
filter_hz = _default_filter_hz;
_accel_filter.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
_gyro_filter.set_cutoff_frequency(raw_sample_rate_hz, filter_hz);
}
@ -188,9 +183,9 @@ bool AP_InertialSensor_Flymaple::update(void)
gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_publish_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
if (_last_filter_hz != _accel_filter_cutoff()) {
_set_filter_frequency(_accel_filter_cutoff());
_last_filter_hz = _accel_filter_cutoff();
}
return true;

1
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -34,7 +34,6 @@ private:
// support for updating filter at runtime
uint8_t _last_filter_hz;
uint8_t _default_filter_hz;
void _set_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel

15
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -116,13 +116,11 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
_have_gyro_sample(false),
_have_accel_sample(false),
_accel_filter(800, 10),
_gyro_filter(800, 10),
_gyro_filter(800, 10)
{}
bool AP_InertialSensor_L3G4200D::_init_sensor(void)
{
_default_filter_hz = _default_filter();
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@ -207,7 +205,7 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
// Set up the filter desired
_set_filter_frequency(_imu.get_filter());
_set_filter_frequency(_accel_filter_cutoff());
// give back i2c semaphore
i2c_sem->give();
@ -228,9 +226,6 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
*/
void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
{
if (filter_hz == 0)
filter_hz = _default_filter_hz;
_accel_filter.set_cutoff_frequency(800, filter_hz);
_gyro_filter.set_cutoff_frequency(800, filter_hz);
}
@ -257,9 +252,9 @@ bool AP_InertialSensor_L3G4200D::update(void)
gyro *= L3G4200D_GYRO_SCALE_R_S;
_publish_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
if (_last_filter_hz != _accel_filter_cutoff()) {
_set_filter_frequency(_accel_filter_cutoff());
_last_filter_hz = _accel_filter_cutoff();
}
return true;

1
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -38,7 +38,6 @@ private:
// support for updating filter at runtime
uint8_t _last_filter_hz;
uint8_t _default_filter_hz;
void _set_filter_frequency(uint8_t filter_hz);

41
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -178,7 +178,8 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
_drdy_pin(NULL),
_spi(NULL),
_spi_sem(NULL),
_last_filter_hz(0),
_last_accel_filter_hz(-1),
_last_gyro_filter_hz(-1),
_error_count(0),
#if MPU6000_FAST_SAMPLING
_accel_filter(1000, 15),
@ -300,14 +301,27 @@ bool AP_InertialSensor_MPU6000::update( void )
accel *= MPU6000_ACCEL_SCALE_1G / num_samples;
_publish_accel(_accel_instance, accel);
if (_last_filter_hz != _imu.get_filter()) {
#if MPU6000_FAST_SAMPLING
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_accel_filter.set_cutoff_frequency(1000, _accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_gyro_filter.set_cutoff_frequency(1000, _gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
#else
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_imu.get_filter());
_set_filter_register(_accel_filter_cutoff());
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_spi_sem->give();
_last_accel_filter_hz = _accel_filter_cutoff();
}
}
#endif
return true;
}
@ -441,14 +455,13 @@ void AP_InertialSensor_MPU6000::_register_write_check(uint8_t reg, uint8_t val)
/*
set the DLPF filter frequency. Assumes caller has taken semaphore
*/
void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz)
void AP_InertialSensor_MPU6000::_set_filter_register(uint16_t filter_hz)
{
if (filter_hz == 0) {
filter_hz = _default_filter();
}
uint8_t filter;
// choose filtering frequency
if (filter_hz <= 5) {
if (filter_hz == 0) {
filter = BITS_DLPF_CFG_256HZ_NOLPF2;
} else if (filter_hz <= 5) {
filter = BITS_DLPF_CFG_5HZ;
} else if (filter_hz <= 10) {
filter = BITS_DLPF_CFG_10HZ;
@ -456,10 +469,11 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz)
filter = BITS_DLPF_CFG_20HZ;
} else if (filter_hz <= 42) {
filter = BITS_DLPF_CFG_42HZ;
} else {
} else if (filter_hz <= 98) {
filter = BITS_DLPF_CFG_98HZ;
} else {
filter = BITS_DLPF_CFG_256HZ_NOLPF2;
}
_last_filter_hz = filter_hz;
_register_write(MPUREG_CONFIG, filter);
}
@ -530,12 +544,15 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
}
#endif
_set_filter_register(_imu.get_filter());
#if MPU6000_FAST_SAMPLING
// disable sensor filtering
_set_filter_register(256);
// set sample rate to 1000Hz and apply a software filter
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ);
#else
_set_filter_register(_accel_filter_cutoff());
// set sample rate to 200Hz, and use _sample_divider to give
// the requested rate to the application
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);

5
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -65,9 +65,10 @@ private:
static const float _gyro_scale;
// support for updating filter at runtime
uint8_t _last_filter_hz;
int8_t _last_accel_filter_hz;
int8_t _last_gyro_filter_hz;
void _set_filter_register(uint8_t filter_hz);
void _set_filter_register(uint16_t filter_hz);
// count of bus errors
uint16_t _error_count;

44
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -328,7 +328,7 @@ AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_have_sample_available(false),
_accel_filter(800, 10),
_gyro_filter(800, 10),
_gyro_filter(800, 10)
{
}
@ -350,14 +350,18 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &
}
/*
set the filter frequency
set the accel filter frequency
*/
void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz)
void AP_InertialSensor_MPU9150::_set_accel_filter_frequency(uint8_t filter_hz)
{
if (filter_hz == 0)
filter_hz = _default_filter_hz;
_accel_filter.set_cutoff_frequency(800, filter_hz);
}
/*
set the gyro filter frequency
*/
void AP_InertialSensor_MPU9150::_set_gyro_filter_frequency(uint8_t filter_hz)
{
_gyro_filter.set_cutoff_frequency(800, filter_hz);
}
@ -369,8 +373,6 @@ bool AP_InertialSensor_MPU9150::_init_sensor(void)
// Sensors pushed to the FIFO.
uint8_t sensors;
_default_filter_hz = _default_filter();
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
@ -430,8 +432,8 @@ bool AP_InertialSensor_MPU9150::_init_sensor(void)
hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n");
goto failed;
}
// Set digital low pass filter (using _default_filter_hz, 20 for 100 Hz of sample rate)
if (mpu_set_lpf(_default_filter_hz)){
// Set digital low pass filter to 256Hz (DLPF disabled)
if (mpu_set_lpf(256)) {
hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_lpf.\n");
goto failed;
}
@ -456,8 +458,9 @@ bool AP_InertialSensor_MPU9150::_init_sensor(void)
mpu_set_sensors(sensors);
// Set the filter frecuency (_mpu6000_filter configured to the default value, check AP_InertialSensor.cpp)
_set_filter_frequency(_imu.get_filter());
// Set the filter frecuency
_set_accel_filter_frequency(_accel_filter_cutoff());
_set_gyro_filter_frequency(_gyro_filter_cutoff());
// give back i2c semaphore
i2c_sem->give();
@ -550,7 +553,11 @@ int16_t AP_InertialSensor_MPU9150::mpu_set_lpf(uint16_t lpf)
{
uint8_t data;
if (lpf >= 188){
if (lpf == 0) {
data = INV_FILTER_256HZ_NOLPF2;
} else if (lpf >= 256){
data = INV_FILTER_256HZ_NOLPF2;
} else if (lpf >= 188){
data = INV_FILTER_188HZ;
}
else if (lpf >= 98){
@ -1106,9 +1113,14 @@ bool AP_InertialSensor_MPU9150::update(void)
gyro *= MPU9150_GYRO_SCALE_2000;
_publish_gyro(_gyro_instance, gyro);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_set_accel_filter_frequency(_accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_set_gyro_filter_frequency(_gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
return true;

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
View File

@ -34,8 +34,8 @@ private:
bool _have_sample_available;
// // support for updating filter at runtime
uint8_t _last_filter_hz;
uint8_t _default_filter_hz;
uint8_t _last_accel_filter_hz;
uint8_t _last_gyro_filter_hz;
int16_t mpu_set_gyro_fsr(uint16_t fsr);
int16_t mpu_set_accel_fsr(uint8_t fsr);
@ -49,7 +49,8 @@ private:
int16_t mpu_set_int_latched(uint8_t enable);
int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp, uint8_t *sensors, uint8_t *more);
void _set_filter_frequency(uint8_t filter_hz);
void _set_accel_filter_frequency(uint8_t filter_hz);
void _set_gyro_filter_frequency(uint8_t filter_hz);
// Low Pass filters for gyro and accel
LowPassFilter2pVector3f _accel_filter;

34
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -173,7 +173,8 @@ extern const AP_HAL::HAL& hal;
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_last_filter_hz(-1),
_last_accel_filter_hz(-1),
_last_gyro_filter_hz(-1),
_shared_data_idx(0),
_accel_filter(1000, 15),
_gyro_filter(1000, 15),
@ -292,9 +293,14 @@ bool AP_InertialSensor_MPU9250::update( void )
_publish_gyro(_gyro_instance, gyro);
_publish_accel(_accel_instance, accel);
if (_last_filter_hz != _imu.get_filter()) {
_set_filter(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_set_accel_filter(_accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_set_gyro_filter(_gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
return true;
@ -383,16 +389,18 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
}
/*
set the accel/gyro filter frequency
set the accel filter frequency
*/
void AP_InertialSensor_MPU9250::_set_filter(uint8_t filter_hz)
void AP_InertialSensor_MPU9250::_set_accel_filter(uint8_t filter_hz)
{
if (filter_hz == 0) {
filter_hz = _default_filter_hz;
_accel_filter.set_cutoff_frequency(1000, filter_hz);
}
_accel_filter.set_cutoff_frequency(1000, filter_hz);
/*
set the gyro filter frequency
*/
void AP_InertialSensor_MPU9250::_set_gyro_filter(uint8_t filter_hz)
{
_gyro_filter.set_cutoff_frequency(1000, filter_hz);
}
@ -449,11 +457,9 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
#endif
_default_filter_hz = _default_filter();
// used a fixed filter of 42Hz on the sensor, then filter using
// used no filter of 256Hz on the sensor, then filter using
// the 2-pole software filter
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ);
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2);
// set sample rate to 1kHz, and use the 2 pole filter to give the
// desired rate

9
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -44,10 +44,12 @@ private:
AP_HAL::Semaphore *_spi_sem;
// support for updating filter at runtime
int16_t _last_filter_hz;
int16_t _last_gyro_filter_hz;
int16_t _last_accel_filter_hz;
// change the filter frequency
void _set_filter(uint8_t filter_hz);
void _set_accel_filter(uint8_t filter_hz);
void _set_gyro_filter(uint8_t filter_hz);
// This structure is used to pass data from the timer which reads
// the sensor to the main thread. The _shared_data_idx is used to
@ -66,9 +68,6 @@ private:
// do we currently have a sample pending?
bool _have_sample_available;
// default filter frequency when set to zero
uint8_t _default_filter_hz;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;

51
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -20,7 +20,10 @@ const extern AP_HAL::HAL& hal;
AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_last_get_sample_timestamp(0)
_last_get_sample_timestamp(0),
_last_sample_timestamp(0),
_last_gyro_filter_hz(-1),
_last_accel_filter_hz(-1)
{
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_delta_angle_accumulator[i].zero();
@ -149,7 +152,8 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
}
}
_set_filter_frequency(_imu.get_filter());
_set_accel_filter_frequency(_accel_filter_cutoff());
_set_gyro_filter_frequency(_gyro_filter_cutoff());
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
_product_id = AP_PRODUCT_ID_VRBRAIN;
@ -164,22 +168,10 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
}
/*
set the filter frequency
set the accel filter frequency
*/
void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
void AP_InertialSensor_PX4::_set_accel_filter_frequency(uint8_t filter_hz)
{
if (filter_hz == 0) {
filter_hz = _default_filter();
}
for (uint8_t i=0; i<_num_gyro_instances; i++) {
int samplerate = ioctl(_gyro_fd[i], GYROIOCGSAMPLERATE, 0);
if(samplerate < 100 || samplerate > 2000) {
// sample rate doesn't seem sane, turn off filter
_gyro_filter[i].set_cutoff_frequency(0, 0);
} else {
_gyro_filter[i].set_cutoff_frequency(samplerate, filter_hz);
}
}
for (uint8_t i=0; i<_num_accel_instances; i++) {
int samplerate = ioctl(_accel_fd[i], ACCELIOCGSAMPLERATE, 0);
if(samplerate < 100 || samplerate > 2000) {
@ -191,6 +183,22 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
}
}
/*
set the gyro filter frequency
*/
void AP_InertialSensor_PX4::_set_gyro_filter_frequency(uint8_t filter_hz)
{
for (uint8_t i=0; i<_num_gyro_instances; i++) {
int samplerate = ioctl(_gyro_fd[i], GYROIOCGSAMPLERATE, 0);
if(samplerate < 100 || samplerate > 2000) {
// sample rate doesn't seem sane, turn off filter
_gyro_filter[i].set_cutoff_frequency(0, 0);
} else {
_gyro_filter[i].set_cutoff_frequency(samplerate, filter_hz);
}
}
}
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
@ -223,9 +231,14 @@ bool AP_InertialSensor_PX4::update(void)
_delta_velocity_accumulator[i].zero();
}
if (_last_filter_hz != _imu.get_filter()) {
_set_filter_frequency(_imu.get_filter());
_last_filter_hz = _imu.get_filter();
if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_set_accel_filter_frequency(_accel_filter_cutoff());
_last_accel_filter_hz = _accel_filter_cutoff();
}
if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
_set_gyro_filter_frequency(_gyro_filter_cutoff());
_last_gyro_filter_hz = _gyro_filter_cutoff();
}
return true;

8
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -53,10 +53,12 @@ private:
// calculate right queue depth for a sensor
uint8_t _queue_depth(uint16_t sensor_sample_rate) const;
// support for updating filter at runtime
uint8_t _last_filter_hz;
// support for updating filter at runtime (-1 means unset)
int8_t _last_gyro_filter_hz;
int8_t _last_accel_filter_hz;
void _set_filter_frequency(uint8_t filter_hz);
void _set_gyro_filter_frequency(uint8_t filter_hz);
void _set_accel_filter_frequency(uint8_t filter_hz);
// accelerometer and gyro driver handles
uint8_t _num_accel_instances;