PID source

while(1)
{

현재Error = 목표각도 - 현재각도 // 롤이면 롤각도 피치면 피치 각도

dt = 0.01 // 초당 100번 처리한다고 가정한다면

P출력 = P게인 * 현재Error ; //p gain*오차 = P제어기 출력값
누적합 += 현재Error * dt; // 누적오차

// 추가적으로 누적에 Integnal windup 방지를 위해 상 하한을 해줘야하고

누적출력 = 누적 게인 * 누적합; // 누적값에 I gain을 곱해주고

미분출력 = (현제Error - 과거Error) /dt; // D 제어기는 현재 편차와 이전를 가지고 구하고

모터 제어출력 = P출력 + 누적출력 + 미분출력;

드로틀 출력 = 현재의 트로틀 조작위치;

과거Error = 현재Error // 다음 루틴에 현재에러를 과거 에러로 써먹기 위해 저장하고

모터 1 출력 = 드로틀 출력 + 모터제어출력 ;
모터 2 출력 = 드로틀 출력 - 모터제어출력 ;

}

[출처] PID 질문에 대한 답변 (quadcopter 자작카페) | 작성자 쌍둥아빠 이기성

dt를 정확하게 하기 위해선 while 반복문 보다는 타이머 인터럽트를 사용하면 될 것같다..