[헥사콥터] F550 헥사콥터 제작 - PID (4)

main.c

0.03MB

stm32f4xx_it.c

0.01MB

PID.c

0.00MB

PID.h

0.00MB

init.h

0.00MB

드론을 만들 때 제대로 된 PID 제어를 하지 못한다면 제대로 비행을 못한다.

 

제어 코드를 잘 짜는 것도 중요하지만 파라미터 튜닝을 제대로 하는 것도 중요하다. 파라미터는 날려보면서 진행해야 해서 드론을 조립하고 날릴 수 있을 때 튜닝할 것이다.

 

사용하는 PID코드는 쿼드콥터와 비슷한 코드를 사용하고 약간의 정리와 파라미터 하나를 추가한 점이 다르다.

 

PID.c 코드와 main.c의 변경되고 추가된 코드들 외의 다른 코드에 대한 직접적인 분석은 하지 않을 생각이며 코드 생성을 따로 안 해도 된다.

 

 

PID 제어 - (0)

PID

일반적인 PID제어 block diagram이다.

 

e(t)는 에러값을 나타내고, Kp, Kr, Kd는 PID 제어 파라미터 값이다.

 

기본적인 PID제어 코드는 PID.c내부의 NORMAL_PID() 함수를 확인하면 된다.

 

PID 제어에 관한 내용은 더 좋은 설명을 하는 글이 많기 때문에 추가적인 설명은 하지 않을 것이다.

 

Yaw제어와 GPS, 고도제어 등등은 일반적인 PID제어를 사용해 제어할 수 있지만 Pitch, Roll의 경우 일반적인 PID제어를 사용하여 제어하기가 쉽지 않다.

 

따라서 더 복잡한 제어 방식을 사용해야 하고 위의 block diagram방식의 PID제어가 HAL드라이버로 만든 쿼드콥터를 제어할 때 사용한 방식이다.

 

실제로 꽤 괜찮은 비행을 보여줬다.

 

하지만 이번 헥사콥터를 제어할 때는 위의 방식에 RATE_D 값을 추가하여 내부에서도 PI제어가 아닌 PID제어로 변경해볼 생각이다.

 

실제 코드는 PID.c의 DUAL_PID() 함수를 확인하면 된다.

 

 

코드 - main.c (1)

 

//========================================================================================================================= PID
      else if(condition_pid){
          //LL_GPIO_SetOutputPin(GPIOB,LL_GPIO_PIN_1);
          DUAL_PID(&pitch_para,0.0,angle.pitch-angle.pitch_offset,angle.f_gyy);
 
          DUAL_PID(&roll_para,0.0,angle.roll-angle.roll_offset,angle.f_gyx);
 
          NORMAL_PID(&yaw_para,0.0,3.0);
 
          condition_pid=0;
          //LL_GPIO_ResetOutputPin(GPIOB,LL_GPIO_PIN_1);
      }
 
//=========================================================================================================================

 

while 문 내부에 존재하는 코드이며 condition_pid 변수는 stm32f4xx_it.c 파일에 TIM2 UPDATE interrupt가 발생하면 1로 SET 한다.

 

TIM2는 PWM 신호를 발생하는 타이머이며, 약 2ms마다 PID계산 결과를 모터 값을 업데이트한다고 보면 된다.

 

값에 들어가는 0.0은 target_angle이고 3.0의 값은 Yaw의 현재 각도인데, 위의 코드는 임의로 넣어준 값이며 나중에 제대로 된 값이 들어갈 것이다.

 

 

코드 - PID.c (2)

 

