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;
}