aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: moved default filter and sample_rate to frontend

Browse Source 
this simplifies the backends and prevents code repitition
This commit is contained in:
Andrew Tridgell 2014-10-17 07:52:21 +11:00
parent 17b2214798
commit a047d1f569
20 changed files with 105 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -258,9 +258,12 @@ void
AP_InertialSensor::init( Start_style style,
Sample_rate sample_rate)
{
// remember the sample rate
_sample_rate = sample_rate;
if (_gyro_count == 0 && _accel_count == 0) {
// detect available backends. Only called once
_detect_backends(sample_rate);
_detect_backends();
}
_product_id = 0; // FIX
@ -303,12 +306,12 @@ AP_InertialSensor::init( Start_style style,
/*
try to load a backend
*/
void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &, Sample_rate), Sample_rate sample_rate)
void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &))
{
if (_backend_count == INS_MAX_BACKENDS) {
hal.scheduler->panic(PSTR("Too many INS backends"));
}
_backends[_backend_count] = detect(*this, sample_rate);
_backends[_backend_count] = detect(*this);
if (_backends[_backend_count] != NULL) {
_backend_count++;
}
@ -319,20 +322,20 @@ void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *(detect)(AP_Iner
detect available backends for this board
*/
void
AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
AP_InertialSensor::_detect_backends(void)
{
#if HAL_INS_DEFAULT == HAL_INS_HIL
_add_backend(AP_InertialSensor_HIL::detect, sample_rate);
_add_backend(AP_InertialSensor_HIL::detect);
#elif HAL_INS_DEFAULT == HAL_INS_MPU6000
_add_backend(AP_InertialSensor_MPU6000::detect, sample_rate);
_add_backend(AP_InertialSensor_MPU6000::detect);
#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
_add_backend(AP_InertialSensor_PX4::detect, sample_rate);
_add_backend(AP_InertialSensor_PX4::detect);
#elif HAL_INS_DEFAULT == HAL_INS_OILPAN
_add_backend(AP_InertialSensor_Oilpan::detect, sample_rate);
_add_backend(AP_InertialSensor_Oilpan::detect);
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250
_add_backend(AP_InertialSensor_MPU9250::detect, sample_rate);
_add_backend(AP_InertialSensor_MPU9250::detect);
#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE
_add_backend(AP_InertialSensor_Flymaple::detect, sample_rate);
_add_backend(AP_InertialSensor_Flymaple::detect);
#else
#error Unrecognised HAL_INS_TYPE setting
#endif
@ -340,7 +343,7 @@ AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
#if 0 // disabled due to broken hardware on some PXF capes
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
// the PXF also has a MPU6000
_add_backend(AP_InertialSensor_MPU6000::detect, sample_rate);
_add_backend(AP_InertialSensor_MPU6000::detect);
#endif
#endif

10
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -178,6 +178,9 @@ public:
// 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; }
uint16_t error_count(void) const { return 0; }
bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
@ -189,8 +192,8 @@ public:
private:
// load backend drivers
void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &, Sample_rate), Sample_rate sample_rate);
void _detect_backends(Sample_rate sample_rate);
void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &));
void _detect_backends(void);
// accel and gyro initialisation
void _init_accel();
@ -222,6 +225,9 @@ private:
uint8_t _accel_count;
uint8_t _backend_count;
// the selected sample rate
Sample_rate _sample_rate;
// Most recent accelerometer reading
Vector3f _accel[INS_MAX_INSTANCES];

