Making my own controller, Yaw,Pitch and Roll angles.

V

vortix

Well-Known Member
Hello everyone. im creating my own controller and quad-copter code and im using a mpu6050 Chip that Uses an accelerometer and gyroscope to give back The Yaw, Roll and pitch angles in degrees. my problem is Using these angles to set the appropriate motor speed for stable flight since each motor would have unequal thrust. The basic idea as i understand it is to read the Angles, determine an error based on your desired value, correct for the error and give it some time so that your change has an effect then read the angles again etc... So given a change of say 0.5 in Roll, 0 in Yaw and 3 in pitch, how much would each motor change? whats the rate of change? is there a formula for this?
 
vortix said:
havent watched all of it yet but hes using almost the exact same idea and modules. Thank you, ill learn alot from this.
Click to expand...
No problem I am a programmer but haven't personally ever written a PID control either but electronoobs guy there has done a pretty darn good job breaking it all down. Keep us posted here curious to hear how you get along, also the PID control is the same used in Betaflight they've just done a lot of work on filtering out bad data and motor vibrations etc. to handle the real state of things.
 
wafflejock said:
No problem I am a programmer but haven't personally ever written a PID control either but electronoobs guy there has done a pretty darn good job breaking it all down. Keep us posted here curious to hear how you get along, also the PID control is the same used in Betaflight they've just done a lot of work on filtering out bad data and motor vibrations etc. to handle the real state of things.
Click to expand...


thank you for the video. I did almost the same thing as he did but added another axis so thats X and Y axis. But it didnt work. im not sure if i need to further adjust the constants or its a problem in the code.
code is long and include bluetooth communication so ill post what i think is relevant :
C: 
void loop(){

    LastPidTime = time;
    time = millis();
    TimeError = (time - LastPidTime) / 1000;
ReadMessage(); //Read the Incomeing Message.
if (SignalEnd==true) {
    HandleMessage(Data);
    SignalEnd=false;
   }
SetMotorsSpeed();


}
void SetMotorsSpeed(){
   
  Adjust();

 ESC.writeMicroseconds(Motor_1_Speed); //ESC is a Servo.
 ESC2.writeMicroseconds(Motor_2_Speed); 
 ESC3.writeMicroseconds(Motor_3_Speed); 
  ESC4.writeMicroseconds(Motor_4_Speed);

  
}
void Adjust(){
CurrentReading_Yaw=ypr[0] * 180/M_PI;;// in degrees
CurrentReading_Roll=ypr[2] * 180/M_PI;
CurrentReading_Pitch=ypr[1]* 180/M_PI ;

Desired_Roll=Initial_Roll;
Desired_Pitch=Initial_Pitch;

Error_Pitch=CurrentReading_Pitch-Desired_Pitch;
Error_Roll=CurrentReading_Roll-Desired_Roll;

Integral_Pitch+=(Integral_gain*(Error_Pitch*TimeError));
Integral_Roll+=Integral_gain*(Error_Roll*TimeError);

Derivative_Roll=Derivative_gain*((Error_Roll-Prev_Error_Roll)/TimeError);
Derivative_Pitch=Derivative_gain*((Error_Pitch-Prev_Error_Pitch)/TimeError);

Input_Pitch=(Porpotional_gain*Error_Pitch)+Integral_Pitch+Derivative_Pitch;
Input_Roll=(Porpotional_gain*Error_Roll)+Integral_Roll+Derivative_Roll;



Motor_1_Speed=((MotorPower*10)+1000)+Input_Pitch;
Motor_3_Speed=((MotorPower*10)+1000)-Input_Pitch;

Motor_2_Speed=((MotorPower*10)+1000)+Input_Roll;
Motor_4_Speed=((MotorPower*10)+1000) -Input_Roll;

   Prev_Error_Pitch=Error_Pitch;
   Prev_Error_Roll=Error_Roll;
 
   LastPidTime=PidTime;

  }

MotorPower*10 is the Scale for the throttle. The quad receives Pot value from the controller which then is set to the MotorPower variable.
 
You'll definitely need to adjust/change the different gain values to get things to stop wobbling too much. Even with betaflight 4.0.3 that I flashed recently on my quad there is some wobble that needs to be tuned out by adjusting the PID values depending on the characteristics of your build (power to weight, the way weight is distributed etc. etc.)

The other thing that's missing here aside from tweaking gain/constants for multiplying the PID values is filtering of the gyro/accelerometer data. Before running the PID control loop the raw data itself is filtered in betaflight code to leave it with a more accurate picture of what's happening in terms of the overall quad motion rather than the moment to moment vibrations from motors and resonance of the frame etc. etc. All this is way above my pay grade though :D I can talk about it conceptually but in terms of implementing that "dynamic notch filtering" they are doing now it's basically magic to me. Would say maybe consider reaching out to either betaflight maintainers or maintainers of one of the other open source flight control firmware to see if someone with more hands on experience building this up can give you better guidance.
 
wafflejock said:
You'll definitely need to adjust/change the different gain values to get things to stop wobbling too much. Even with betaflight 4.0.3 that I flashed recently on my quad there is some wobble that needs to be tuned out by adjusting the PID values depending on the characteristics of your build (power to weight, the way weight is distributed etc. etc.)

The other thing that's missing here aside from tweaking gain/constants for multiplying the PID values is filtering of the gyro/accelerometer data. Before running the PID control loop the raw data itself is filtered in betaflight code to leave it with a more accurate picture of what's happening in terms of the overall quad motion rather than the moment to moment vibrations from motors and resonance of the frame etc. etc. All this is way above my pay grade though :D I can talk about it conceptually but in terms of implementing that "dynamic notch filtering" they are doing now it's basically magic to me. Would say maybe consider reaching out to either betaflight maintainers or maintainers of one of the other open source flight control firmware to see if someone with more hands on experience building this up can give you better guidance.
Click to expand...

ah i see. Im using a MPU6050 if im not mistaken. The creators of the chip provided the code to access the gyroscope/ Acc but not directly. It is suppose to spit out an accurate Angle readings of Yaw,Pitch and Roll. Not sure how to check if its accurate though.
i looked into filtering mathematical equations But it looks way too messy so i understand what you mean.

i actually found this blog: https://www.wilselby.com/research/arducopter/modeling/
which compiles almost all relevant mathematical equations but it got really complex and messy so it might take some time to accurately use this Info. thought that you might want to check it it out as well :D .
 
Last edited:
Back
Top