use a unique calcOffsets() method for gyro and accelero

szorcc · Nov 15, 2020 · 6bca2da · 6bca2da
1 parent 2ff268a
commit 6bca2da
Show file tree

Hide file tree

Showing 5 changed files with 58 additions and 70 deletions.
diff --git a/README.md b/README.md
@@ -16,13 16,12 @@ The default complementary filter is `0.98` for the gyro data and `0.02` for the
 
 The important methods of the MPU6050 are the following ones.
 
-| FUNCTION | DESCRIPTION |
-|---------------------|------------------------------------------------------------|
-| MPU6050(Wire) | Constructor initialised with default complementary filter |
-| MPU6050(Wire,ac,gc) | Constructor initialised with custom complementary filter |
-| begin() | Start communication with MPU6050 |
-| calcGyroOffsets() | Compute gyro offset. Device must be stable meanwhile |
-| update() | Update data. Must be called often to get consistent angles |
 | FUNCTION | DESCRIPTION |
 |------------------------|------------------------------------------------------------|
 | MPU6050(Wire) | Constructor |
 | begin() | Start communication with MPU6050 |
 | calcOffsets(bool,bool) | Compute offsets. Device must be stable meanwhile |
 | update() | Update data. Must be called often to get consistent angles |
 
 All data is available through the following getters. They all return a `float`.
 
@@ -36,9 35,10 @@ All data is available through the following getters. They all return a `float`.
 
 The gyro offsets can also be provided by the user if they are already known.
 
-| FUNCTION | DESCRIPTION | UNIT |
-|--------------------------|----------------------------------------------|------------|
-| setGyroOffsets(gx,gy,gz) | Set gyro offsets on X, Y or Z | deg/s |
 | FUNCTION | DESCRIPTION | UNIT |
 |--------------------------|----------------------------------------------|---------------|
 | setGyroOffsets(gx,gy,gz) | Set gyro offsets on X, Y or Z | deg/s |
 | setAccOffsets(ax,ay,az) | Set accelero offsets on X, Y or Z | g (=9.81m/s²) |
 
 ## Examples
 