23
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -35,3 +35,26 @@ void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const
_imu._accel[instance] -= _imu._accel_offset[instance];
_imu._accel_healthy[instance] = true;
}
/*
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;
}

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -70,6 +70,9 @@ protected:
// backend should fill in its product ID from AP_PRODUCT_ID_*
int16_t _product_id;
// return the default filter frequency in Hz for the sample rate
uint8_t _default_filter(void) const;
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor
// driver if the sensor is available

26
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -80,40 +80,22 @@ AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) :
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_Flymaple::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_Flymaple::_init_sensor(void)
{
// Sensors are raw sampled at 800Hz.
// Here we figure the divider to get the rate that update should be called
switch (sample_rate) {
case AP_InertialSensor::RATE_50HZ:
_default_filter_hz = 10;
break;
case AP_InertialSensor::RATE_100HZ:
_default_filter_hz = 20;
break;
case AP_InertialSensor::RATE_200HZ:
_default_filter_hz = 20;
break;
case AP_InertialSensor::RATE_400HZ:
_default_filter_hz = 30;
break;
default:
return false;
}
_default_filter_hz = _default_filter();
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();

5
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -22,11 +22,10 @@ public:
bool accel_sample_available(void) { _accumulate(); return _have_accel_sample; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _init_sensor(void);
void _accumulate(void);
Vector3f _accel_filtered;
Vector3f _gyro_filtered;

33
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -6,47 +6,28 @@
const extern AP_HAL::HAL& hal;
AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_sample_period_usec(0)
AP_InertialSensor_Backend(imu)
{
}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_HIL::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_HIL::_init_sensor(void)
{
switch (sample_rate) {
case AP_InertialSensor::RATE_50HZ:
_sample_period_usec = 20000;
break;
case AP_InertialSensor::RATE_100HZ:
_sample_period_usec = 10000;
break;
case AP_InertialSensor::RATE_200HZ:
_sample_period_usec = 5000;
break;
case AP_InertialSensor::RATE_400HZ:
_sample_period_usec = 2500;
break;
default:
return false;
}
// grab the used instances
_imu.register_gyro();
_imu.register_accel();
@ -62,9 +43,3 @@ bool AP_InertialSensor_HIL::update(void)
// doesn't need to do anything
return true;
}
bool AP_InertialSensor_HIL::_sample_available()
{
// just use the timing based wait_for_sample() in the frontend
return true;
}

11
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

@ -13,17 +13,14 @@ public:
/* update accel and gyro state */
bool update();
bool gyro_sample_available(void) { return _sample_available(); }
bool accel_sample_available(void) { return _sample_available(); }
bool gyro_sample_available(void) { return true; }
bool accel_sample_available(void) { return true; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _sample_available(void);
uint32_t _sample_period_usec;
bool _init_sensor(void);
};
#endif // __AP_INERTIALSENSOR_HIL_H__

