Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added filter frequency support to PX4 driver

Browse Source
This commit is contained in:
Andrew Tridgell 2013-02-07 11:20:45 +11:00
parent 5643c371b9
commit 1121254606
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -29,13 +29,16 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
switch (sample_rate) {
case RATE_50HZ:
_sample_divider = 4;
_default_filter_hz = 10;
break;
case RATE_100HZ:
_sample_divider = 2;
_default_filter_hz = 42;
break;
case RATE_200HZ:
default:
_sample_divider = 1;
_default_filter_hz = 42;
break;
}
@ -67,9 +70,23 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
// register a 1kHz timer to read from PX4 sensor drivers
hal.scheduler->register_timer_process(_ins_timer);
_set_filter_frequency(_mpu6000_filter);
return AP_PRODUCT_ID_PX4;
}
/*
set the filter frequency
*/
void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
{
if (filter_hz == 0) {
filter_hz = _default_filter_hz;
}
ioctl(_gyro_fd, GYROIOCSLOWPASS, filter_hz);
ioctl(_accel_fd, ACCELIOCSLOWPASS, filter_hz);
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
bool AP_InertialSensor_PX4::update(void)
@ -106,6 +123,11 @@ bool AP_InertialSensor_PX4::update(void)
_gyro.rotate(_board_orientation);
_gyro -= _gyro_offset;
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
return true;
}

6
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -41,6 +41,12 @@ private:
static uint64_t _last_gyro_timestamp;
uint8_t _sample_divider;
// support for updating filter at runtime
uint8_t _last_filter_hz;
uint8_t _default_filter_hz;
void _set_filter_frequency(uint8_t filter_hz);
// accelerometer and gyro driver handles
static int _accel_fd;
static int _gyro_fd;