@@ -52,7 52,7 @@ MPU6050 mpu(Wire);
 void setup() {
  Wire.begin();
  mpu.begin();
- mpu.calcGyroOffsets();
  mpu.calcOffsets();
 }
 
 void loop() {

diff --git a/examples/GetAllData/GetAllData.ino b/examples/GetAllData/GetAllData.ino
@@ -23,9 23,9 @@ void setup() {
  Serial.println(status);
  while(status!=0){ } // stop everything if could not connect to MPU6050
 
- Serial.println(F("Calculating gyro offset, do not move MPU6050"));
  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
- mpu.calcGyroOffsets();
  mpu.calcOffsets(true,true); // gyro and accelero
  Serial.println("Done!\n");
 
 }
@@ -34,22 34,22 @@ void loop() {
  mpu.update();
 
  if(millis() - timer > 1000){ // print data every second
- Serial.print(F("TEMPERATURE : "));Serial.println(mpu.getTemp());
- Serial.print(F("ACCELERO  X : "));Serial.print(mpu.getAccX());
- Serial.print("\tY : ");Serial.print(mpu.getAccY());
- Serial.print("\tZ : ");Serial.println(mpu.getAccZ());
  Serial.print(F("TEMPERATURE: "));Serial.println(mpu.getTemp());
  Serial.print(F("ACCELERO X: "));Serial.print(mpu.getAccX());
  Serial.print("\tY: ");Serial.print(mpu.getAccY());
  Serial.print("\tZ: ");Serial.println(mpu.getAccZ());
 
- Serial.print(F("GYRO  X : "));Serial.print(mpu.getGyroX());
- Serial.print("\tY : ");Serial.print(mpu.getGyroY());
- Serial.print("\tZ : ");Serial.println(mpu.getGyroZ());
  Serial.print(F("GYRO X: "));Serial.print(mpu.getGyroX());
  Serial.print("\tY: ");Serial.print(mpu.getGyroY());
  Serial.print("\tZ: ");Serial.println(mpu.getGyroZ());
 
- Serial.print(F("ACC ANGLE  X : "));Serial.print(mpu.getAccAngleX());
- Serial.print("\tY : ");Serial.println(mpu.getAccAngleY());
  Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX());
  Serial.print("\tY: ");Serial.println(mpu.getAccAngleY());
 
- Serial.print(F("ANGLE  X : "));Serial.print(mpu.getAngleX());
- Serial.print("\tY : ");Serial.print(mpu.getAngleY());
- Serial.print("\tZ : ");Serial.println(mpu.getAngleZ());
- Serial.println(F("=======================================================\n"));
  Serial.print(F("ANGLE X: "));Serial.print(mpu.getAngleX());
  Serial.print("\tY: ");Serial.print(mpu.getAngleY());
  Serial.print("\tZ: ");Serial.println(mpu.getAngleZ());
  Serial.println(F("=====================================================\n"));
  timer = millis();
  }
 

diff --git a/examples/GetAngle/GetAngle.ino b/examples/GetAngle/GetAngle.ino
@@ -19,9 19,9 @@ void setup() {
  Serial.println(status);
  while(status!=0){ } // stop everything if could not connect to MPU6050
 
- Serial.println(F("Calculating gyro offset, do not move MPU6050"));
  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
- mpu.calcGyroOffsets();
  mpu.calcOffsets(); // gyro and accelero
  Serial.println("Done!\n");
 }
 

diff --git a/src/MPU6050_light.cpp b/src/MPU6050_light.cpp
@@ -75,56 75,38 @@ void MPU6050::setFilterAccCoef(float acc_coeff){
 
 /* CALC OFFSET */
 
-void MPU6050::calcGyroOffsets(){
- float xyz[3] = {0,0,0};
- int16_t b;
 void MPU6050::calcOffsets(bool is_calc_gyro, bool is_calc_acc){
  if(is_calc_gyro){ setGyroOffsets(0,0,0); }
  if(is_calc_acc){ setAccOffsets(0,0,0); }
  float ag[6] = {0,0,0,0,0,0}; // 3*acc, 3*gyro
 
  for(int i = 0; i < CALIB_OFFSET_NB_MES; i ){
- wire->beginTransmission(MPU6050_ADDR);
- wire->write(MPU6050_GYRO_OUT_REGISTER);
- wire->endTransmission(false);
- wire->requestFrom((int)MPU6050_ADDR, 6);
-
- for(int j=0;j<3;j ){
- b = wire->read() << 8;
- b |= wire->read();
- xyz[j] = ((float)b) / GYRO_LSB_2_DEGSEC;
- }
-
- delay(1);
  this->fetchData();
  ag[0] = accX;
  ag[1] = accY;
  ag[2] = (accZ-1.0);
  ag[3] = gyroX;
  ag[4] = gyroY;
  ag[5] = gyroZ;
  delay(1); // wait a little bit between 2 measurements
  }
- gyroXoffset = xyz[0] / CALIB_OFFSET_NB_MES;
- gyroYoffset = xyz[1] / CALIB_OFFSET_NB_MES;
- gyroZoffset = xyz[2] / CALIB_OFFSET_NB_MES;
-}
-
-void MPU6050::calcAccOffsets(){
- float xyz[3] = {0,0,0};
- int16_t b;
 
- for(int i = 0; i < CALIB_OFFSET_NB_MES; i ){
- wire->beginTransmission(MPU6050_ADDR);
- wire->write(MPU6050_ACCEL_OUT_REGISTER);
- wire->endTransmission(false);
- wire->requestFrom((int)MPU6050_ADDR, 6);
-
- for(int j=0;j<3;j ){
- b = wire->read() << 8;
- b |= wire->read();
- xyz[j] = ((float)b) / ACC_LSB_2_G;
- }
-
- delay(1);
  if(is_calc_acc){
  accXoffset = ag[0] / CALIB_OFFSET_NB_MES;
  accYoffset = ag[1] / CALIB_OFFSET_NB_MES;
  accZoffset = ag[2] / CALIB_OFFSET_NB_MES;
  }
 
- accXoffset = xyz[0] / CALIB_OFFSET_NB_MES;
- accYoffset = xyz[1] / CALIB_OFFSET_NB_MES;
- accZoffset = xyz[2] / CALIB_OFFSET_NB_MES - 1.0;
  if(is_calc_gyro){
  gyroXoffset = ag[3] / CALIB_OFFSET_NB_MES;
  gyroYoffset = ag[4] / CALIB_OFFSET_NB_MES;
  gyroZoffset = ag[5] / CALIB_OFFSET_NB_MES;
  }
 }
 
 /* UPDATE */
 
-void MPU6050::update(){
 void MPU6050::fetchData(){
  wire->beginTransmission(MPU6050_ADDR);
  wire->write(MPU6050_ACCEL_OUT_REGISTER);
  wire->endTransmission(false);
@@ -144,7 126,13 @@ void MPU6050::update(){
  gyroX = ((float)rawData[4]) / GYRO_LSB_2_DEGSEC - gyroXoffset;
  gyroY = ((float)rawData[5]) / GYRO_LSB_2_DEGSEC - gyroYoffset;
  gyroZ = ((float)rawData[6]) / GYRO_LSB_2_DEGSEC - gyroZoffset;
 }
 
 void MPU6050::update(){
  // retrieve raw data
  this->fetchData();
 
  // process data to get angles
  float sgZ = (accZ>=0)-(accZ<0);
  angleAccX = atan2(accY, sgZ*sqrt(accZ*accZ accX*accX)) * RAD_2_DEG;
  angleAccY = - atan2(accX, sqrt(accZ*accZ accY*accY)) * RAD_2_DEG;

diff --git a/src/MPU6050_light.h b/src/MPU6050_light.h
@@ -65,8 65,7 @@ class MPU6050{
  byte writeData(byte reg, byte data);
  byte readData(byte reg);
 
- void calcGyroOffsets();
- void calcAccOffsets();
  void calcOffsets(bool is_calc_gyro=true, bool is_calc_acc=true);
 
  // MPU CONFIG SETTER
  void setGyroOffsets(float x, float y, float z);
@@ -106,6 105,7 @@ class MPU6050{
  float getAngleZ(){ return angleZ; };
 
  // INLOOP UPDATE
  void fetchData(); // user should better call 'update' that includes 'fetchData'
  void update();