20
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -123,25 +123,9 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
_gyro_filter_z(800, 10)
{}
bool AP_InertialSensor_L3G4200D::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_L3G4200D::_init_sensor(void)
{
switch (sample_rate) {
case AP_InertialSensor::RATE_50HZ:
_default_filter_hz = 10;
break;
case AP_InertialSensor::RATE_100HZ:
_default_filter_hz = 20;
break;
case AP_InertialSensor::RATE_200HZ:
_default_filter_hz = 30;
break;
case AP_InertialSensor::RATE_400HZ:
_default_filter_hz = 30;
break;
default:
return false;
}
_default_filter_hz = _default_filter();
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();

5
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -23,14 +23,13 @@ public:
bool accel_sample_available(void) { return _have_accel_sample; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
// return product ID
int16_t product_id(void) const { return AP_PRODUCT_ID_L3G4200D; }
private:
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _init_sensor(void);
void _accumulate(void);
Vector3f _accel_filtered;
Vector3f _gyro_filtered;

13
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -190,14 +190,13 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
@ -208,7 +207,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &
/*
initialise the sensor
*/
bool AP_InertialSensor_MPU6000::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_MPU6000::_init_sensor(void)
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
_spi_sem = _spi->get_semaphore();
@ -222,7 +221,7 @@ bool AP_InertialSensor_MPU6000::_init_sensor(AP_InertialSensor::Sample_rate samp
uint8_t tries = 0;
do {
bool success = _hardware_init(sample_rate);
bool success = _hardware_init();
if (success) {
hal.scheduler->delay(5+2);
if (!_spi_sem->take(100)) {
@ -446,7 +445,7 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t
}
bool AP_InertialSensor_MPU6000::_hardware_init(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_MPU6000::_hardware_init(void)
{
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
@ -493,7 +492,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(AP_InertialSensor::Sample_rate sa
// sample rate and filtering
// to minimise the effects of aliasing we choose a filter
// that is less than half of the sample rate
switch (sample_rate) {
switch (_imu.get_sample_rate()) {
case AP_InertialSensor::RATE_50HZ:
// this is used for plane and rover, where noise resistance is
// more important than update rate. Tests on an aerobatic plane

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -24,8 +24,7 @@ public:
bool accel_sample_available(void) { return _sum_count >= _sample_count; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
#if MPU6000_DEBUG
@ -38,7 +37,7 @@ private:
AP_HAL::DigitalSource *_drdy_pin;
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _init_sensor(void);
bool _sample_available();
void _read_data_transaction();
bool _data_ready();
@ -46,7 +45,7 @@ private:
uint8_t _register_read( uint8_t reg );
void _register_write( uint8_t reg, uint8_t val );
void _register_write_check(uint8_t reg, uint8_t val);
bool _hardware_init(AP_InertialSensor::Sample_rate sample_rate);
bool _hardware_init(void);
AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem;

28
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -340,14 +340,13 @@ AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_MPU9150 *sensor = new AP_InertialSensor_MPU9150(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
@ -371,31 +370,14 @@ void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz)
}
/**
* @brief Init method
* @param[in] Sample_rate The sample rate, check the struct def.
* @return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE if successful.
* Init method
*/
bool AP_InertialSensor_MPU9150::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_MPU9150::_init_sensor(void)
{
// Sensors pushed to the FIFO.
uint8_t sensors;
switch (sample_rate) {
case AP_InertialSensor::RATE_50HZ:
_default_filter_hz = 10;
break;
case AP_InertialSensor::RATE_100HZ:
_default_filter_hz = 20;
break;
case AP_InertialSensor::RATE_200HZ:
_default_filter_hz = 20;
break;
case AP_InertialSensor::RATE_400HZ:
_default_filter_hz = 20;
break;
default:
return false;
}
_default_filter_hz = _default_filter();
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();

5
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
View File

@ -24,11 +24,10 @@ public:
bool accel_sample_available(void) { return _have_sample_available; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _init_sensor();
void _accumulate(void);
Vector3f _accel_filtered;
Vector3f _gyro_filtered;

35
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -189,14 +189,13 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
@ -207,7 +206,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &
/*
initialise the sensor
*/
bool AP_InertialSensor_MPU9250::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_MPU9250::_init_sensor(void)
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
_spi_sem = _spi->get_semaphore();
@ -226,7 +225,7 @@ bool AP_InertialSensor_MPU9250::_init_sensor(AP_InertialSensor::Sample_rate samp
uint8_t tries = 0;
do {
bool success = _hardware_init(sample_rate);
bool success = _hardware_init();
if (success) {
hal.scheduler->delay(10);
if (!_spi_sem->take(100)) {
@ -401,7 +400,7 @@ void AP_InertialSensor_MPU9250::_set_filter(uint8_t filter_hz)
/*
initialise the sensor configuration registers
*/
bool AP_InertialSensor_MPU9250::_hardware_init(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_MPU9250::_hardware_init(void)
{
if (!_spi_sem->take(100)) {
hal.console->printf("MPU9250: Unable to get semaphore");
@ -442,29 +441,7 @@ bool AP_InertialSensor_MPU9250::_hardware_init(AP_InertialSensor::Sample_rate sa
// Disable I2C bus (recommended on datasheet)
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
// sample rate and filtering
// to minimise the effects of aliasing we choose a filter
// that is less than half of the sample rate
switch (sample_rate) {
case AP_InertialSensor::RATE_50HZ:
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case AP_InertialSensor::RATE_100HZ:
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case AP_InertialSensor::RATE_200HZ:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
case AP_InertialSensor::RATE_400HZ:
_default_filter_hz = 30;
_sample_time_usec = 2500;
break;
default:
return false;
}
_default_filter_hz = _default_filter();
// used a fixed filter of 42Hz on the sensor, then filter using
// the 2-pole software filter

9
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -27,25 +27,22 @@ public:
bool accel_sample_available(void) { return _have_sample_available; }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _init_sensor(void);
void _read_data_transaction();
bool _data_ready();
void _poll_data(void);
uint8_t _register_read( uint8_t reg );
void _register_write( uint8_t reg, uint8_t val );
bool _hardware_init(AP_InertialSensor::Sample_rate sample_rate);
bool _hardware_init(void);
bool _sample_available();
AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem;
uint32_t _sample_time_usec;
// support for updating filter at runtime
int16_t _last_filter_hz;

9
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -49,14 +49,13 @@ AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan(AP_InertialSensor &imu) :
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_Oilpan *sensor = new AP_InertialSensor_Oilpan(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
@ -64,11 +63,11 @@ AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_
return sensor;
}
bool AP_InertialSensor_Oilpan::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_Oilpan::_init_sensor(void)
{
apm1_adc.Init();
switch (sample_rate) {
switch (_imu.get_sample_rate()) {
case AP_InertialSensor::RATE_50HZ:
_sample_threshold = 20;
break;

6
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
View File

@ -18,10 +18,10 @@ public:
bool accel_sample_available(void) { return _sample_available(); }
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
private:
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _init_sensor(void);
bool _sample_available() const;
static const uint8_t _sensors[6];
static const int8_t _sensor_signs[6];

32
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -20,29 +20,27 @@ const extern AP_HAL::HAL& hal;
AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_last_get_sample_timestamp(0),
_sample_time_usec(0)
_last_get_sample_timestamp(0)
{
}
/*
detect the sensor
*/
AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu,
AP_InertialSensor::Sample_rate sample_rate)
AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu)
{
AP_InertialSensor_PX4 *sensor = new AP_InertialSensor_PX4(_imu);
if (sensor == NULL) {
return NULL;
}
if (!sensor->_init_sensor(sample_rate)) {
if (!sensor->_init_sensor()) {
delete sensor;
return NULL;
}
return sensor;
}
bool AP_InertialSensor_PX4::_init_sensor(AP_InertialSensor::Sample_rate sample_rate)
bool AP_InertialSensor_PX4::_init_sensor(void)
{
// assumes max 3 instances
_accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
@ -71,27 +69,7 @@ bool AP_InertialSensor_PX4::_init_sensor(AP_InertialSensor::Sample_rate sample_r
return false;
}
switch (sample_rate) {
case AP_InertialSensor::RATE_50HZ:
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case AP_InertialSensor::RATE_100HZ:
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case AP_InertialSensor::RATE_200HZ:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
case AP_InertialSensor::RATE_400HZ:
_default_filter_hz = 30;
_sample_time_usec = 2500;
break;
default:
return false;
}
_default_filter_hz = _default_filter();
_set_filter_frequency(_imu.get_filter());
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)

6
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -23,14 +23,13 @@ public:
bool update();
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
AP_InertialSensor::Sample_rate sample_rate);
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
bool gyro_sample_available(void);
bool accel_sample_available(void);
private:
bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
bool _init_sensor(void);
void _get_sample(void);
bool _sample_available(void);
Vector3f _accel_in[INS_MAX_INSTANCES];
@ -41,7 +40,6 @@ private:
uint64_t _last_gyro_update_timestamp[INS_MAX_INSTANCES];
uint64_t _last_get_sample_timestamp;
uint64_t _last_sample_timestamp;
uint32_t _sample_time_usec;
// support for updating filter at runtime
uint8_t _last_filter_hz;