File content as of revision 0:b0f9c5ac0305:

#include "PID_roll.h"

RawSerial pc_pid_roll(USBTX, USBRX);
 
PID_ROLL_Class::PID_ROLL_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){
 
        Angle_Kp=AngleKp;
        Angle_Ki=AngleKi;
        Rate_Kp=RateKp;
        Rate_Kd=RateKd;
}
 
float PID_ROLL_Class::update_roll(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>100.0f) Angle_I=100.0f;
      else if(Angle_I<-100.0f) Angle_I= -100.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;
}