Dominik Diedrich
/
drone_quado_x
code to fly a quadrocopter
PID/PID_yaw.cpp@0:b0f9c5ac0305, 2020-05-05 (annotated)
- Committer:
- DD1993
- Date:
- Tue May 05 21:11:38 2020 +0000
- Revision:
- 0:b0f9c5ac0305
initial
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DD1993 | 0:b0f9c5ac0305 | 1 | #include "PID_yaw.h" |
DD1993 | 0:b0f9c5ac0305 | 2 | |
DD1993 | 0:b0f9c5ac0305 | 3 | RawSerial pc_pid_yaw(USBTX, USBRX); |
DD1993 | 0:b0f9c5ac0305 | 4 | |
DD1993 | 0:b0f9c5ac0305 | 5 | PID_YAW_Class::PID_YAW_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){ |
DD1993 | 0:b0f9c5ac0305 | 6 | |
DD1993 | 0:b0f9c5ac0305 | 7 | Angle_Kp=AngleKp; |
DD1993 | 0:b0f9c5ac0305 | 8 | Angle_Ki=AngleKi; |
DD1993 | 0:b0f9c5ac0305 | 9 | Rate_Kp=RateKp; |
DD1993 | 0:b0f9c5ac0305 | 10 | Rate_Kd=RateKd; |
DD1993 | 0:b0f9c5ac0305 | 11 | } |
DD1993 | 0:b0f9c5ac0305 | 12 | |
DD1993 | 0:b0f9c5ac0305 | 13 | float PID_YAW_Class::update_yaw(float Setpoint, float CurrentPosition, float Rate, float dt){ |
DD1993 | 0:b0f9c5ac0305 | 14 | |
DD1993 | 0:b0f9c5ac0305 | 15 | AngleError = 0; |
DD1993 | 0:b0f9c5ac0305 | 16 | AngleError = Setpoint - CurrentPosition; |
DD1993 | 0:b0f9c5ac0305 | 17 | //pc_pid.printf("Setpoint: %f| CurrentPosition: %f| angle: %f\n", Setpoint, CurrentPosition, AngleError); |
DD1993 | 0:b0f9c5ac0305 | 18 | |
DD1993 | 0:b0f9c5ac0305 | 19 | Angle_I +=AngleError*dt; |
DD1993 | 0:b0f9c5ac0305 | 20 | |
DD1993 | 0:b0f9c5ac0305 | 21 | if(Angle_I>1023.0f) Angle_I=1023.0f; |
DD1993 | 0:b0f9c5ac0305 | 22 | else if(Angle_I<-1023.0f) Angle_I= -1023.0f; |
DD1993 | 0:b0f9c5ac0305 | 23 | |
DD1993 | 0:b0f9c5ac0305 | 24 | RateError = (AngleError*Angle_Kp)- Rate; |
DD1993 | 0:b0f9c5ac0305 | 25 | |
DD1993 | 0:b0f9c5ac0305 | 26 | Rate_D = (RateError-PreviousError)/dt; |
DD1993 | 0:b0f9c5ac0305 | 27 | |
DD1993 | 0:b0f9c5ac0305 | 28 | Output = (float)((RateError*Rate_Kp) + (Angle_I*Angle_Ki) + (Rate_D*Rate_Kd)); |
DD1993 | 0:b0f9c5ac0305 | 29 | |
DD1993 | 0:b0f9c5ac0305 | 30 | PreviousError = RateError; |
DD1993 | 0:b0f9c5ac0305 | 31 | |
DD1993 | 0:b0f9c5ac0305 | 32 | return Output; |
DD1993 | 0:b0f9c5ac0305 | 33 | } |