if (tim9_event_flag == 1) // 800Hz Timer flag On
{
tim9_event_flag = 0;// clear the flag
count1++;// loop를 거칠 때 마다 증가하는 counter
//=== 변수 초기화 ===
acc_ahrs.AXIS_X = 0; //X축 가속도 데이터 정보 저장 변수 초기화
acc_ahrs.AXIS_Y = 0; //Y축 가속도 데이터 정보 저장 변수 초기화
acc_ahrs.AXIS_Z = 0; //Z축 가속도 데이터 정보 저장 변수 초기화
gyro_ahrs.AXIS_X = 0;//X축 자이로 데이터 정보 저장 변수 초기화
gyro_ahrs.AXIS_Y = 0;//Y축 자이로 데이터 정보 저장 변수 초기화
gyro_ahrs.AXIS_Z = 0;//Z축 자이로 데이터 정보 저장 변수 초기화
//============ 가속도, 자이로 센서 정보 업데이트 =============
for(i=0;i<FIFO_Order;i++)
{
acc_ahrs.AXIS_X += acc_ahrs_FIFO[i].AXIS_X;
acc_ahrs.AXIS_Y += acc_ahrs_FIFO[i].AXIS_Y;
acc_ahrs.AXIS_Z += acc_ahrs_FIFO[i].AXIS_Z;
gyro_ahrs.AXIS_X += gyro_ahrs_FIFO[i].AXIS_X;
gyro_ahrs.AXIS_Y += gyro_ahrs_FIFO[i].AXIS_Y;
gyro_ahrs.AXIS_Z += gyro_ahrs_FIFO[i].AXIS_Z;
}
//=========================================================
//====FIFO_Order_Recip 1/5=================================
acc_ahrs.AXIS_X *= FIFO_Order_Recip;
acc_ahrs.AXIS_Y *= FIFO_Order_Recip;
acc_ahrs.AXIS_Z *= FIFO_Order_Recip;
gyro_ahrs.AXIS_X *= FIFO_Order_Recip;
gyro_ahrs.AXIS_Y *= FIFO_Order_Recip;
gyro_ahrs.AXIS_Z *= FIFO_Order_Recip;
//=========================================================
//====xxx_fil_int는 int32_t로 형변환 하여 저장 하기위한 변수==
acc_fil_int.AXIS_X = (int32_t) acc_ahrs.AXIS_X;
acc_fil_int.AXIS_Y = (int32_t) acc_ahrs.AXIS_Y;
acc_fil_int.AXIS_Z = (int32_t) acc_ahrs.AXIS_Z;
gyro_fil_int.AXIS_X = (int32_t) gyro_ahrs.AXIS_X;
gyro_fil_int.AXIS_Y = (int32_t) gyro_ahrs.AXIS_Y;
gyro_fil_int.AXIS_Z = (int32_t) gyro_ahrs.AXIS_Z;
//=========================================================
// AHRS update, quaternion & true gyro data are stored in ahrs
// ahrs_fusion_ag는 가속도계와 자이로 센서 측정치를 이용하여
// 동체의 자세를 추정하는 함수이다.
ahrs_fusion_ag(&acc_ahrs, &gyro_ahrs, &ahrs);
// Calculate euler angle drone
// QuaternionToEuler는 함수명과 동일하게
// Quaternion을 Euler로 변환하는 함수이다.
QuaternionToEuler(&ahrs.q, &euler_ahrs);
#ifdef REMOCON_BLE
//전달된 조종기 신호를 받아서 처리하는 부분
//조종기 신호는 사용자 APP에서 전잘되는 정보를 기반으로 하고
//rudder (joydata[2]), throtle(joydata[3]),
//aileron(joydata[4]) 그리고 elevator(joydata[5])가 있다.
//추가적으로 빼고 더하는 부분은 제어 입력(오일러 각도)으로 사용
//하기위해 적용된 Scale Factor 같은 것이다.
gRUD = (joydata[2]-128)*(-13);
gTHR = joydata[3]*13;
gAIL = (joydata[4]-128)*(-13);
gELE = (joydata[5]-128)*13;
/* joydata[6]: seek bar data*/
/* joydata[7]: additional button data
first bit: Takeoff (0 = Land, 1 = Takeoff)
second bit: Calibration When it changes status is active
third bit: Arming (0 = Disarmed, 1 = Armed) */
// joydata[7]은 rudder, throtle... 과다르게 버튼과 같은 0 또는 1
// 과 같은 상태만 필요로하기 때문에 1비트로 충분히 충당할 수 있기 때문에
// first bit, second bit.. 등으로 상태를 나눠서 한 바이트로 보내준 것이다.
// 가장 중요한 버튼으로 3번 째 비트인 Arming이 있다. 이를 활성화 시키면
// 모터에 제어신호가 전달되기 때문에 모터가 동작하게 된다.
// 그렇지 않을경우 모터가 동작하지 않는다. 즉 사용자 안전을 위해 꼭 필요한 기능
gJoystick_status = joydata[7];
if ((gJoystick_status&0x04)==0x04){//3번째 비트가 on:1 됬을 경우
//모터에 제어신호를 인가할 수 있도록 활성화한다.
rc_enable_motor = 1;//모터 신호 인가 활성화
fly_ready = 1;//이륙 준비 OK
BSP_LED_On(LED2);// 확인을 위한 LED 점멸
}
else {//3번째 비트가 0이면 모터에 신호 인가 X
rc_enable_motor = 0;//모터 신호 인가 X
fly_ready = 0;
}
if (connected){//사용자 스마트폰 어플과 블루투스 어플이 연동 되었는지 확인
rc_connection_flag = 1; // 연결되었다면 flg 1로 설정
/* BLE Remocon connected flag for enabling motor output */
SendMotionData();//드론의 자세 데이터 전송
SendBattEnvData();//배터리 상태 전송
SendArmingData();
}
else{//연동되지 않았을 경우
rc_connection_flag = 0;//connection flg 0으로 설정
gTHR=0;//throtle 급발진이 생기지 않도록 0으로 설정
rc_enable_motor = 0;//모터도 회전하지 않도록 신호 인가 flg 0으로 설정
fly_ready = 0;
BSP_LED_Off(LED1);
BSP_LED_Off(LED2);
}
if (joydata[7]&0x02){//foydata[7]의 두번째 비트가 on될 경우
rc_cal_flag = 1;//기체에 장착되어 있는 센서를 캘리브레이션한다
BSP_LED_On(LED1);
}
#endif
// Get target euler angle from remote control
//RC로 받은 조종기 조이스틱 정보를 euler 각도로 제어를 위해 변환하는 과정
GetTargetEulerAngle(&euler_rc, &euler_ahrs);
//조종기의 Throtle의 최소 입력이하의 정보가 들어왔을 때 0으로 초기화
//안전기능과 같다. throtle을 가장 아래로 내렸을 경우 자세제어가 발산할 수
//있기 때문에 넣기도 한다.
if(gTHR<MIN_THR)
{
euler_ahrs_offset.thx = 0;
euler_ahrs_offset.thy = 0;
}
//센서를 통해 얻어진(추정된) 동체의 자세가 radian이므로 degree로 변환
Fly_origin.X_Degree = (int16_t)(euler_ahrs.thx * 5730);
Fly_origin.Y_Degree = (int16_t)(euler_ahrs.thy * 5730);
Fly_origin.Z_Degree = (int16_t)(euler_ahrs.thz * 5730);
if(gTHR<MIN_THR)//위와 같은 이유로 안전기능
{
euler_rc.thz = 0;
euler_ahrs.thz = 0;
}
//앞서 계산된 블루투스를 통해 입력받은 사용자 요구 각도를 계산한 값을
//자세 제어를 위해 저장하는 변수
euler_rc_fil.thx = euler_rc.thx;
euler_rc_fil.thy = euler_rc.thy;
euler_rc_fil.thz = euler_rc.thz;
//자세제어 진행
FlightControlPID_OuterLoop(&euler_rc_fil, &euler_ahrs, &ahrs, &pid);
Comments
Post a Comment