So after a while I finally got my MPU6050 working and I've been getting accelerometer and gyroscope values, but I've faced more problems. I've been working on software that uses the Madgwick filter library and a PID controller to stabilize error. Originally I was using the Madgwick filter in the Drehm Flight VTOL code however that wasn't working so I re-wrote my code using the Madgwick library. Finally I'm getting IMU, pitch, yaw, roll and PID values and it's writing to the servo but it isn't working correctly. The orientation data is significantly inaccurate, stops working suddenly and will stop writing to the servo, as well as stopping even measuring these values, there is also significant lag. I don't know how to fix the many problems I'm encountering (partly because I don't know what's causing most of them) and would love some help. I am only 13 so I am not particularly experienced and probably overlooked/ignored something somewhere.

```
#include <MadgwickAHRS.h>
#include <basicMPU6050.h>
#include <Wire.h>
#include <Servo.h>
// Create instance
basicMPU6050<> imu;
Madgwick filter;
//sensor rate fixed at 1 kHz.
float sensorRate = 1000.0;
float im;
float ACCEL_SCALE_FACTOR = 16384.0;
float GYRO_SCALE_FACTOR = 131.0;
float B_madgwick = 0.4; //Madgwick filter parameter
float B_accel = 0.14; //Accelerometer filter parameter
float B_gyro = 0.1; //Gyroscope filter parameter
float Kp_roll_rate = 1; //Roll P-gain
float Ki_roll_rate = 1; //Roll I-gain
float Kd_roll_rate = 1; //Roll D-gain
float Kp_pitch = 1; //Pitch P-gain
float Ki_pitch = 1; //Pitch I-gain
float Kd_pitch = 1; //Pitch D-gain
float Kp_yaw = 1; //Yaw P-gain
float Ki_yaw = 1; //Yaw i-gain
float Kd_yaw = 1; //Yaw D-gain
Servo servoX; //X axis servomotr
Servo servoY; //Y axis servomotor
Servo ESC; //the esc for the engine
float dt;
unsigned long current_time, prev_time;
unsigned long print_counter, serial_counter;
//IMU
float AccX, AccY, AccZ;
float AccX_prev, AccY_prev, AccZ_prev;
float GyroX, GyroY, GyroZ;
float GyroX_prev, GyroY_prev, GyroZ_prev;
float roll_IMU, pitch_IMU, yaw_IMU;
float roll_IMU_prev;
float pitch_IMU_prev, yaw_IMU_prev;
float AccErrorX, AccErrorY, AccErrorZ, GyroErrorX, GyroErrorY, GyroErrorZ;
float pitch, yaw, roll;
float AcX, AcY, AcZ;
float GyX, GyY, GyZ;
float thro, roll_des, pitch_des, yaw_des;
float error_roll, error_roll_prev, integral_roll, integral_roll_prev, integral_roll_il, integral_roll_ol, integral_roll_prev_il, integral_roll_prev_ol, derivative_roll, derivative_roll_prev, PID_roll = 0;
float error_pitch, error_pitch_prev, integral_pitch, integral_pitch_prev, integral_pitch_il, integral_pitch_ol, integral_pitch_pev_il, integral_pitch_prev_ol, derivative_pitch, derivative_pitch_prev, PID_pitch = 0;
float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, integral_yaw_il, integral_yaw_ol, integral_yaw_prev_il, integral_yaw_prev_ol, derivative_yaw, derivative_yaw_prev, PID_yaw = 0;
float prop_pitch, prop_yaw, prop_roll;
void setup() {
// Set registers - Always required
imu.setup();
// Initial calibration of gyro
imu.setBias();
// Start console
Serial.begin(38400);
servoX.attach(12);
servoY.attach(11);
delay(10);
servoX.write(0);
servoY.write(0);
delay(100);
}
void loop() {
// Update gyro calibration
calibrateAttitude();
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.print( " " );
// Temp
Serial.print( imu.temp() );
Serial.println();
*/
}
void getIMUdata() {
//retrieves IMU data
imu.updateBias();
float accelX = imu.ax();
float accelY = imu.ay();
float accelZ = imu.az();
//Serial.println(accelX);
//corrects outputs by subracting from error value
//acceX = accelX - AccErrorX;
//AccY = AccY - AccErrorY;
//AccZ = AccZ - AccErrorZ;
//Low-pass filter applied to accelerometer data
/*accelX = (1.0 - B_accel) * AccX_prev + B_accel * accelX;
accelY = (1.0 - B_accel) * AccY_prev + B_accel * accelY;
accelZ = (1.0 - B_accel) * AccZ_prev + B_accel * accelZ;
AccX_prev = accelX;
AccY_prev = accelY;
AccZ_prev = accelZ;
*/
//Gyro values
float GyroX = imu.gx();
float GyroY = imu.gy();
float GyroZ = imu.gz();
//Serial.print(imu.gx());
//Serial.print(GyroX);
/*GyroX = GyroX / GYRO_SCALE_FACTOR; // converts to degrees per second
GyroY = GyroY / GYRO_SCALE_FACTOR;
GyroZ = GyroZ / GYRO_SCALE_FACTOR;
*/
//Gyro error correct values
//GyroX = GyroX - GyroErrorX;
//GyroX = GyroX - GyroErrorX;
// GyroX = GyroX - GyroErrorX;
//Low-Pass filter for Gyro data
/*GyroX = (1.0 - B_gyro) * GyroX_prev + B_gyro * GyroX;
GyroY = (1.0 - B_gyro) * GyroY_prev + B_gyro * GyroY;
GyroZ = (1.0 - B_gyro) * GyroZ_prev + B_gyro * GyroZ;
GyroX_prev = GyroX;
GyroY_prev = GyroY;
GyroZ_prev = GyroZ; */
//normalizes acceleration
float ax = convertRawAcceleration(accelX);
float ay = convertRawAcceleration(accelY);
float az = convertRawAcceleration(accelZ);
float gx = convertRawGyro(GyroX);
float gy = convertRawGyro(GyroY);
float gz = convertRawGyro(GyroZ);
filter.updateIMU(ax, ay, az, gx, gy, gz);
//prints values.
roll = filter.getRoll();
pitch = filter.getPitch();
yaw = filter.getYaw();
Serial.println(pitch);
//Serial.println(pitch);
PID();
}
void calibrateAttitude() {
prev_time = current_time;
current_time = micros();
dt = (current_time - prev_time) / 1000000.0; //change in microseconds
getIMUdata();
//loopRate(2000);
//motion();
}
float convertRawAcceleration(int aRaw) {
// since we are using 2G range
// -2g maps to a raw value of -32768
// +2g maps to a raw value of 32767
float a = (aRaw * 2.0) / 32768.0;
return a;
}
float convertRawGyro(int gRaw) {
// since we are using 250 degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float g = (gRaw * 250.0) / 32768.0;
return g;
}
void getDesiredStates() {
thro = 0;
roll_des = 0;
pitch_des = 0;
yaw_des = 0;
}
void PID() { //My PID logic controller.
//Pitch PID calculation
error_pitch_prev = error_pitch;
error_pitch = pitch_des - pitch;
prop_pitch = Kp_pitch * error_pitch; //proportional term fine.
float totalerror_pitch;
totalerror_pitch += error_pitch;
integral_pitch = Ki_pitch * ((totalerror_pitch) * dt); //Integral term needs revision
derivative_pitch = Kd_pitch * ((error_pitch - error_pitch_prev) / dt); //derivative term is Kd multiplied by change in error over change in time?
PID_pitch = prop_pitch + integral_pitch + derivative_pitch;
//Roll PID calculation
error_roll_prev = error_roll;
error_roll = roll_des - roll;
float totalerror_roll;
totalerror_roll += error_roll;
prop_roll = Kp_roll_rate * error_roll; //proportional term fine.
integral_roll = Ki_roll_rate * ((totalerror_roll) * dt); //Integral term needs revision
derivative_roll = Kd_roll_rate * ((error_roll - error_roll_prev) / dt); //derivative term is Kd multiplied by change in error over change in time?
PID_roll = prop_roll + integral_roll + derivative_roll;
//Yaw PID calculation
error_yaw_prev = error_yaw;
error_yaw = yaw_des - yaw;
float totalerror_yaw;
totalerror_yaw += error_yaw;
prop_yaw = Kp_yaw * error_yaw; //proportional term fine.
integral_yaw = Ki_yaw * ((totalerror_yaw) * dt); //Integral term needs revision
derivative_yaw = Kd_yaw * ((error_yaw - error_yaw_prev) / dt); //derivative term is Kd multiplied by change in error over change in time?
PID_yaw = prop_yaw + integral_yaw + derivative_yaw;
//servoX.write(PID_pitch);
//Serial.println(PID_pitch);
}
float invSqrt(float x) {
//Fast inverse sqrt for madgwick filter
/*
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
*/
//alternate form:
unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
float tmp = *(float*)&i;
float y = tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
return y;
}
```