Changing code from BNO055 to MPU6050

[rozanski_propulsion]

Hello everyone,

I am working on a project that uses a BNO055 for orientation on a rocket. The code has a PID controller implemented for thrust vector control. Anyway, I have been frantically searching the internet for a BNO055, but it would appear that no body sells them anymore. I was wondering if someone could help me "replace" the BNO055 code, so that it would work with a MPU6050.

I have the code below:

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
#include <Servo.h>
#include <Adafruit_BMP280.h>

int P1=23;
int P2=22;
int P3=17;
int P4=16;
int P5=15;
int P6=14;

Adafruit_BMP280 bmp; // I2C
 
Servo pitchServo;
Servo rollServo;

float startAltitude = 0;
float altitudeAGL = 0;
float lastAltitude;
 
float q0;
float q1;
float q2;
float q3;
                        //Change sensitivity of PID controller here!!!!//
float k1=.5;  //P//
float k3=.001 //I//
float k2=70;  //D//

int milliOld;
int milliNew;
int dt;
 
float rollTarget=0;
float rollActual;
float rollError=0; 
float rollErrorOld;
float rollErrorChange;
float rollErrorSlope=0;
float rollErrorArea=0;
float rollServoVal=90;
 
float pitchTarget=0;
float pitchActual;
float pitchError=0;
float pitchErrorOld;
float pitchErrorChange;
float pitchErrorSlope=0;
float pitchErrorArea=0;
float pitchServoVal=90;
 
#define BNO055_SAMPLERATE_DELAY_MS (100)
 
Adafruit_BNO055 myIMU = Adafruit_BNO055();
 
void setup() {
  


  

Serial.begin(115200);
  Serial.println(F("BMP280 test"));

  if (!bmp.begin()) {
    Serial.println(F("Could not find a valid BMP280 sensor, check wiring / I2C address"));
    while (true) yield();
  }

  /* Default settings from datasheet. */
  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
                  Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
                  Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
                  Adafruit_BMP280::FILTER_X16,      /* Filtering. */
                  Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */

             startAltitude = bmp.readAltitude(1013.25);


Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
rollServo.attach(6);
pitchServo.attach(5);
 
rollServo.write(rollServoVal);
delay(20);
pitchServo.write(pitchServoVal);
delay(20);


milliNew=millis(); 

}
 
void loop() {



  

Serial.print(F("Relative altitude = "));
  Serial.print(bmp.readAltitude(1013.25) - startAltitude);  // approximative
  Serial.println(" m");
  delay(1000);

  altitudeAGL=bmp.readAltitude(1013.25)-startAltitude;

  if (altitudeAGL-lastAltitude <= -2) //Checks to see if altitude has dropped by 2
        {digitalWrite P1, HIGH
            }


uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);
 
imu::Quaternion quat=myIMU.getQuat();
 
q0=quat.w();
q1=quat.x();
q2=quat.y();
q3=quat.z();
 
rollActual=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
pitchActual=asin(2*(q0*q2-q3*q1));
 
rollActual=rollActual/(2*3.141592654)*360;
pitchActual=pitchActual/(2*3.141592654)*360;
 
milliOld=milliNew;
milliNew=millis();
dt=milliNew-milliOld;

rollErrorOld=rollError;
rollError=rollTarget-rollActual;
rollErrorChange=rollError-rollErrorOld;
rollErrorSlope=rollErrorChange/dt;
rollErrorArea=rollErrorArea+rollError*dt;

pitchErrorOld=pitchError;
pitchError=pitchTarget-pitchActual;
pitchErrorChange=pitchError-pitchErrorOld;
pitchErrorSlope=pitchErrorChange/dt;
pitchErrorArea=pitchErrorArea+pitchError*dt;

rollServoVal=rollServoVal+k1*rollError+k2*rollErrorSlpoe+k3*rollErrorArea;
rollServo.write(rollServoVal);

pitchServoVal=pitchServoVal+k1*pitchError+k2*pitchErrorSlope+k3*pitchErrorArea;
pitchServo.write(pitchServoVal);


Serial.print(rollTarget);
Serial.print(",");
Serial.print(rollActual);
Serial.print(",");
Serial.print(pitchTarget);
Serial.print(",");
Serial.print(pitchActual);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system);
 
delay(BNO055_SAMPLERATE_DELAY_MS);
}