Dominik Diedrich
/
drone_quado_x
code to fly a quadrocopter
PID/PID_yaw.cpp
- Committer:
- DD1993
- Date:
- 2020-05-05
- Revision:
- 0:b0f9c5ac0305
File content as of revision 0:b0f9c5ac0305:
#include "PID_yaw.h" RawSerial pc_pid_yaw(USBTX, USBRX); PID_YAW_Class::PID_YAW_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){ Angle_Kp=AngleKp; Angle_Ki=AngleKi; Rate_Kp=RateKp; Rate_Kd=RateKd; } float PID_YAW_Class::update_yaw(float Setpoint, float CurrentPosition, float Rate, float dt){ AngleError = 0; AngleError = Setpoint - CurrentPosition; //pc_pid.printf("Setpoint: %f| CurrentPosition: %f| angle: %f\n", Setpoint, CurrentPosition, AngleError); Angle_I +=AngleError*dt; 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; }