ESP32 Forum

ESP32 Official Forum
http://esp32.io/

Help with Converting the raw values from a BMX055 on a ESP32.

http://esp32.io/viewtopic.php?f=19&t=4741

Page 1 of 1

Help with Converting the raw values from a BMX055 on a ESP32.

Posted: Sat Feb 17, 2018 2:04 pm
by macd200
Hello Everyone

I was hoping some of you lovely people on here, would be able to tell what i'm doing wrong, with my code.
Currently I am trying to build a self leveling quadcopter, using an ESP32, and BMX055 (which is a 9 DOF sensor), I have no issues reading the data off the Sensor, and converting that into degrees for my pitch, roll and yaw. The problem starts when I start moving the sensor on more than one axis, for example tilting it up say 60 Degrees in pitch then spinning it 40 Degrees in a Z direction, when i put it down my roll is at a -50 or so value while my pitch goes back to zero, the yaw value is correct it's just when i use the roll and pitch together that they go all crazy. My code started out ok but after the last week of fault finding and searching the internet, and trying other peoples codes from other examples I have hit a dead end. Also to note I have a bmt280 in there which presented no issues at all when I added the code in for that. Also my Acceleration values are fine, currently have them //commented out to help with the fault finding.
Any help would be much appreciated.

Code: Select all

#include <Wire.h>
#include <Arduino.h>
#include <Math.h>



//Declaring some global variables
int16_t gyro_x, gyro_y, gyro_z;
int32_t pressure_raw;
int32_t bmt280_temp;
int acc_x, acc_y, acc_z, acc_total_vector;
int temperature;
int16_t gyro_x_cal, gyro_y_cal, gyro_z_cal;
long gyro_x_count, gyro_y_count, gyro_z_count;
long loop_timer, current_time;
int lcd_loop_counter;

float angle_pitch_buffer, angle_roll_buffer;
bool set_gyro_angles;
float angle_roll_acc, angle_pitch_acc, angle_yaw_acc;
float angle_pitch_output, angle_roll_output, angle_yaw_output;
float acc_roll, acc_pitch;
int magnet_x, magnet_y, magnet_z;
float combine_pitch, combine_roll, combine_yaw;


void read_bmt280(){
  int16_t data[5];

  for (int i = 0; i < 5; i++){
    Wire.beginTransmission(0x76);
    Wire.write((247 + i));
    Wire.endTransmission();
    Wire.requestFrom(0x76, 1);

    if (Wire.available() == 1) {
      data[i] = Wire.read();
    }
  }

  pressure_raw = data[0]<<8|data[1]<<16|data[2];
  pressure_raw = pressure_raw >> 4;
  bmt280_temp = data[3]<<8|data[4]<<16|data[5];
  bmt280_temp = pressure_raw >> 4;
  pressure_raw *= 0.16;


}

void setup_bmt280(){
  Wire.beginTransmission(0x76);
  Wire.write(0xF4);  //Set-up regigster
  Wire.write(0x37);  // x16 pressure sampling x1 temperature and normal porwer_m
  Wire.endTransmission();
}

void read_mpu_6050_data(){
  int16_t data[7];


  for (int i = 0; i < 7; i++)
  {
    Wire.beginTransmission(0x19);
    Wire.write((2 + i));
    Wire.endTransmission();

    Wire.requestFrom(0x19, 1);

    if (Wire.available() == 1)
    {
      data[i] = Wire.read();
    }
}

  acc_x = data[1]<<8|data[0];
  acc_y = data[3]<<8|data[2];
  acc_z = data[5]<<8|data[4];
  temperature = data[6];
  acc_x = acc_x >> 4;
  acc_y = acc_y >> 4;
  acc_z = acc_z >> 4;
  if (acc_x > 2047) {
    acc_x -= 4096;
  }
  if (acc_y > 2047) {
    acc_y -= 4096;
  }
  if (acc_z > 2047) {
    acc_z -= 4096;
  }

  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(0x69);
    Wire.write((2 + i));
    Wire.endTransmission();

    Wire.requestFrom(0x69, 1);

    if (Wire.available() == 1)
    {
      data[i] = Wire.read();
    }
}
gyro_x = data[1]<<8|data[0];
if (gyro_x > 32767)
{
  gyro_x -= 65536;
}
gyro_y = data[3]<<8|data[2];
if (gyro_y > 32767)
{
  gyro_y -= 65536;
}
gyro_z = data[5]<<8|data[4];
if (gyro_z > 32767)
{
  gyro_z -= 65536;
}


for (int i = 0; i < 6; i++)
{
  Wire.beginTransmission(0x10);
  Wire.write((66 + i));
  Wire.endTransmission();

  Wire.requestFrom(0x10, 1);

  if (Wire.available() == 1)
  {
    data[i] = Wire.read();
  }
}

magnet_x = data[1]<<8|data[0];
magnet_y = data[3]<<8|data[2];
magnet_z = data[5]<<8|data[4];
magnet_x = magnet_x >> 3;
magnet_y = magnet_y >> 3;
magnet_z = magnet_z >> 1;
if (magnet_x > 4095) {
  magnet_x -= 8192;
}
if (magnet_y > 4095) {
  magnet_y -= 8192;
}
if (magnet_z > 16383) {
  magnet_z -= 32768;
}





