Dominik Diedrich
/
drone_quado_x
code to fly a quadrocopter
Diff: PID/PID_pitch.cpp
- Revision:
- 0:b0f9c5ac0305
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID/PID_pitch.cpp Tue May 05 21:11:38 2020 +0000 @@ -0,0 +1,37 @@ +#include "PID_pitch.h" + +RawSerial pc_pid_pitch(USBTX, USBRX); + +PID_PITCH_Class::PID_PITCH_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){ + + Angle_Kp=AngleKp; + Angle_Ki=AngleKi; + Rate_Kp=RateKp; + Rate_Kd=RateKd; +} + +float PID_PITCH_Class::update_pitch(float Setpoint, float CurrentPosition, float Rate, float dt){ + + AngleError = 0; + AngleError = Setpoint - CurrentPosition; + //pc_pid_pitch.printf("Setpoint: %f| CurrentPosition: %f| angle: %f\n", Setpoint, CurrentPosition, AngleError); + + Angle_I +=AngleError*dt; + //pc_pid_pitch.printf("Angle_I: %f\n", Angle_I); + + if(Angle_I>1023.0f) Angle_I=1023.0f; + else if(Angle_I<-1023.0f) Angle_I= -1023.0f; + + + + RateError = (AngleError*Angle_Kp)- Rate; + + Rate_D = (RateError-PreviousError)/dt; + + Output = (float)((RateError*Rate_Kp) + (Angle_I*Angle_Ki) + (Rate_D*Rate_Kd)); + + PreviousError = RateError; + + + return Output; +} \ No newline at end of file