void NORMAL_PID(RATE_PID* PID,float set_angle, float input_angle){
    float temp_error;
    float temp_p;
    float temp_d;
 
    temp_error = set_angle - input_angle;
 
    temp_p = (PID->para_p) * temp_error;
    temp_d = (PID->para_d) * (temp_error=PID->pre_rate_d_error);
    PID->pre_rate_d_error = temp_error;
 
    if( (PID->rate_i_mem) < 300 && (PID->rate_i_mem>-300) ){
        PID->rate_i_mem += (PID->para_i) * temp_error * pid_dt;
    }
    else{
        if(PID->rate_i_mem >299.5 && temp_error<0.0){
            PID->rate_i_mem += (PID->para_i) * temp_error * pid_dt;
        }
        else if(PID->rate_i_mem <-299.5 && temp_error>0.0){
            PID->rate_i_mem += (PID->para_i) * temp_error * pid_dt;
        }
    }
 
    PID->pid_result = temp_p + temp_d +PID->rate_i_mem;
}
 
 
void DUAL_PID(STABILIZE_PID* PID,float set_angle, float input_angle,float input_rate){
    float temp_error;
    float temp_p;
    float temp_d;
 
 
    temp_error = set_angle - input_angle;
 
    temp_p = (PID->stabilize_p) * temp_error;
 
    if( (PID->stabilize_i_mem < 270) && (PID->stabilize_i_mem > -270) ){
        PID->stabilize_i_mem += (PID->stabilize_i) * temp_error *pid_dt;
    }
    else{
        if(PID->stabilize_i_mem > 269.5 && temp_error<0.0){
            PID->stabilize_i_mem += (PID->stabilize_i) *temp_error *pid_dt;
        }
        else if(PID->stabilize_i_mem < -269.5 && temp_error>0.0){
            PID->stabilize_i_mem += (PID->stabilize_i) * temp_error *pid_dt;
        }
    }
 
 
    temp_error = temp_p - input_rate;
    temp_p = PID->rate_p * temp_error;
 
    temp_d = PID->rate_d * (temp_error - PID->dual_pre_rate_d_error);
    PID->dual_pre_rate_d_error = temp_error;
 
    if( (PID->dual_rate_i_mem) < 300 && (PID->dual_rate_i_mem>-300) ){
        PID->dual_rate_i_mem += PID->rate_i * temp_error * pid_dt;
    }
    else{
        if(PID->dual_rate_i_mem >299.5 && temp_error<0.0){
            PID->dual_rate_i_mem += PID->rate_i * temp_error * pid_dt;
        }
        else if(PID->dual_rate_i_mem <-299.5 && temp_error>0.0){
            PID->dual_rate_i_mem += PID->rate_i * temp_error * pid_dt;
        }
    }
 
    PID->dual_pid_result = temp_p + temp_d + (PID->dual_rate_i_mem) + (PID->stabilize_i_mem);
}
 

 

PID.c의 NORMAL_PID(), DUAL_PID() 함수의 내부이다.

 

내부 코드 중 i_mem 값을 제한하는 코드들이 있는데 i는 에러를 적분하여 계산하는데 만약 제한이 없을 경우 값이 무한정 커질 수 있다. 따라서 값에 제한을 뒀다.

 

DUAL_PID()의 매개변수 중 input_rate의 값은 각속도가 들어가며 각속도에 대한 제어가 내부 PID이다.

 

stabilize_i_mem의 값이 생각보다 중요하다. 실제로 모터의 출력이 전부 일정하지 않을 수 있는데 이때 사용한다. stabilize_i 값에 작은 값을 넣을 경우 stabilize_i_mem 값이 제대로 커지지 않아 균형이 어긋날 수 있다.

 

D제어를 추가했고 HAL드라이버로 만든 쿼드콥터에는 적용하지 않았다.

 

 

 

-

PID제어에 대한 이론적인 내용은 쓰지 않았다. 구글에 검색해도 더 설명을 잘해놓은 글이 있고 잘못된 내용을 전달할 수도 있기 때문이다. 또한 블로그의 글을 쓰는 이유도 어느 정도 이론적 지식은 있지만 실제로 코드를 작성해보고자 할 때 어려움을 느끼는 사람들을 위해서 약간이라도 도움을 얻어가면 좋겠다고 생각하기 때문이다.

 

쿼드콥터의 코드를 적용하기 쉽고 보기 좋게 변경한 것이 끝이고 실제 쿼드콥터에 적용된 코드이다. 따라서 아직 비행 테스트는 하지 않았지만 비행이 가능하다고 생각하고 있다.

 

헥사콥터 회로와 프레임을 조립하고 지자기센서 코드를 만들면 바로 PID 튜닝이 가능할 것이라고 생각한다.

 

부품을 잘못 구입한 게 있기 때문에 아직 회로를 만들지 못하고 있다. 부품이 도착하는 데로 바로 만들 것이다.

 

 

 

모든 코드는 LL드라이버를 사용한다.