From 089ae0f9f35770d563c271099bfd080a83c79854 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 29 Nov 2012 22:56:13 +1100 Subject: [PATCH] AP_InertialSensor: allow specification of sample rate in init() call this lets the caller not need to know the underlying sample rate. They just ask for what rate updates happen. This also changes the MPU6k filtering to be less than half the sample rate --- .../AP_InertialSensor/AP_InertialSensor.cpp | 3 +- .../AP_InertialSensor/AP_InertialSensor.h | 10 ++++- .../AP_InertialSensor_MPU6000.cpp | 38 +++++++++++++++---- .../AP_InertialSensor_MPU6000.h | 4 +- .../AP_InertialSensor_Oilpan.cpp | 19 +++++++++- .../AP_InertialSensor_Oilpan.h | 4 +- .../AP_InertialSensor_Stub.cpp | 2 +- .../AP_InertialSensor_Stub.h | 2 +- .../examples/MPU6000/MPU6000.pde | 4 +- 9 files changed, 69 insertions(+), 17 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 2887ea6882..445d905eca 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -25,11 +25,12 @@ AP_InertialSensor::AP_InertialSensor() void AP_InertialSensor::init( Start_style style, + Sample_rate sample_rate, void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), AP_PeriodicProcess * scheduler ) { - _product_id = _init_sensor(scheduler); + _product_id = _init_sensor(scheduler, sample_rate); // check scaling Vector3f accel_scale = _accel_scale.get(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 410ad5abea..809e85ecf3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -24,6 +24,13 @@ public: WARM_START }; + // the rate that updates will be available to the application + enum Sample_rate { + RATE_50HZ, + RATE_100HZ, + RATE_200HZ + }; + /// Perform startup initialisation. /// /// Called to initialise the state of the IMU. @@ -38,6 +45,7 @@ public: /// @param style The initialisation startup style. /// virtual void init( Start_style style, + Sample_rate sample_rate, void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), AP_PeriodicProcess * scheduler ); @@ -135,7 +143,7 @@ public: protected: // sensor specific init to be overwritten by descendant classes - virtual uint16_t _init_sensor( AP_PeriodicProcess * scheduler ) = 0; + virtual uint16_t _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) = 0; // no-save implementations of accel and gyro initialisation routines virtual void _init_accel(void (*delay_cb)(unsigned long t), diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 1f74bcb34a..9450ae9517 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -210,13 +210,13 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() _dmp_initialised = false; } -uint16_t AP_InertialSensor_MPU6000::_init_sensor( AP_PeriodicProcess * scheduler ) +uint16_t AP_InertialSensor_MPU6000::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) { if (_initialised) return _mpu6000_product_id; _initialised = true; _scheduler = scheduler; // store pointer to scheduler so that we can suspend/resume scheduler when we pull data from the MPU6000 scheduler->suspend_timer(); - hardware_init(); + hardware_init(sample_rate); scheduler->resume_timer(); return _mpu6000_product_id; } @@ -381,7 +381,7 @@ void AP_InertialSensor_MPU6000::data_interrupt(void) _scheduler->queue_process( AP_InertialSensor_MPU6000::read ); } -void AP_InertialSensor_MPU6000::hardware_init() +void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate) { // MPU6000 chip select setup pinMode(MPU6000_CS_PIN, OUTPUT); @@ -401,12 +401,36 @@ void AP_InertialSensor_MPU6000::hardware_init() // Disable I2C bus (recommended on datasheet) register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); delay(1); - // SAMPLE RATE - register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + + uint8_t rate, filter; + + // 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 RATE_50HZ: + rate = MPUREG_SMPLRT_50HZ; + filter = BITS_DLPF_CFG_20HZ; + break; + case RATE_100HZ: + rate = MPUREG_SMPLRT_100HZ; + filter = BITS_DLPF_CFG_42HZ; + break; + case RATE_200HZ: + default: + rate = MPUREG_SMPLRT_200HZ; + filter = BITS_DLPF_CFG_98HZ; + break; + } + + // set sample rate + register_write(MPUREG_SMPLRT_DIV, rate); delay(1); - // FS & DLPF FS=2000º/s, DLPF = 98Hz (low pass filter) - register_write(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ); + + // set low pass filter + register_write(MPUREG_CONFIG, filter); delay(1); + register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s delay(1); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index dd616c506e..a0bc7e157e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -52,7 +52,7 @@ public: uint32_t get_delta_time_micros(); protected: - uint16_t _init_sensor( AP_PeriodicProcess * scheduler ); + uint16_t _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ); private: @@ -60,7 +60,7 @@ private: static void data_interrupt(void); static uint8_t register_read( uint8_t reg ); static void register_write( uint8_t reg, uint8_t val ); - static void hardware_init(); + static void hardware_init(Sample_rate sample_rate); float _temp; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 2aee5dfeaa..0f592560fd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -41,10 +41,22 @@ AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) : { } -uint16_t AP_InertialSensor_Oilpan::_init_sensor( AP_PeriodicProcess * scheduler) +uint16_t AP_InertialSensor_Oilpan::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate) { _adc->Init(scheduler); + switch (sample_rate) { + case RATE_50HZ: + _sample_threshold = 20; + break; + case RATE_100HZ: + _sample_threshold = 10; + break; + case RATE_200HZ: + _sample_threshold = 5; + break; + } + #if defined(DESKTOP_BUILD) return AP_PRODUCT_ID_SITL; #elif defined(__AVR_ATmega1280__) @@ -130,5 +142,8 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void) // get number of samples read from the sensors uint16_t AP_InertialSensor_Oilpan::num_samples_available() { - return _adc->num_samples_available(_sensors); + if (_adc->num_samples_available(_sensors) >= _sample_threshold) { + return 1; + } + return 0; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index 5ed61762df..0991047e4e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -34,7 +34,7 @@ public: uint16_t num_samples_available(); protected: - uint16_t _init_sensor(AP_PeriodicProcess * scheduler); + uint16_t _init_sensor(AP_PeriodicProcess * scheduler, Sample_rate sample_rate); private: @@ -55,6 +55,8 @@ private: static const float _gyro_gain_z; static const float _adc_constraint; + + uint8_t _sample_threshold; }; #endif // __AP_INERTIAL_SENSOR_OILPAN_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp index 42d5372b40..7d9f02af2a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp @@ -2,7 +2,7 @@ #include "AP_InertialSensor_Stub.h" -uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler ) { +uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) { return AP_PRODUCT_ID_NONE; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h index 284a3629c3..ceafae5fe2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h @@ -32,7 +32,7 @@ public: uint16_t num_samples_available(); protected: - uint16_t _init_sensor( AP_PeriodicProcess * scheduler ); + uint16_t _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ); }; #endif // __AP_INERTIAL_SENSOR_STUB_H__ diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde index dde00fb36f..393e478e22 100644 --- a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde +++ b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde @@ -55,7 +55,9 @@ void setup(void) #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 adc.Init(&scheduler); // APM ADC library initialization #endif - ins.init(AP_InertialSensor::COLD_START, delay, NULL, &scheduler); + ins.init(AP_InertialSensor::COLD_START, + AP_InertialSensor::RATE_100HZ, + delay, NULL, &scheduler); // display initial values display_offsets_and_scaling();