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
}