I am using MPU6050 to calculate pitch and roll. My code is working perfect but only if it is a drone. But i want to place my pcb vertically.
Here is how it is now.
But i want to use it like this.
I am sharing my code. I'll appreciate it if someone can help me.
Here is how it is now.
But i want to use it like this.
I am sharing my code. I'll appreciate it if someone can help me.
C++:
void velocitySignals() {
Wire.beginTransmission(0x68);
Wire.write(0x1A); //LOW PASS FILTER
Wire.write(0x05);
Wire.endTransmission();
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDRESS, 6);
Gyro_Velocity_X = Wire.read() << 8 | Wire.read();
Gyro_Velocity_Y = Wire.read() << 8 | Wire.read();
Gyro_Velocity_Z = Wire.read() << 8 | Wire.read();
Rate_Roll = (float) Gyro_Velocity_X / GYRO_LSB_SENS; //degree/
Rate_Pitch = (float) Gyro_Velocity_Y / GYRO_LSB_SENS;
Rate_Yaw = (float) Gyro_Velocity_Z / GYRO_LSB_SENS;
}
void accelSignals(){
Wire.beginTransmission(0x68);
Wire.write(0x1A); //LOW PASS FILTER
Wire.write(0x05);
Wire.endTransmission();
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU_ADDRESS, 6);
Gyro_Accel_X_Raw = Wire.read() << 8 | Wire.read();
Gyro_Accel_Y_Raw = Wire.read() << 8 | Wire.read();
Gyro_Accel_Z_Raw = Wire.read() << 8 | Wire.read();
//convert data to G unite 1g = 9.8 m/s^2
Gyro_Accel_X = (float) Gyro_Accel_X_Raw / LSB_SENS - 0.02;
Gyro_Accel_Y = (float) Gyro_Accel_Y_Raw / LSB_SENS -0.02;
Gyro_Accel_Z = (float) Gyro_Accel_Z_Raw / LSB_SENS + 0.04;
Angle_Roll=
atan(Gyro_Accel_Y/sqrt(pow(Gyro_Accel_X,2)+pow(Gyro_Accel_Z,2)))*1/(3.142/180);
Angle_Pitch=-
atan(Gyro_Accel_X/sqrt(pow(Gyro_Accel_Y,2)+pow(Gyro_Accel_Z,2)))*1/(3.142/180);
}
void gyroDataFusion() {
kalman_1d(Kalman_Angle_Roll,Kalman_Uncertainty_Angle_Roll,Rate_Roll,Angle_Roll);
Kalman_Angle_Roll=Kalman1D_Output[0];
Kalman_Uncertainty_Angle_Roll=Kalman1D_Output[1];
kalman_1d(Kalman_Angle_Pitch, Kalman_Uncertainty_Angle_Pitch, Rate_Pitch,
Angle_Pitch);
Kalman_Angle_Pitch=Kalman1D_Output[0];
Kalman_Uncertainty_Angle_Pitch=Kalman1D_Output[1];
}