From a047d1f56916193b1a019737945316f0840e0b1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 17 Oct 2014 07:52:21 +1100 Subject: [PATCH] AP_InertialSensor: moved default filter and sample_rate to frontend this simplifies the backends and prevents code repitition --- .../AP_InertialSensor/AP_InertialSensor.cpp | 25 +++++++------ .../AP_InertialSensor/AP_InertialSensor.h | 10 ++++-- .../AP_InertialSensor_Backend.cpp | 23 ++++++++++++ .../AP_InertialSensor_Backend.h | 3 ++ .../AP_InertialSensor_Flymaple.cpp | 26 +++----------- .../AP_InertialSensor_Flymaple.h | 5 ++- .../AP_InertialSensor_HIL.cpp | 33 +++-------------- .../AP_InertialSensor/AP_InertialSensor_HIL.h | 11 +++--- .../AP_InertialSensor_L3G4200D.cpp | 20 ++--------- .../AP_InertialSensor_L3G4200D.h | 5 ++- .../AP_InertialSensor_MPU6000.cpp | 13 ++++--- .../AP_InertialSensor_MPU6000.h | 7 ++-- .../AP_InertialSensor_MPU9150.cpp | 28 +++------------ .../AP_InertialSensor_MPU9150.h | 5 ++- .../AP_InertialSensor_MPU9250.cpp | 35 ++++--------------- .../AP_InertialSensor_MPU9250.h | 9 ++--- .../AP_InertialSensor_Oilpan.cpp | 9 +++-- .../AP_InertialSensor_Oilpan.h | 6 ++-- .../AP_InertialSensor_PX4.cpp | 32 +++-------------- .../AP_InertialSensor/AP_InertialSensor_PX4.h | 6 ++-- 20 files changed, 105 insertions(+), 206 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 4d733cae78..b07f13cbba 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 4208287fe4..47a631c125 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -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]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 3f7678f816..0a82c8674d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -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; +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 5b341c74ca..e37f7f07f1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index bb0daf9bcc..684cb327e9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -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(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h index 5e56f696a3..724e03dbb4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index 5e805e2b7f..e0070d7616 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -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; -} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h index b49a7e56ca..9c1422c662 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h @@ -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__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index edc2672991..e1e4c5ce25 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -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(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index fc26dc97cb..674dc87c15 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index bea8dd60ea..ea8bfb08cd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index c27dab15ff..40aa92df2f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp index f89d5c66f5..95130f12eb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp @@ -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(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h index e7369d0298..52de4f9d71 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index e77d38f7ff..1137e6558c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index b928acd0a7..1ce5b1a67f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index d2b856a775..df56b44746 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index 8e9d7cb1ac..e00169424c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -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]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index fc0ef65aad..4d41a15428 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -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) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index fe584aa050..fa82acb9f7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -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;