From 191030cc3a4a2d4c5455d60eb9e59c3fda788cba Mon Sep 17 00:00:00 2001 From: RCmags Date: Sun, 4 Dec 2022 13:57:32 -0400 Subject: [PATCH] updrade to 0.2.0 --- README.md | 32 +- examples/output/output.ino | 0 examples/parameters/parameters.ino | 145 ++++----- examples/raw_output/raw_output.ino | 0 keywords.txt | 2 + library.properties | 6 +- src/basicMPU6050.h | 263 +++++++++-------- src/basicMPU6050.tpp | 454 +++++++++++++++-------------- 8 files changed, 455 insertions(+), 447 deletions(-) mode change 100644 => 100755 README.md mode change 100644 => 100755 examples/output/output.ino mode change 100644 => 100755 examples/parameters/parameters.ino mode change 100644 => 100755 examples/raw_output/raw_output.ino mode change 100644 => 100755 keywords.txt mode change 100644 => 100755 library.properties mode change 100644 => 100755 src/basicMPU6050.h mode change 100644 => 100755 src/basicMPU6050.tpp diff --git a/README.md b/README.md old mode 100644 new mode 100755 index d3f897d..cc285e8 --- a/README.md +++ b/README.md @@ -1,15 +1,17 @@ -# basicMPU6050 -The purpose of this library is to make a basic and lightweight interface for the MPU6050. It can do the following: -- Configure the inbuilt low pass filter -- Configure the sensitivity of the accelerometer and gyro -- Retrieve the raw output of the sensor -- Output scaled accelerometer and gyro values - -The library includes two functions to calibrate the gyro and remove bias. The first is intended to be called when the sensor is turned on and is not moving. It takes a long term average of the output of each axis and subtracts these values from the raw signals. - -The second function is designed to update the averages. It updates the values with a moving average and the gain is controlled by something akin to a kalman filter. By polling this function one can correct for drift in the gyro bias. - -By combining these two functions one can obtain stable and consistent gyro outputs. The accelerometer needs to calibrated manually by correcting the bias and scale. - -See this link for the information on the registers of the MPU6050: -[MPU 6050 register map](https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf) +# basicMPU6050 +The purpose of this library is to make a basic and lightweight interface for the MPU6050. It can do the following: +- Configure the inbuilt low pass filter +- Configure the sensitivity of the accelerometer and gyro +- Retrieve the raw output of the sensor +- Output scaled accelerometer and gyro values + +The library includes two functions to calibrate the gyro and remove bias. The first is intended to be called when the sensor is turned on and is not moving. It takes a long term average of the output of each axis and subtracts these values from the raw signals. + +The second function is designed to update the averages. It updates the values with a moving average and the gain is controlled by something akin to a kalman filter. By polling this function one can correct for drift in the gyro bias. By combining these two functions one can obtain stable and consistent gyro outputs. However, The accelerometer needs to calibrated manually by correcting the bias and scale. + +Finally, the library was written for a single MPU6050 connected to a MEGA board as follows. It may require some modifications to work succesfully on other boards: + + + +See this link for the information on the registers of the MPU6050: +[MPU 6050 register map](https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf) diff --git a/examples/output/output.ino b/examples/output/output.ino old mode 100644 new mode 100755 diff --git a/examples/parameters/parameters.ino b/examples/parameters/parameters.ino old mode 100644 new mode 100755 index e144f07..d8f7eac --- a/examples/parameters/parameters.ino +++ b/examples/parameters/parameters.ino @@ -1,72 +1,73 @@ -/* - This sketch shows how to configure the output - */ - -#include - -//-- Input parameters: - -// Gyro settings: -#define LP_FILTER 3 // Low pass filter. Value from 0 to 6 -#define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3 -#define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3 - -// Accelerometer offset: -constexpr int AX_OFFSET = 552; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. -constexpr int AY_OFFSET = -241; // These values are unlikely to be zero. -constexpr int AZ_OFFSET = -3185; - -// Output scale: -constexpr float AX_SCALE = 1.00457; // Multiplier for accelerometer outputs. Use this to calibrate the sensor. If unknown set to 1. -constexpr float AY_SCALE = 0.99170; -constexpr float AZ_SCALE = 0.98317; - -constexpr float GX_SCALE = 0.99764; // Multiplier to gyro outputs. Use this to calibrate the sensor. If unknown set to 1. -constexpr float GY_SCALE = 1.0; -constexpr float GZ_SCALE = 1.01037; - -// Bias estimate: -#define GYRO_BAND 35 // Standard deviation of the gyro signal. Gyro signals within this band (relative to the mean) are suppresed. -#define BIAS_COUNT 5000 // Samples of the mean of the gyro signal. Larger values provide better calibration but delay suppression response. - -//-- Set the template parameters: - -basicMPU6050imu; - -void setup() { - // Set registers - Always required - imu.setup(); - - // Initial calibration of gyro - imu.setBias(); - - // Start console - Serial.begin(38400); -} - -void loop() { - // Update gyro calibration - imu.updateBias(); - - //-- Scaled and calibrated output: - // Accel - Serial.print( imu.ax() ); - Serial.print( " " ); - Serial.print( imu.ay() ); - Serial.print( " " ); - Serial.print( imu.az() ); - Serial.print( " " ); - - // Gyro - Serial.print( imu.gx() ); - Serial.print( " " ); - Serial.print( imu.gy() ); - Serial.print( " " ); - Serial.print( imu.gz() ); - Serial.println(); -} +/* + This sketch shows how to configure the output + */ + +#include + +//-- Input parameters: + +// Gyro settings: +#define LP_FILTER 3 // Low pass filter. Value from 0 to 6 +#define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3 +#define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3 +#define ADDRESS_A0 HIGH // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW + // A0 -> 5v : ADDRESS_A0 = HIGH +// Accelerometer offset: +constexpr int AX_OFFSET = 552; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. +constexpr int AY_OFFSET = -241; // These values are unlikely to be zero. +constexpr int AZ_OFFSET = -3185; + +// Output scale: +constexpr float AX_SCALE = 1.00457; // Multiplier for accelerometer outputs. Use this to calibrate the sensor. If unknown set to 1. +constexpr float AY_SCALE = 0.99170; +constexpr float AZ_SCALE = 0.98317; + +constexpr float GX_SCALE = 0.99764; // Multiplier to gyro outputs. Use this to calibrate the sensor. If unknown set to 1. +constexpr float GY_SCALE = 1.0; +constexpr float GZ_SCALE = 1.01037; + +// Bias estimate: +#define GYRO_BAND 35 // Standard deviation of the gyro signal. Gyro signals within this band (relative to the mean) are suppresed. +#define BIAS_COUNT 5000 // Samples of the mean of the gyro signal. Larger values provide better calibration but delay suppression response. + +//-- Set the template parameters: + +basicMPU6050imu; + +void setup() { + // Set registers - Always required + imu.setup(); + + // Initial calibration of gyro + imu.setBias(); + + // Start console + Serial.begin(38400); +} + +void loop() { + // Update gyro calibration + imu.updateBias(); + + //-- Scaled and calibrated output: + // Accel + Serial.print( imu.ax() ); + Serial.print( " " ); + Serial.print( imu.ay() ); + Serial.print( " " ); + Serial.print( imu.az() ); + Serial.print( " " ); + + // Gyro + Serial.print( imu.gx() ); + Serial.print( " " ); + Serial.print( imu.gy() ); + Serial.print( " " ); + Serial.print( imu.gz() ); + Serial.println(); +} diff --git a/examples/raw_output/raw_output.ino b/examples/raw_output/raw_output.ino old mode 100644 new mode 100755 diff --git a/keywords.txt b/keywords.txt old mode 100644 new mode 100755 index 28a4d32..c2349b7 --- a/keywords.txt +++ b/keywords.txt @@ -2,3 +2,5 @@ basicMPU6050 KEYWORD1 setup KEYWORD2 setBias KEYWORD2 updateBias KEYWORD2 + +# NOTE: seperate keywords with single tab diff --git a/library.properties b/library.properties old mode 100644 new mode 100755 index fddd72e..326e6bd --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=basicMPU6050 -version=0.1.0 +version=0.2.0 author=RCmags maintainer=RCmags -sentence=light library for the MPU6050. -paragraph=Lightweight library to configure and retrieve the raw sensor outputs of the MPU6050. It includes simples routines to calibrate the gyro. +sentence=lightweight library for the MPU6050. +paragraph=library to configure and retrieve the raw sensor outputs of the MPU6050. It includes simples routines to calibrate the gyro. category=Device Control url=https://github.com/RCmags/basicMPU6050.git architectures=* diff --git a/src/basicMPU6050.h b/src/basicMPU6050.h old mode 100644 new mode 100755 index 2c54966..8f7eca6 --- a/src/basicMPU6050.h +++ b/src/basicMPU6050.h @@ -1,132 +1,131 @@ -#include "Arduino.h" -#include - -#ifndef basicMPU6050_h -#define basicMPU6050_h - -//------------------ Common terms -------------------- - -// Sensor values -#define MPU_ADDRESS 0x68 -#define ACCEL_LBS_0 16384.0 -#define N_AXIS 3 - -//---------------- Default settings ------------------ [ No characters after backlash! ] - -constexpr float DEFAULT_SCALE = 1; // Default scale of Gyro and Accelerometer - -#define TEMPLATE_DEFAULT \ - uint8_t DLPF_CFG = 6 , /* Low pass filter setting - 0 to 6 */ \ - uint8_t FS_SEL = 0 , /* Gyro sensitivity - 0 to 3 */ \ - uint8_t AFS_SEL = 0 , /* Accelerometer sensitivity - 0 to 3 */ \ - int16_t AX_OFS = 0 , /* Accel X offset */ \ - int16_t AY_OFS = 0 , /* Accel Y offset */ \ - int16_t AZ_OFS = 0 , /* Accel Z offset */ \ - const float* AX_S = &DEFAULT_SCALE , /* Accel X Multiplier */ \ - const float* AY_S = &DEFAULT_SCALE , /* Accel Y Multiplier */ \ - const float* AZ_S = &DEFAULT_SCALE , /* Accel Z Multiplier */ \ - const float* GX_S = &DEFAULT_SCALE , /* Gyro X Multiplier */ \ - const float* GY_S = &DEFAULT_SCALE , /* Gyro Y Multiplier */ \ - const float* GZ_S = &DEFAULT_SCALE , /* Gyro Z Multiplier */ \ - uint16_t GYRO_BAND = 16 , /* Standard deviation of gyro signals */ \ - uint32_t N_BIAS = 10000 /* Samples of average used to calibrate gyro*/ - -//--------------- Template Parameters ---------------- [ No characters after backlash! ] - -#define TEMPLATE_TYPE \ - uint8_t DLPF_CFG ,\ - uint8_t FS_SEL ,\ - uint8_t AFS_SEL ,\ - int16_t AX_OFS ,\ - int16_t AY_OFS ,\ - int16_t AZ_OFS ,\ - const float* AX_S ,\ - const float* AY_S ,\ - const float* AZ_S ,\ - const float* GX_S ,\ - const float* GY_S ,\ - const float* GZ_S ,\ - uint16_t GYRO_BAND ,\ - uint32_t N_BIAS - -#define TEMPLATE_INPUTS \ - DLPF_CFG ,\ - FS_SEL ,\ - AFS_SEL ,\ - AX_OFS ,\ - AY_OFS ,\ - AZ_OFS ,\ - AX_S ,\ - AY_S ,\ - AZ_S ,\ - GX_S ,\ - GY_S ,\ - GZ_S ,\ - GYRO_BAND ,\ - N_BIAS - -//---------------- Class definition ------------------ - -template -class basicMPU6050 { - private: - float mean[N_AXIS] = {0}; - float var = 0; - - // Common settings - static const float ACCEL_LBS; - static const float GYRO_LBS; - static const float MEAN; - - // I2C communication - void setRegister(uint8_t, uint8_t); - void readRegister( uint8_t ); - int readWire(); - - public: - void setup(); - - //-- Raw measurements - - // Accel - int rawAx(); - int rawAy(); - int rawAz(); - - // Temp - int rawTemp(); - - // Gyro - int rawGx(); - int rawGy(); - int rawGz(); - - //-- Scaled measurements - - // Accel - float ax(); - float ay(); - float az(); - - // Temp - float temp(); - - // Gyro - float gx(); - float gy(); - float gz(); - - //-- Gyro bias estimate - void setBias(); - void updateBias(); -}; - -#include "basicMPU6050.tpp" - -//----------------- Clearing labels ------------------ - -#undef TEMPLATE_TYPE -#undef TEMPLATE_INPUTS -#undef TEMPLATE_DEFAULT - -#endif \ No newline at end of file +#include "Arduino.h" +#include + +#ifndef basicMPU6050_h +#define basicMPU6050_h + +//------------------ Common terms -------------------- + +// Sensor values +#define MPU_ADDRESS_LOW 0x68 +#define MPU_ADDRESS_HIGH 0x69 +#define ACCEL_LSB_0 16384.0 +#define N_AXIS 3 + +//---------------- Default settings ------------------ [ No characters after backlash! ] + +constexpr float DEFAULT_SCALE = 1; // Default scale of Gyro and Accelerometer + +#define TEMPLATE_DEFAULT \ + uint8_t DLPF_CFG = 6 , /* Low pass filter setting - 0 to 6 */ \ + uint8_t FS_SEL = 0 , /* Gyro sensitivity - 0 to 3 */ \ + uint8_t AFS_SEL = 0 , /* Accelerometer sensitivity - 0 to 3 */ \ + bool ADDRESS_A0 = LOW , /* State of A0 pin to set I2C address */ \ + int16_t AX_OFS = 0 , /* Accel X offset */ \ + int16_t AY_OFS = 0 , /* Accel Y offset */ \ + int16_t AZ_OFS = 0 , /* Accel Z offset */ \ + const float* AX_S = &DEFAULT_SCALE , /* Accel X Multiplier */ \ + const float* AY_S = &DEFAULT_SCALE , /* Accel Y Multiplier */ \ + const float* AZ_S = &DEFAULT_SCALE , /* Accel Z Multiplier */ \ + const float* GX_S = &DEFAULT_SCALE , /* Gyro X Multiplier */ \ + const float* GY_S = &DEFAULT_SCALE , /* Gyro Y Multiplier */ \ + const float* GZ_S = &DEFAULT_SCALE , /* Gyro Z Multiplier */ \ + uint16_t GYRO_BAND = 16 , /* Standard deviation of gyro signals */ \ + uint32_t N_BIAS = 10000 /* Samples of average used to calibrate gyro*/ + +//--------------- Template Parameters ---------------- [ No characters after backlash! ] + +#define TEMPLATE_TYPE \ + uint8_t DLPF_CFG ,\ + uint8_t FS_SEL ,\ + uint8_t AFS_SEL ,\ + bool ADDRESS_A0 ,\ + int16_t AX_OFS ,\ + int16_t AY_OFS ,\ + int16_t AZ_OFS ,\ + const float* AX_S ,\ + const float* AY_S ,\ + const float* AZ_S ,\ + const float* GX_S ,\ + const float* GY_S ,\ + const float* GZ_S ,\ + uint16_t GYRO_BAND ,\ + uint32_t N_BIAS + +#define TEMPLATE_INPUTS \ + DLPF_CFG ,\ + FS_SEL ,\ + AFS_SEL ,\ + ADDRESS_A0 ,\ + AX_OFS ,\ + AY_OFS ,\ + AZ_OFS ,\ + AX_S ,\ + AY_S ,\ + AZ_S ,\ + GX_S ,\ + GY_S ,\ + GZ_S ,\ + GYRO_BAND ,\ + N_BIAS + +//---------------- Class definition ------------------ + +template +class basicMPU6050 { + private: + float mean[N_AXIS] = {0}; + float var = 0; + + // Common settings + static const uint8_t MPU_ADDRESS; + static const float ACCEL_LSB; + static const float GYRO_LSB; + static const float MEAN; + + // I2C communication + void setRegister(uint8_t, uint8_t); + void readRegister(uint8_t); + int readWire(); + + public: + void setup(); + + //-- Raw measurements + + // Accel + int rawAx(); + int rawAy(); + int rawAz(); + + // Temp + int rawTemp(); + + // Gyro + int rawGx(); + int rawGy(); + int rawGz(); + + //-- Scaled measurements + + // Accel + float ax(); + float ay(); + float az(); + + // Temp + float temp(); + + // Gyro + float gx(); + float gy(); + float gz(); + + //-- Gyro bias estimate + void setBias(); + void updateBias(); +}; + +#include "basicMPU6050.tpp" + +#endif diff --git a/src/basicMPU6050.tpp b/src/basicMPU6050.tpp old mode 100644 new mode 100755 index d872628..1f4be5a --- a/src/basicMPU6050.tpp +++ b/src/basicMPU6050.tpp @@ -1,225 +1,229 @@ -//--------------------- Constants -------------------- - -template -const float basicMPU6050 -::MEAN = 1.0/float(N_BIAS); // Inverse of sample count - -template -const float basicMPU6050 -::ACCEL_LBS = AFS_SEL < 0 || AFS_SEL > 3 ? 1 : // Scaling factor for accelerometer. Depends on sensitivity setting. - 1.0/( AFS_SEL == 0 ? 16384.0 : // Output is in: g - AFS_SEL == 1 ? 8192.0 : - AFS_SEL == 2 ? 4096.0 : - 2048.0 ); -template -const float basicMPU6050 -::GYRO_LBS = FS_SEL < 0 || FS_SEL > 3 ? 1 : // Scaling factor for gyro. Depends on sensitivity setting. - (PI/180.0)/( FS_SEL == 0 ? 131.0 : // Output is in: rad/s - FS_SEL == 1 ? 65.5 : - FS_SEL == 2 ? 32.8 : - 16.4 ); - -//----------------- I2C communication ----------------- - -template -void basicMPU6050 -::setRegister( uint8_t reg, uint8_t mode ) { - Wire.beginTransmission(MPU_ADDRESS); - Wire.write(reg); - Wire.write(mode); - Wire.endTransmission(); -} - -template -void basicMPU6050 -::readRegister( uint8_t reg ) { - Wire.beginTransmission(MPU_ADDRESS); - Wire.write(reg); - Wire.endTransmission(); - Wire.requestFrom( MPU_ADDRESS, 2, true ); -} - -template -int basicMPU6050 -::readWire() { - return int( Wire.read()<<8|Wire.read() ); -} - -template -void basicMPU6050 -::setup() { - // Enable communication - Wire.begin(); - - // Enable sensor - setRegister( 0x6B, 0x00 ); - - // Low pass filter - setRegister( 0x1A, DLPF_CFG <= 6 ? DLPF_CFG : 0x00 ); - - // Gyro sensitivity - setRegister( 0x1B, FS_SEL == 1 ? 0x08 : - FS_SEL == 2 ? 0x10 : - FS_SEL == 3 ? 0x18 : - 0x00 ); - // Accel sensitivity - setRegister( 0x1C, AFS_SEL == 1 ? 0x08 : - AFS_SEL == 2 ? 0x10 : - AFS_SEL == 3 ? 0x18 : - 0x00 ); -} - -//---------------- Raw measurements ------------------ - -// Acceleration -template -int basicMPU6050 -::rawAx() { - readRegister( 0x3B ); - return readWire(); -} - -template -int basicMPU6050 -::rawAy() { - readRegister( 0x3D ); - return readWire(); -} - -template -int basicMPU6050 -::rawAz() { - readRegister( 0x3F ); - return readWire(); -} - -// Temperature -template -int basicMPU6050 -::rawTemp() { - readRegister( 0x41 ); - return readWire(); -} - -// Gyro -template -int basicMPU6050 -::rawGx() { - readRegister( 0x43 ); - return readWire(); -} - -template -int basicMPU6050 -::rawGy() { - readRegister( 0x45 ); - return readWire(); -} - -template -int basicMPU6050 -::rawGz() { - readRegister( 0x47 ); - return readWire(); -} - -//--------------- Scaled measurements ---------------- - -// Acceleration -template -float basicMPU6050 -::ax() { - const float SCALE = (*AX_S) * ACCEL_LBS; - const float OFFSET = (*AX_S) * float(AX_OFS)/ACCEL_LBS_0; - - return float( rawAx() )*SCALE - OFFSET; -} - -template -float basicMPU6050 -::ay() { - const float SCALE = (*AY_S) * ACCEL_LBS; - const float OFFSET = (*AY_S) * float(AY_OFS)/ACCEL_LBS_0; - - return float( rawAy() )*SCALE - OFFSET; -} - -template -float basicMPU6050 -::az() { - const float SCALE = (*AZ_S) * ACCEL_LBS; - const float OFFSET = (*AZ_S) * float(AZ_OFS)/ACCEL_LBS_0; - - return float( rawAz() )*SCALE - OFFSET; -} - -// Temperature -template -float basicMPU6050 -::temp() { - const float TEMP_MUL = 1.0/340.0; - return rawTemp()*TEMP_MUL + 36.53; -} - -// Gyro -template -float basicMPU6050 -::gx() { - const float SCALE = (*GX_S) * GYRO_LBS; - return ( float( rawGx() ) - mean[0] )*SCALE; -} -template -float basicMPU6050 -::gy() { - const float SCALE = (*GY_S) * GYRO_LBS; - return ( float( rawGy() ) - mean[1] )*SCALE; -} - -template -float basicMPU6050 -::gz() { - const float SCALE = (*GZ_S) * GYRO_LBS; - return ( float( rawGz() ) - mean[2] )*SCALE; -} - -//--------------- Gyro bias estimate ----------------- - -template -void basicMPU6050 -::setBias() { - for( int count = 0; count < N_BIAS; count += 1 ) { - int gyro[] = { rawGx(), rawGy(), rawGz() }; - - // Sum all samples - for( int index = 0; index < N_AXIS; index += 1 ) { - mean[index] += float( gyro[index] ); - } - } - - // Divide sums by sample count - for( int index = 0; index < N_AXIS; index += 1 ) { - mean[index] *= MEAN; - } -} - -template -void basicMPU6050 -::updateBias() { - const float BAND_SQ = GYRO_BAND*GYRO_BAND; - - // Error in reading - float dw[N_AXIS] = { rawGx() - mean[0] , - rawGy() - mean[1] , - rawGz() - mean[2] }; - - // Calculate kalman gain - float mag = dw[0]*dw[0] + dw[1]*dw[1] + dw[2]*dw[2]; - float gain = BAND_SQ/( BAND_SQ + var + mag ); - - var += mag + (gain - 1)*var; // covariance in the magnitude of gyro signal - - // Update mean with gain - for( int index = 0; index < N_AXIS; index += 1 ) { - mean[index] += dw[index]*gain * MEAN; - } -} \ No newline at end of file +//--------------------- Constants -------------------- + +template +const uint8_t basicMPU6050 +::MPU_ADDRESS = ADDRESS_A0 == HIGH ? MPU_ADDRESS_HIGH : MPU_ADDRESS_LOW; + +template +const float basicMPU6050 +::MEAN = 1.0/float(N_BIAS); // Inverse of sample count + +template +const float basicMPU6050 +::ACCEL_LSB = AFS_SEL < 0 || AFS_SEL > 3 ? 1 : // Scaling factor for accelerometer. Depends on sensitivity setting. + 1.0/( AFS_SEL == 0 ? 16384.0 : // Output is in: g + AFS_SEL == 1 ? 8192.0 : + AFS_SEL == 2 ? 4096.0 : + 2048.0 ); +template +const float basicMPU6050 +::GYRO_LSB = FS_SEL < 0 || FS_SEL > 3 ? 1 : // Scaling factor for gyro. Depends on sensitivity setting. + (PI/180.0)/( FS_SEL == 0 ? 131.0 : // Output is in: rad/s + FS_SEL == 1 ? 65.5 : + FS_SEL == 2 ? 32.8 : + 16.4 ); + +//----------------- I2C communication ----------------- + +template +void basicMPU6050 +::setRegister( uint8_t reg, uint8_t mode ) { + Wire.beginTransmission(MPU_ADDRESS); + Wire.write(reg); + Wire.write(mode); + Wire.endTransmission(); +} + +template +void basicMPU6050 +::readRegister( uint8_t reg ) { + Wire.beginTransmission(MPU_ADDRESS); + Wire.write(reg); + Wire.endTransmission(); + Wire.requestFrom( MPU_ADDRESS, uint8_t(2), uint8_t(true) ); +} + +template +int basicMPU6050 +::readWire() { + return int( Wire.read()<<8|Wire.read() ); +} + +template +void basicMPU6050 +::setup() { + // Turn on communication + Wire.begin(); + + // Enable sensor + setRegister( 0x6B, 0x00 ); + + // Low pass filter + setRegister( 0x1A, DLPF_CFG <= 6 ? DLPF_CFG : 0x00 ); + + // Gyro sensitivity + setRegister( 0x1B, FS_SEL == 1 ? 0x08 : + FS_SEL == 2 ? 0x10 : + FS_SEL == 3 ? 0x18 : + 0x00 ); + // Accel sensitivity + setRegister( 0x1C, AFS_SEL == 1 ? 0x08 : + AFS_SEL == 2 ? 0x10 : + AFS_SEL == 3 ? 0x18 : + 0x00 ); +} + +//---------------- Raw measurements ------------------ + +// Acceleration +template +int basicMPU6050 +::rawAx() { + readRegister( 0x3B ); + return readWire(); +} + +template +int basicMPU6050 +::rawAy() { + readRegister( 0x3D ); + return readWire(); +} + +template +int basicMPU6050 +::rawAz() { + readRegister( 0x3F ); + return readWire(); +} + +// Temperature +template +int basicMPU6050 +::rawTemp() { + readRegister( 0x41 ); + return readWire(); +} + +// Gyro +template +int basicMPU6050 +::rawGx() { + readRegister( 0x43 ); + return readWire(); +} + +template +int basicMPU6050 +::rawGy() { + readRegister( 0x45 ); + return readWire(); +} + +template +int basicMPU6050 +::rawGz() { + readRegister( 0x47 ); + return readWire(); +} + +//--------------- Scaled measurements ---------------- + +// Acceleration +template +float basicMPU6050 +::ax() { + const float SCALE = (*AX_S) * ACCEL_LSB; + const float OFFSET = (*AX_S) * float(AX_OFS)/ACCEL_LSB_0; + + return float( rawAx() )*SCALE - OFFSET; +} + +template +float basicMPU6050 +::ay() { + const float SCALE = (*AY_S) * ACCEL_LSB; + const float OFFSET = (*AY_S) * float(AY_OFS)/ACCEL_LSB_0; + + return float( rawAy() )*SCALE - OFFSET; +} + +template +float basicMPU6050 +::az() { + const float SCALE = (*AZ_S) * ACCEL_LSB; + const float OFFSET = (*AZ_S) * float(AZ_OFS)/ACCEL_LSB_0; + + return float( rawAz() )*SCALE - OFFSET; +} + +// Temperature +template +float basicMPU6050 +::temp() { + const float TEMP_MUL = 1.0/340.0; + return rawTemp()*TEMP_MUL + 36.53; +} + +// Gyro +template +float basicMPU6050 +::gx() { + const float SCALE = (*GX_S) * GYRO_LSB; + return ( float( rawGx() ) - mean[0] )*SCALE; +} +template +float basicMPU6050 +::gy() { + const float SCALE = (*GY_S) * GYRO_LSB; + return ( float( rawGy() ) - mean[1] )*SCALE; +} + +template +float basicMPU6050 +::gz() { + const float SCALE = (*GZ_S) * GYRO_LSB; + return ( float( rawGz() ) - mean[2] )*SCALE; +} + +//--------------- Gyro bias estimate ----------------- + +template +void basicMPU6050 +::setBias() { + for( int count = 0; count < N_BIAS; count += 1 ) { + int gyro[] = { rawGx(), rawGy(), rawGz() }; + + // Sum all samples + for( int index = 0; index < N_AXIS; index += 1 ) { + mean[index] += float( gyro[index] ); + } + } + + // Divide sums by sample count + for( int index = 0; index < N_AXIS; index += 1 ) { + mean[index] *= MEAN; + } +} + +template +void basicMPU6050 +::updateBias() { + const float BAND_SQ = GYRO_BAND*GYRO_BAND; + + // Error in reading + float dw[N_AXIS] = { rawGx() - mean[0] , + rawGy() - mean[1] , + rawGz() - mean[2] }; + + // Calculate kalman gain + float mag = dw[0]*dw[0] + dw[1]*dw[1] + dw[2]*dw[2]; + float gain = BAND_SQ/( BAND_SQ + var + mag ); + + var += mag + (gain - 1)*var; // covariance in the magnitude of gyro signal + + // Update mean with gain + for( int index = 0; index < N_AXIS; index += 1 ) { + mean[index] += dw[index]*gain * MEAN; + } +}