Hi, I'm trying to build a drone but I have some problems. So when the motors start spinning at a decent speed the accelerometer values go to -32769. It might just have to do with my hardware but I have added anti-vibration screws and balanced the props so I don't see what else there is to do over there.
On the drone I have an Arduino uno, 2 mpu6050's and a hc-12 wich is read out by a Arduino nano wich then send the data to the Uno over an I2C bus.
What I have noticed is that when the hc-12 is receiving somethings to the values from the mpu6050's glitches sometimes so I think they are interfering with each other.
So there probably is something wrong with my code
Here is the code related to this:
const int MPU_addr1=0x68;
const int MPU_addr2=0x69;
const int NANO_addr=0x29;
void ReadMPU6050(){
if ((double)(micros() - TLoop)/1000000<double(0.05)) {
Read=1;
Read1MPU6050();
Read2MPU6050();
*Some code to get the average of both values*
}
else{
Read=0;
}
}
void Read1MPU6050() {
Wire.beginTransmission(MPU_addr1);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr1,14,true); // request a total of 14 registers
AcX1=-(Wire.read()<<8|Wire.read())- 200; // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY1=-(Wire.read()<<8|Wire.read())- 598; // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ1=(Wire.read()<<8|Wire.read()); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX1=-(Wire.read()<<8|Wire.read())+ 4550; // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY1=-(Wire.read()<<8|Wire.read())-380; // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ1=-(Wire.read()<<8|Wire.read()); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L))
}
void Read2MPU6050(){
Wire.beginTransmission(MPU_addr2);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr2,14,true); // request a total of 14 registers
AcX2=-(Wire.read()<<8|Wire.read())+708; // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY2=-(Wire.read()<<8|Wire.read())-390; // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ2=(Wire.read()<<8|Wire.read()); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX2=-(Wire.read()<<8|Wire.read()); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY2=-(Wire.read()<<8|Wire.read())+430; // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ2=-(Wire.read()<<8|Wire.read()); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}
}
void CalculateAngles() {
dt = (double)(micros() - TLoop)/1000000;
TLoop = micros();
if (Read==1) {
roll = atan2(AcY, AcZ)*degconvert;
pitch = atan2(-AcX, AcZ)*degconvert;
gyroXrate = GyX/131.0;
gyroYrate = GyY/131.0;
gyroZrate = GyZ/131.0;
compAngleX = .99 * (compAngleX + gyroXrate * dt) + .01 * roll; // Calculate the angle using a Complimentary filter
compAngleY = .99 * (compAngleY + gyroYrate * dt) + .01 * pitch;
AngleX = compAngleX;
AngleY = compAngleY;
AngleZ = AngleZ + gyroZrate * dt;
}
}
void ReadNano(){
Wire.begin(NANO_addr);
Wire.onReceive(ReadController);
}
void ReadController() {
double dtNano = (double)(micros() - TNano)/1000;
if (dtNano<75){
while (0 < Wire.available()) {
char c = Wire.read();
Stringinput = Stringinput + c;
firstnumber = Stringinput.substring(0,1).toInt();
}
*Some code to determine what value is what*
}
}
void starti2cbus(){
Wire.begin();
Wire.beginTransmission(MPU_addr1);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Wire.begin();
Wire.beginTransmission(MPU_addr2);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
}
void setup() {
starti2cbus();
ReadNano();
}
void loop() {
ReadMPU6050();
CalculateAngles();
/*GetSpeed(); (non related things)
Stabilization();
ControlESCs();
Output();*/
TNano = micros();
delay(2);
}
I hope someone finds my mistake
Thank you for your time anyway