Styjun

I've built a quadcopter and tried to stabilize it with the MPU6050(accelerometer and gyroskop) and an Arduino Uno as flightcontroller.

I'm trying to implement a Cascade PID Controller but without success. I've allready implemented a Rate PID-Loop which worked as expected. So you could fly the quadcopter but not in Angle-Mode like in a Flightcontoller with Betaflight flashed.

I've allready tried to dampen the vibrations but this only helped in Rate PID Mode.

This is the PID-Loop and the calculations.
If you set a looptime it will return the previous output of the PID-Calculation.

float PID::pidLoop(float setpoint, float position, float dt)
 if(isLoopTime)
 if(prevLoopTime + loopTime < millis())
 prevLoopTime = millis();
 return calcPid(setpoint, position, dt);
 else 
 return output;
 
 else 
 return calcPid(setpoint, position, dt);
 



float PID::calcPid(float setpoint, float position, float dt)
 error = position - setpoint;
 errorSum += error * dt;
 dError = (error - prevError) / dt;
 prevError = error;
 output = kp * error + ki * errorSum + kd * dError;
 if(isMinMax)
 output = minMax(min, max, output);
 
 return output;

Since the outerloop should be 4 times slower then the inner loop I've set an refreshtime of 0.016 seconds, because the inner Loop is as fast as the escs wich is a refreshreate of 0.004 seconds

//outer PID LOOP
pitchAnglePid = anglePid[pitch].pidLoop(setpoint[pitch], mpu6050.getPitchAngle(), 0.016);
rollAnglePid = anglePid[roll].pidLoop(setpoint[roll],mpu6050.getRollAngle(), 0.016);

//inner PID 
pitch_pid = ratePid[pitch].pidLoop(pitchAnglePid, mpu6050.getPitchRate(), interval);
roll_pid = ratePid[roll].pidLoop(rollAnglePid, mpu6050.getRollRate(),interval);
yaw_pid = ratePid[yaw].pidLoop(setpoint[yaw], mpu6050.getYawRate(), interval);

The results of the inner loop are added/subtracted to the ESCs.

esc_1 = instructions[throttle]- pitch_pid + roll_pid - yaw_pid;
esc_2 = instructions[throttle]+ pitch_pid + roll_pid + yaw_pid;
esc_3 = instructions[throttle]+ pitch_pid - roll_pid - yaw_pid;
esc_4 = instructions[throttle]- pitch_pid - roll_pid + yaw_pid;

My PID coefficients:

//PID Controller //PITCH ROLL YAW
PID ratePid[3] = PID(2,0,0), PID(2,0,0), PID(4,0,0);
PID anglePid[2] = PID(1,0,0,0.016),PID(1,0,0,0.016);

In theory it should stabilize in degree now, but the Quadcopter just flips at the start for no reason. It is not possible to fly it.
Any help is appreciated. Thanks in advance