//PI제어_X축
cf_err_x = cf_result_x-accel_angle_x; // 필터값.accel 오일러각, P제어에 사용 오차=목표치·현재값
cf_err_sum_x += cf_erR_x * dt; // 에러를 적분해서 저장, I제어에 사용
cf_kp_val_x = cf_err_x*cf_kp; // P value
cf_ki_val_x = cf_err_sum_x*CF_KI; // I value
cf_pid_out_x = (p*qsinx*tany*rcosx*tany) – (cf_kp_val_x + cf_ki_val_x);
// 자이로 각도값을 PID제어를 통해 얼마나 빠르게 가속도 각도값에 맞출 것인지 결정
cf_result_x += (cf_pid_out_x*dt);

쿼드 코드에서 구현된 상보필터 (X축)

void ROLL PID(void) {
//Roll_Cmd = C_Roll_Cmd*0.01;

Roll_Ang_Err = Roll_Cmd – cf_cf_result_x;
Roll_Out_Err = Roll_Ang_Err * Roll_Out_Pgain – gyro_x_t;
Roll_In_P = Roll_Out_Err * Roll_In_Pgain;
Roll_In_I = Roll_In_I + (Roll_Out_Err * Roll_In_Igain)*dt_pid;
// Limit_var(&Roll_In_I,-300,300);

Roll_In_D = (Roll_Out_Err – Pre_Roll_Out_Err)/dt_pid * Roll_In_Dgain;
Pre_Roll = (Roll_In_P + Roll_In_I + Roll_In_D);
//// 이중 PID ROLL

Quad Copter 코드에서의 2중 PID함수

case 34: GPIOC -> BSRR=0xe7ff; flag)cycle = 1;
while(flag_cycle){
while(rx_flag == 0); rx_flag=0;

parsing();
count = atoi(da[0];
C_Roll_Cmd = atoi(da[1]);
C_Pitch_Cmd = atoi(da[2]);
C_Throttle = atoi(da[3]);
C_Check_sum = atoi(da[4]);

if((C_Roll_Cmd+C_Pitch_Cmd+C_throttle)==C_CHECK_SUM){
gy_ac();
Result_PID();
//Limit_var(&PID_Roll, -50,50);
//Limit_var(&Pid_Pitch,-50,50);
X_Motor();
}

Key_backselect(34);
} ? end while flag_cycle? break;

#include “stm32f10x.h”
#include “MPU6050.h”
#include “MPU6050_Lib.h”
#include “HAL_MPU6050.h”
#include<math.h>
#include<stdio.h>
#include<string.h>
#include<stdlib.h>
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#define on 0
#define off 1
#define TRUE 1
#define FALSE 0
#define PWM_ocr1 (TIM2->CCR1)
#define PWM_ocr2 (TIM2->CCR2)
#define PWM_ocr3 (TIM4->CCR3)
#define PWM_ocr4 (TIM4->CCR4)

volatile int send=0,mflag=0;
unsigned int Key_count=0;

//실시간 자이로 가속도 값
int16_t accelgyro[6];

volatile int RxCounter=0,tokencount=0;
uint8_t NbrOfDataToTransfer=10;
char TxBuffer[50], TxCounter, RxBuffer[52]={0,},G_DATA[52]={0,};
char *da[5]={0,};
char *last = NULL;
char *token;
char ch;
char* context = NULL;
float pitch_cont=0,roll_cont=0,yaw_cont=0;
int calibrate_flag = 1;
float calibrate_cf_x = 0;
float calibrate_cf_y = 0;
unsigned int calibrate_flag_if = 100;

int C_check_sum=0;
int throttle=0;
//float test1=0,test2=0;
int test_t=800;
//I2C TEST 용
struct {
u16 tmr;
u16 connect;
}test;

// KEY TEST
struct {
u16 bf;
} key;
u16 key_flag;
u16 key_flag1=0;

int K_count = 0;
volatile int flag_num = 1;
int flag_count = 0;

int flag_cycle = 1;
int rx_flag = 0;
int count=0;

#define SW_up (GPIOD->IDR&GPIO_Pin_0)
#define SW_sel (GPIOD->IDR&GPIO_Pin_1)
#define SW_down (GPIOD->IDR&GPIO_Pin_2)

//—————————————————
void RCC_Configuration(void);
void NVIC_Configuration(void);
void GPIO_Configuration(void);
void Uart1_Initialize(void);
int fputc(int a, FILE*f);
void Sensor_Test();
void I2C_ini(void);
void TIM2_Configuration(void);
void TIM2_IRQHandler(void);
void SerialPutChar(uint8_t c);
void Serial_PutString(uint8_t *s);
void Delay_us(vu32 nCount);
void Delay_ms(vu32 nCount);
void Usart1_init(void);
void SendSerial(uint8_t buffer[], uint8_t size);
void USART1_IRQHandler(void);
void parsing(void);
void TIM2_Configuration(void);
void ocr_limit(void);

void GY_AC(void);
void Result_PID();
void X_Motor();

//—————————————————
void Delay_us(vu32 nCount){

nCount *= 8;
for(; nCount!=0; nCount–);
}
void Delay_ms(vu32 nCount){

nCount *= 6000;
for(; nCount!=0; nCount–);
}
void I2C_ini(void){

I2C_InitTypeDef I2C_InitStruct;

I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;
I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStruct.I2C_OwnAddress1 = MPU6050_DEFAULT_ADDRESS;
I2C_InitStruct.I2C_Ack = I2C_Ack_Enable;
I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStruct.I2C_ClockSpeed = MPU6050_I2C_Speed;

I2C_Init(MPU6050_I2C, &I2C_InitStruct);
I2C_Cmd(MPU6050_I2C, ENABLE);
}
void gy_ac(void){
if(MPU6050_TestConnection()==TRUE){ test.connect++; }
MPU6050_GetRawAccelGyro(accelgyro);
calibrate_sensors(accelgyro);
SendSerialAccelGryro(accelgyro);
}

void RCC_Configuration(void){

ErrorStatus HSEStartUpStatus;

RCC_DeInit(); //RCC system reset(for debug purpose)
RCC_HSEConfig(RCC_HSE_ON); //Enable HSE
HSEStartUpStatus = RCC_WaitForHSEStartUp(); //Wait till HSE is ready

if(HSEStartUpStatus == SUCCESS) {
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
FLASH_SetLatency(FLASH_Latency_2); //Flash 2 wait state
RCC_HCLKConfig(RCC_SYSCLK_Div1); //HCLK = SYSCLK
RCC_PCLK2Config(RCC_HCLK_Div1); //PCLK2 = HCLK
RCC_PCLK1Config(RCC_HCLK_Div4); //PCLK1 = HCLK/4

RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9); //PLLCLK = 8MHz * 9 = 72 MHz
RCC_PLLCmd(ENABLE);

while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET); //Wait till PLL is ready
RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK); //Select PLL as system clock source
while (RCC_GetSYSCLKSource() != 0×08); //Wait till PLL is used as system clock source
}
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE ); //클럭설정
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
//C_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB1PeriphClockCmd(MPU6050_I2C_RCC_Periph|RCC_APB1Periph_TIM3, ENABLE);

}