![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
code to fly a quadrocopter
PID/PID_pitch.cpp
- Committer:
- DD1993
- Date:
- 2020-05-05
- Revision:
- 0:b0f9c5ac0305
File content as of revision 0:b0f9c5ac0305:
#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; }