Wire.endTransmission();
}


 void setup_mpu_6050_registers(){
  //Activate the MPU-6050
  Wire.beginTransmission(0x19);
  // Select PMU_Range register
  Wire.write(0x0F);
  // Range = +/- 8g
  Wire.write(0x08);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x19);
  // Select PMU_BW register
  Wire.write(0x10);
  // Bandwidth = 125 Hz
  Wire.write(0x0C);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x19);
  // Select PMU_LPW register
  Wire.write(0x11);
  // Normal mode, Sleep duration = 0.5ms
  Wire.write(0x00);
  // Stop I2C Transmission on the device
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x69);
  // Select Range register
  Wire.write(0x0F);
  // Full scale = +/- 125 degree/s
  Wire.write(0x00);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x69);
  // Select Bandwidth register
  Wire.write(0x10);
  // ODR = 100 Hz
  Wire.write(0x02);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x69);
  // Select LPM1 register
  Wire.write(0x11);
  // Normal mode, Sleep duration = 2ms
  Wire.write(0x00);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x13);
  // Select Mag register
  Wire.write(0x4B);
  // Soft reset
  Wire.write(0x83);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x13);
  // Select Mag register
  Wire.write(0x4C);
  // Normal Mode, ODR = 10 Hz
  Wire.write(0x00);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x13);
  // Select Mag register
  Wire.write(0x4E);
  // X, Y, Z-Axis enabled
  Wire.write(0x84);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x13);
  // Select Mag register
  Wire.write(0x51);
  // No. of Repetitions for X-Y Axis = 9
  Wire.write(0x04);
  // Stop I2C Transmission
  Wire.endTransmission();

  // Start I2C Transmission
  Wire.beginTransmission(0x13);
  // Select Mag register
  Wire.write(0x52);
  // No. of Repetitions for Z-Axis = 15
  Wire.write(0x0F);
  // Stop I2C Transmission
  Wire.endTransmission();                                            //End the transmission
//  Configure the accelerometer (+/-8g)
  /*Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1C);                                                    //Send the requested starting register
  Wire.write(0x01);                                                    //Set the requested starting register
  Wire.endTransmission();                                              //End the transmission
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1B);                                                    //Send the requested starting register
  Wire.write(0xE0);                                                    //Set the requested starting register
  Wire.endTransmission(); */
  delay(1000);
                                                //End the transmission
}




void setup() {

  Serial.begin(115200);
  Serial.println("Serial Connection Established!");
  Wire.begin(21,22);
  Wire.setClock(400000);
  Serial.println("Wire Connection Established!");                                             //Use only for debugging

  Serial.print("Setting up BMX055...");
  setup_mpu_6050_registers();
  Serial.println("Complete");
  Serial.print("Setting up BMP280...");
  setup_bmt280();
  Serial.print("Complete!");
  Wire.endTransmission();
  delay(500);
  Serial.print("Calibrating.");
  int i = 0;
  for (int i = 0; i < 550; i++ ) {
    read_mpu_6050_data();
    if (i > 50) {
    gyro_x_count += gyro_x;
    gyro_y_count += gyro_y;
    gyro_z_count += gyro_z;
    Serial.print(".");
  }

    delay(10);
  }
  gyro_x_cal = gyro_x_count / 500;
  gyro_y_cal = gyro_y_count / 500;
  gyro_z_cal = gyro_z_count / 500;



}

void loop(){



  read_mpu_6050_data();
  read_bmt280();
  //Gyroscope Calculations
  gyro_x = gyro_x - gyro_x_cal;
  gyro_y = gyro_y - gyro_y_cal;
  gyro_z = gyro_z - gyro_z_cal;

  angle_roll_acc = (gyro_x / 16.4) / 100;
  angle_roll_output += angle_roll_acc;
  angle_pitch_acc = (gyro_y / 16.4) / 100;
  angle_pitch_output += angle_pitch_acc;
  angle_yaw_acc = (gyro_z / 16.4) / 100;
  angle_yaw_output += angle_yaw_acc;

  angle_pitch_output += angle_roll_output * sin (gyro_z * 0.000001066); //Problem I think
  angle_roll_output -= angle_pitch_output * sin (gyro_z * 0.000001066); // And here


  //Acceleromerter Angles

  acc_roll = 180  * atan(acc_y/sqrt(acc_x*acc_x + acc_z*acc_z))/3.14159265358979323846;
  acc_pitch = 180 * atan(acc_x/sqrt(acc_y*acc_y + acc_z*acc_z))/3.14159265358979323846;

  //Combining Angles

  combine_roll = angle_roll_output;// * 0.9992 + acc_roll * 0.0008;
  combine_pitch = angle_pitch_output;// * 0.9992 + acc_pitch * 0.0008;




  Serial.print("roll   "); Serial.print(combine_roll); Serial.print("  pitch   "); Serial.print(combine_pitch); Serial.print("  Yaw   ");
  Serial.print(angle_yaw_output); Serial.print("  Pressure  "); Serial.print(pressure_raw); Serial.print("  Scan time  ");
  Serial.println((micros() - loop_timer));
  /*Serial.print(angle_roll_output); Serial.print(" - "); Serial.print(angle_pitch_output); Serial.print(" _ "); Serial.print(angle_yaw_output);
  Serial.print(" _ "); Serial.print(acc_roll); Serial.print(" _ "); Serial.print(acc_pitch);
  Serial.print("__"); Serial.print(acc_total_vector); Serial.print(" - "); Serial.print(magnet_x); Serial.print(" - "); Serial.print(magnet_y); Serial.print(" - "); Serial.print(magnet_z);
  Serial.print("_-_Temp-");  Serial.println(temperature);      */                                                      //Write the roll and pitch values to the LCD display

  while(micros() - loop_timer < 10000);                                 //Wait until the loop_timer reaches 10000us (100Hz) before starting the next loop
  loop_timer = micros();                                               //Reset the loop timer
}
All times are UTC
Page 1 of 1
Powered by phpBB® Forum Software © phpBB Limited
https://www.phpbb.com/