File content as of revision 1:dd6e70abeb8e:

#include "mbed.h"
#include "Global_Variables.h"
#include "Pin_Assign.h"
#include "PWMOut.h"
#include "PPMIn.h"
#include "PPMOut.h"

void PWMPID(void)
{
    if(fLock==1&&fRFread==1)
    {
        fRFread=0;
        YawT= YawRel+Azi;   //Clockwise(+), CC(-)                 //RF CW(+),CC(-)
        //PitchT= ;  //down angle(+), up angle(-)                     //RF Up(+)
    }
    else if(fLock==0)
    {
        YawT = YawDegCMD;
        PitchT = PitchDegCMD;
        
        //if(YawT>60.0f) YawT=60.0f;
        //if(YawT<-60.0f) YawT=60.0f;
    }
    
    //---------Yaw PID----------------
    ErrorY = YawT - YawRel;
    ErrorYI += ErrorY*dt;
    if(ErrorYI<-9.0f||ErrorYI>9.0f)ErrorYI=0;
                    
    YawPWM = ErrorY*KpY + ErrorYI*KiY + ((ErrorY - prevErrorY)*KdY)/dt;
    prevErrorY = ErrorY;
    
                    
    if(YawPWM<-YawLim)             YawPWM = -YawLim;
    else if(YawPWM>YawLim)         YawPWM = YawLim;
    
    
    //----------Pitch PID--------------    
    ErrorP = PitchT - PitchRel;
    ErrorPI += ErrorP*dt;
    if(ErrorPI<-9.0f||ErrorPI>9.0f)ErrorPI=0;
                    
    PitchPWM = ErrorP*KpP + ErrorPI*KiP + ((ErrorP - prevErrorP)*KdP)/dt;
    prevErrorP = ErrorP;
                    
    if(PitchPWM<-PitchLim)             PitchPWM = -PitchLim;
    else if(PitchPWM>PitchLim)         PitchPWM = PitchLim;    
    
}
void PWMOut(void)
{
    PWMPID();
    
    YawCMD.pulsewidth_us((int)YawPWM+IdlePWM);
    PitchCMD.pulsewidth_us((int)PitchPWM+IdlePWM);
    
    //YawCMD.pulsewidth_us((int)YawPPM);
    //PitchCMD.pulsewidth_us((int)PitchPPM);
}