I am writing my own flight controller code for my quadcopter on an Arduino Zero. I attempted my maiden flight yesterday, it went about 2 feet up in the air and began oscillating uncontrollably/rapidly and crashed a few seconds later.
I am using a transmitter, an MPU6050 (gyroscope only), converting those values into degrees per second and feeding all of that information into a PID loop. I have held the quadcopter in my hands, turned on the throttle to abut 40% and move it around to see how it would respond and it fought my movement. Obviously, it wasn't perfectly smooth because it wasn't tuned.
I have a PID loop for each axis (Yaw, Pitch, Roll) and different PID gain values for Pitch/Roll and Yaw. For my maiden flight, I read online that a PID controller is tuned with P first, then I and D last. I read to set my P Gain values for my Pitch/Roll low, slowly increase it until it begins to oscillate and cut that value in half. I never got to that step because the quadcopter wasn't stable at all and I couldn't control it.
Extra Information: I am using a 11.1V 2.2A 3S Lipo along with 4 30A ESC's from HobbyKing. I did notice that my top left ESC was getting hot after some testing and none of my other ESC's were hot. I'm not sure if that's a major issue, I used my multimeter to check and all 4 ESC's were receiving the required voltage.
This is my PID loop:
void PID()
{
static float pError_yaw, iError_yaw, dError_yaw, previouspError_yaw;
static float pError_pitch, iError_pitch, dError_pitch, previouspError_pitch;
static float pError_roll, iError_roll, dError_roll, previouspError_roll;
static float pid_i_yaw, pid_i_pitch, pid_i_roll; //store integral calculations to monitor windup
static const int pid_max_yaw = 270, pid_max_pitch = 180, pid_max_roll = 180; //maximum output = degrees/s maximum range
static int previous_pid_output_yaw, previous_pid_output_pitch, previous_pid_output_roll;
if (reset_pid)
{
pError_yaw = 0; pError_pitch = 0; pError_roll = 0;
iError_yaw = 0; iError_pitch = 0; iError_roll = 0;
dError_yaw = 0; dError_pitch = 0; dError_roll = 0;
pid_i_yaw = 0; pid_i_pitch = 0; pid_i_roll = 0;
pid_output_yaw = 0;
pid_output_pitch = 0;
pid_output_roll = 0;
reset_pid = false;
}
//YAW
pError_yaw = pid_input_yaw - pid_setpoint_yaw; //Looks at the input, compare it to where you want it to be
iError_yaw += pError_yaw; // Looks at the pError qnd compares how long it has been that way
pid_i_yaw = pid_i_gain_yaw * iError_yaw; //Integral Calculations
if (pid_i_yaw > pid_max_yaw) pid_i_yaw = pid_max_yaw; //prevent integral windup by limiting the integral to the max yaw
else if (pid_i_yaw < pid_max_yaw * -1) pid_i_yaw = pid_max_yaw * -1;
dError_yaw = (pError_yaw - previouspError_yaw);
pid_output_yaw = (pid_p_gain_yaw * pError_yaw) + pid_i_yaw + (pid_d_gain_yaw * dError_yaw); //Output calculation
if (pid_output_yaw > pid_max_yaw) pid_output_yaw = pid_max_yaw;
else if (pid_output_yaw < pid_max_yaw * -1) pid_output_yaw = pid_max_yaw * -1;
//PITCH
pError_pitch = pid_input_pitch - pid_setpoint_pitch; //Looks at the input, compare it to where you want it to be
iError_pitch += pError_pitch; // Looks at the pError qnd compares how long it has been that way
pid_i_pitch = pid_i_gain_pitch * iError_pitch; //integral calculations
if (pid_i_pitch > pid_max_pitch) pid_i_pitch = pid_max_pitch; //prevent integral windup by limiting the integral to the max pitch
else if (pid_i_pitch < pid_max_pitch * -1) pid_i_pitch = pid_max_pitch * -1;
dError_pitch = (pError_pitch - previouspError_pitch);
pid_output_pitch = (pid_p_gain_pitch * pError_pitch) + pid_i_pitch + (pid_d_gain_pitch * dError_pitch) ; //Output calculation
if (pid_output_pitch > pid_max_pitch) pid_output_pitch = pid_max_pitch;
else if (pid_output_pitch < pid_max_pitch * -1) pid_output_pitch = pid_max_pitch * -1;
//ROLL
pError_roll = pid_input_roll - pid_setpoint_roll; //Looks at the input, compare it to where you want it to be
iError_roll += pError_roll; // Looks at the pError qnd compares how long it has been that way
pid_i_roll = pid_i_gain_roll * iError_roll;
if (pid_i_roll > pid_max_roll) pid_i_roll = pid_max_roll; //prevent integral windup by limiting the integral to the maximum roll
else if (pid_i_roll < pid_max_roll * -1) pid_i_roll = pid_max_roll * -1;
dError_roll = (pError_roll - previouspError_roll);
pid_output_roll = (pid_p_gain_roll * pError_roll) + pid_i_roll + (pid_d_gain_roll * dError_roll); //Output calculation
if (pid_output_roll > pid_max_roll) pid_output_roll = pid_max_roll;
else if (pid_output_roll < pid_max_roll * -1) pid_output_roll = pid_max_roll * -1;
//Remember previous values
previouspError_yaw = pError_yaw;
previouspError_pitch = pError_pitch;
previouspError_roll = pError_roll;
previous_pid_output_yaw = pid_output_yaw;
previous_pid_output_pitch = pid_output_pitch;
previous_pid_output_roll = pid_output_roll;
}
This is my motor mixing loop:
void motor_output() //output the correct values to the ESC's
{
static const int throttle_max = 1900 * .9, throttle_min = 1200; //throttle max = 90% of 1900, throttle mix
/*Motor Orientation
1 2
\ /
/ \
4 3
*/
if (motors_armed)
{
if (throttle < throttle_min) throttle = throttle_min; //if the throttle is less than the minimum, set it to the minimum
//if any motor output is greater the preset max throttle, reduce the throttle by difference
if (motor1output > throttle_max) throttle -= (motor1output - throttle);
if (motor2output > throttle_max) throttle -= (motor2output - throttle);
if (motor3output > throttle_max) throttle -= (motor3output - throttle);
if (motor4output > throttle_max) throttle -= (motor4output - throttle);
//calculate the values to be output to the motors(ESC's)
motor1output = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //front left
motor2output = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //front right
motor3output = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //bottom right
motor4output = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //bottom left
//limit the outputs between 1200 - 1900 us (1200 is the minimum to keep the motors running while armed)
if (motor1output < 1200) motor1output = 1200;
if (motor2output < 1200) motor2output = 1200;
if (motor3output < 1200) motor3output = 1200;
if (motor4output < 1200) motor4output = 1200;
if (motor1output > 1900) motor1output = 1900;
if (motor2output > 1900) motor2output = 1900;
if (motor3output > 1900) motor3output = 1900;
if (motor4output > 1900) motor4output = 1900;
if (throttle >= throttle_min && throttle <= throttle_max) //if the throttle if within the range, send values to the ESC's
{
esc1.writeMicroseconds(motor1output); //Motor 1 (top left, CW)
esc2.writeMicroseconds(motor2output); //Motor 2 (top right, CCW)
esc3.writeMicroseconds(motor3output); //Motor 3 (bottom right, CW)
esc4.writeMicroseconds(motor4output); //Motor 4 (bottom left, CCW)
}
}
else //if the motors aren't armed, keep the motor outputs at 1000us (off)
{
esc1.writeMicroseconds(1000); //Motor 1 (top left, CW)
esc2.writeMicroseconds(1000); //Motor 2 (top right, CCW)
esc3.writeMicroseconds(1000); //Motor 3 (bottom right, CW)
esc4.writeMicroseconds(1000); //Motor 4 (bottom left, CCW)
}
}
I am not sure how to tune my quad if it's uncontrollably in the air. Maybe my center of gravity is off? Maybe my PID loop has some type of error? Maybe my motor mixing is wrong? I'm not sure what to do. Any advice to proceed would be greatly appreciated.
(I attached my source code if anyone has other concerns about my setup)
PID_TUNING_ZERO_V5.ino (25.3 KB)
