출처 : http://blog.naver.com/housemoon/96039559
// 제 목: RS232 통신 프로그램 // 직렬포트를 이용하여 데이타 통신하는 // 일반적인 순서 // (1) 직렬포트를 연다(open) // (2) 직렬포트의 각종 설정값을 변경한다 // (3) 읽기와 쓰기 타임아웃을 설정한다 // (4) 직렬포트로부터 데이타를 읽거(read)나 쓴다(write) // (5) 직렬포트를 닫는다(close) // * (1),(4),(5)는 파일 입출력 모델과 개념적을 동일하며, // 파일입출력 함수를 그대로 사용함 // * (2),(3)은 직렬통신에만 해당하며, 직렬통신을 위한 전용함수를 사용함 #include <windows.h> #include <stdlib.h> #include <stdio.h> #include <math.h> #include <iostream> #include <time.h> //랜덤 함수를 위해. #include <GL/glut.h> #include <GL/glu.h> #include <GL/glaux.h> //---[ pi 값 설정 ] #define PI 3.141592 //---[ 버퍼사이즈 설정 ] #define BUFSIZE 22 //================================================ // 전연변수 선언부 //------------------------------------------------ // main int counter = 1; int TenSec = 0; // 120Hz이므로 10초인 1200회를 수행하도록 하기 위한 인덱스 변수 double Voltage = 3.3; // 단위 : V double Resolution = 1024; // 분해능 = 2^10 double dt = 1 / 20; // 시간 간격. double Final_Pitch = 0; double Final_Roll = 0; // Gyro double Gyro_sens = 0.002; // 단위 : V/degrees/sec double Scalefactor = Voltage / Resolution / Gyro_sens; // 대략 1.611328 // For Accel double Accel_sens = 0.8; // 단위 : V/g // 1.5g 이기 때문에 double unit = Voltage / Resolution; // 대략 3.222656mV/bit //================================================ //---[ 데이터 통신에 사용할 변수 ] unsigned char buf[BUFSIZE + 1]; DWORD BytesRead; int retval; HANDLE hComm; //================================================ // OpenGL용 변수 //------------------------------------------------ GLfloat xrot = 0.9f, yrot = 0.8f, zrot = 1.0f; unsigned int MyTextureObject[1]; AUX_RGBImageRec *pTextureImage[1]; //텍스쳐 저장 공간을 가리키는 포인터 GLdouble theta[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; // 회전을 위한 배열, 초기화 각각(x, y, z축을 나타냄) static GLint SpinCon = 0; // 회전시에 회전 값을 받는 배열안의 값이 float 값을 넘는것을 방지 하는 변수 //================================================ // 구조체 //------------------------------------------------ //IMU struct struct IMU { int packet[10]; // main에서 센서에서 값 받아오는 배열 변수 int Gyro_offset_sum[3]; // Gyro의 offset의 10초간의 ADC 값의 합을 저장하기 위한 변수 int Accel_offset_sum[3]; // Accel의 offset의 10초간의 ADC 값의 합을 저장하기 위한 변수 double Gyro_offset_avg[3]; // Gyro의 offset의 평균값 double Accel_offset_avg[3]; // Accel의 offset의 평균값 double pitch_vel[4]; // velocity = 각속도 double roll_vel[4]; // pitch, roll, yaw의 적분을 하기 위해 최근 4개 값을 저장하기 위한 배열 double yaw_vel[4]; double G_pitch; // 자이로 센서를 이용해 구한 각속도를 적분하여 얻은 각도 (degree) double G_roll; double G_yaw; double A_pitch; // 가속도 센서를 이용해 얻은 각도 (degree) double A_roll; double A_yaw; double LK_pitch; // 칼만 필터 후 최종 보정된 각도 (degree) double LK_roll; double LK_yaw; }; // Kalman Struct struct Gyro1DKalman { // matrix x의 상태를 나타낸다. double x_angle; // x_angle : 최종 보정 각 double x_bias; // x_bias : 바이어스 // 오차 공분산 double P_00, P_01, P_10, P_11; // Q 잡음 double Q_angle, Q_gyro; // R 잡음 double R_angle; }; //---리니어 칼만필터를 위한 초기화 // Filter_roll_1 = IMU1 / Filter_roll_2 = IMU2 / Filter_roll_3 = IMU3 과 연산 //순서 : x_angle, x_bias, P_00, P_01, P_10, P_11, Q_angle, Q_gyro, R_angle struct Gyro1DKalman filter_roll_1 = { 0, 0, 1, 0, 0, 1, 0.0001, 0.0003, 0.01 }; struct Gyro1DKalman filter_roll_2 = { 0, 0, 1, 0, 0, 1, 0.0001, 0.0003, 0.01 }; struct Gyro1DKalman filter_roll_3 = { 0, 0, 1, 0, 0, 1, 0.0001, 0.0003, 0.01 }; // IMU 구조체 3개 초기화 struct IMU IMU1 = { { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; struct IMU IMU2 = { { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; struct IMU IMU3 = { { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; //================================================ // FUNC : OpenGL용 함수 //------------------------------------------------ // 함수설명 : 화면에 Draw하는 함수들 // 인 수 값 : 메세지윈도우의 타이틀 문자열 포인터 // 리 턴 값 : 없 음 void InitVisibility( ) { glEnable(GL_CULL_FACE); // 후면제거 glFrontFace(GL_CW); glCullFace(GL_BACK); glEnable(GL_DEPTH_TEST); // 은면제거 } void MyReshape(int w, int h) { glViewport(0, 0, w, h); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(40.0, (GLfloat)w / (GLfloat)h, 1.0, 100.0); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); } void MyDisplay( ) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glBindTexture(GL_TEXTURE_2D, MyTextureObject[0]); glBegin(GL_QUADS); glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, -1.0f, 0.2f); //앞면 glTexCoord2f(1.0f, 0.0f); glVertex3f(1.0f, -1.0f, 0.2f); glTexCoord2f(1.0f, 1.0f); glVertex3f(1.0f, 1.0f, 0.2f); glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 1.0f, 0.2f); glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -1.0f, -0.2f); //뒷면 glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, 1.0f, -0.2f); glTexCoord2f(0.0f, 1.0f); glVertex3f(1.0f, 1.0f, -0.2f); glTexCoord2f(0.0f, 0.0f); glVertex3f(1.0f, -1.0f, -0.2f); glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 1.0f, -0.2f); //윗면 glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, 1.0f, 0.2f); glTexCoord2f(1.0f, 0.0f); glVertex3f(1.0f, 1.0f, 0.2f); glTexCoord2f(1.0f, 1.0f); glVertex3f(1.0f, 1.0f, -0.2f); glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, -1.0f, -0.2f); //아랫면 glTexCoord2f(0.0f, 1.0f); glVertex3f(1.0f, -1.0f, -0.2f); glTexCoord2f(0.0f, 0.0f); glVertex3f(1.0f, -1.0f, 0.2f); glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -1.0f, 0.2f); glTexCoord2f(1.0f, 0.0f); glVertex3f(1.0f, -1.0f, -0.2f); //우측면 glTexCoord2f(1.0f, 1.0f); glVertex3f(1.0f, 1.0f, -0.2f); glTexCoord2f(0.0f, 1.0f); glVertex3f(1.0f, 1.0f, 0.2f); glTexCoord2f(0.0f, 0.0f); glVertex3f(1.0f, -1.0f, 0.2f); glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, -1.0f, -0.2f); //좌측면 glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -1.0f, 0.2f); glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, 1.0f, 0.2f); glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 1.0f, -0.2f); /* * glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, -0.2f, 1.0f); //앞면 * glTexCoord2f(1.0f, 0.0f); glVertex3f( 1.0f, -0.2f, 1.0f); * glTexCoord2f(1.0f, 1.0f); glVertex3f( 1.0f, 0.2f, 1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 0.2f, 1.0f); * glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -0.2f, -1.0f); //뒷면 * glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, 0.2f, -1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f( 1.0f, 0.2f, -1.0f); * glTexCoord2f(0.0f, 0.0f); glVertex3f( 1.0f, -0.2f, -1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 0.2f, -1.0f); //윗면 * glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, 0.2f, 1.0f); * glTexCoord2f(1.0f, 0.0f); glVertex3f( 1.0f, 0.2f, 1.0f); * glTexCoord2f(1.0f, 1.0f); glVertex3f( 1.0f, 0.2f, -1.0f); * glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, -0.2f, -1.0f); //아랫면 * glTexCoord2f(0.0f, 1.0f); glVertex3f( 1.0f, -0.2f, -1.0f); * glTexCoord2f(0.0f, 0.0f); glVertex3f( 1.0f, -0.2f, 1.0f); * glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -0.2f, 1.0f); * glTexCoord2f(1.0f, 0.0f); glVertex3f( 1.0f, -0.2f, -1.0f); //우측면 * glTexCoord2f(1.0f, 1.0f); glVertex3f( 1.0f, 0.2f, -1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f( 1.0f, 0.2f, 1.0f); * glTexCoord2f(0.0f, 0.0f); glVertex3f( 1.0f, -0.2f, 1.0f); * glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, -0.2f, -1.0f); //좌측면 * glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -0.2f, 1.0f); * glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, 0.2f, 1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 0.2f, -1.0f); * * * glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, -1.0f, 1.0f); //앞면 * glTexCoord2f(1.0f, 0.0f); glVertex3f( 1.0f, -1.0f, 1.0f); * glTexCoord2f(1.0f, 1.0f); glVertex3f( 1.0f, 1.0f, 1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 1.0f, 1.0f); * glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -1.0f, -1.0f); //뒷면 * glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, 1.0f, -1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f( 1.0f, 1.0f, -1.0f); * glTexCoord2f(0.0f, 0.0f); glVertex3f( 1.0f, -1.0f, -1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 1.0f, -1.0f); //윗면 * glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, 1.0f, 1.0f); * glTexCoord2f(1.0f, 0.0f); glVertex3f( 1.0f, 1.0f, 1.0f); * glTexCoord2f(1.0f, 1.0f); glVertex3f( 1.0f, 1.0f, -1.0f); * glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, -1.0f, -1.0f); //아랫면 * glTexCoord2f(0.0f, 1.0f); glVertex3f( 1.0f, -1.0f, -1.0f); * glTexCoord2f(0.0f, 0.0f); glVertex3f( 1.0f, -1.0f, 1.0f); * glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -1.0f, 1.0f); * glTexCoord2f(1.0f, 0.0f); glVertex3f( 1.0f, -1.0f, -1.0f); //우측면 * glTexCoord2f(1.0f, 1.0f); glVertex3f( 1.0f, 1.0f, -1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f( 1.0f, 1.0f, 1.0f); * glTexCoord2f(0.0f, 0.0f); glVertex3f( 1.0f, -1.0f, 1.0f); * glTexCoord2f(0.0f, 0.0f); glVertex3f(-1.0f, -1.0f, -1.0f); //좌측면 * glTexCoord2f(1.0f, 0.0f); glVertex3f(-1.0f, -1.0f, 1.0f); * glTexCoord2f(1.0f, 1.0f); glVertex3f(-1.0f, 1.0f, 1.0f); * glTexCoord2f(0.0f, 1.0f); glVertex3f(-1.0f, 1.0f, -1.0f); */ glEnd(); glutSwapBuffers(); } AUX_RGBImageRec *LoadBMP(char *szFilename) { FILE *pFile = NULL; if (!szFilename) return NULL; pFile = fopen(szFilename, "r"); if (pFile) { fclose(pFile); return auxDIBImageLoad(szFilename); //파일로부터 메모리로 } return NULL; } int LoadGLTextures(char *szFilePath) //파일을 로드하고 텍스쳐로 변환 { int Status = FALSE; glClearColor(0.0, 0.0, 0.0, 0.5); memset(pTextureImage, 0, sizeof(void *) * 1); //포인터를 널로 //비트맵을 로드하고 오류확인 if (pTextureImage[0] = LoadBMP(szFilePath)) { Status = TRUE; //상태 플랙을 True로 glGenTextures(1, &MyTextureObject[0]); //텍스쳐 생성 glBindTexture(GL_TEXTURE_2D, MyTextureObject[0]); glTexImage2D(GL_TEXTURE_2D, 0, 3, pTextureImage[0]->sizeX, pTextureImage[0]->sizeY, 0, GL_RGB, GL_UNSIGNED_BYTE, pTextureImage[0]->data); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glEnable(GL_TEXTURE_2D); } if (pTextureImage[0]) { //텍스쳐가 존재하면 if (pTextureImage[0]->data) //텍스쳐 영상이 존재하면 free(pTextureImage[0]->data); //텍스쳐 영상공간 반납 free(pTextureImage[0]); //텍스쳐 반납 } return Status; } //================================================ // FUNC : err_quit() //------------------------------------------------ // 함수설명 : 에러 발생시 메세지로 알려줌 // 인 수 값 : 메세지윈도우의 타이틀 문자열 포인터 // 리 턴 값 : 없 음 void err_quit(char *msg) { LPVOID lpMsgBuf; FormatMessage( FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL, GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPTSTR)&lpMsgBuf, 0, NULL); MessageBox(NULL, (LPCTSTR)lpMsgBuf, msg, MB_ICONERROR); LocalFree(lpMsgBuf); exit(-1); } //================================================ // FUNC : Accel() //------------------------------------------------ // 함수설명 : Accelometer x,y,z 값으로 각도를 얻어냄 // 인 수 값 : Accelometer 센서를 통하여 x,y,z 각각의 가속도 값 // 리 턴 값 : IMU 구조체 struct IMU Accel(struct IMU temp) { double Ax = (temp.packet[4] - temp.Accel_offset_avg[0]) / Accel_sens; double Ay = (temp.packet[5] - temp.Accel_offset_avg[1]) / Accel_sens; double Az = (temp.packet[6] - temp.Accel_offset_avg[2]) / Accel_sens; // temp.A_pitch =atan2(-Az , Ax) * (180 / PI) * 2; // temp.A_roll = atan2(-Az , Ay) * (180 / PI) * 2; // temp.A_pitch = atan(-Az/Ax) * (180 / PI) * 2; temp.A_roll = atan(-Az / Ay) * (180 / PI) * 2; // 여기까지 하면 0~360의 범위로 출력되지만 Gyro센서와 통일하기 위해 약간의 조정 if (temp.A_pitch > 180 && temp.A_pitch <= 360) temp.A_pitch = -(360 - temp.A_pitch); if (temp.A_roll > 180 && temp.A_roll <= 360) temp.A_roll = -(360 - temp.A_roll); printf("Accel_Roll : %f \n", temp.A_roll); return temp; } //================================================ // FUNC : Gyro() //------------------------------------------------ // 함수설명 : Gyro pitch, roll, yaw 값으로 각도를 얻어냄 // 인 수 값 : Gyro 센서를 통하여 pitch, roll yaw 각각의 이전값과 현재값 // 리 턴 값 : IMU 구조체 struct IMU Gyro(struct IMU temp) { // 랑게쿠타 적분을 적용하기 위해 최근,이전3개 값을 배열에 저장하기 위한 for문 for (int i = 0; i < 3; i++) { temp.pitch_vel[i] = temp.pitch_vel[i + 1]; temp.roll_vel[i] = temp.roll_vel[i + 1]; temp.yaw_vel[i] = temp.yaw_vel[i + 1]; } // pitch, roll, yaw의 각속도 뽑아내서 각 velocity [3]에 저장 temp.pitch_vel[3] = ((double)temp.packet[7] - temp.Gyro_offset_avg[0]) * Scalefactor; // scalefactor = 1.611328 deg/~ temp.roll_vel[3] = ((double)temp.packet[8] - temp.Gyro_offset_avg[1]) * Scalefactor; // (3300mV/1024bit) / (2.0mV/degrees/second) temp.yaw_vel[3] = ((double)temp.packet[9] - temp.Gyro_offset_avg[2]) * Scalefactor; // 각속도들을 적분 (랑게쿠타 적분법 사용) // pitch와 roll의 값이 바뀌기 때문에 여기서 두개를 바꿔서 적용 // +와 -를 방향을 뒤집기 위해 더해주는 것이 아니라 -= 으로 적용 temp.G_pitch -= ((temp.roll_vel[0] + 2 * temp.roll_vel[1] + 2 * temp.roll_vel[2] + temp.roll_vel[3]) / 6) / 20; temp.G_roll -= ((temp.pitch_vel[0] + 2 * temp.pitch_vel[1] + 2 * temp.pitch_vel[2] + temp.pitch_vel[3]) / 6) / 20; temp.G_yaw -= ((temp.yaw_vel[0] + 2 * temp.yaw_vel[1] + 2 * temp.yaw_vel[2] + temp.yaw_vel[3]) / 6) / 20; printf("Gyro_Roll : %f \n", temp.G_roll); return temp; } //================================================ // FUNC : Update1() //------------------------------------------------ // 함수설명 : 시간 갱신(Predict), 자이로 센서 값 사용 // 인 수 값 : Kalman 구조체와 dotAngle(자이로 센서로 얻은 각-pitch or roll) // 리 턴 값 : 없 음 void Update1(struct Gyro1DKalman filterdata, const double dotAngle, const double dt) { filterdata.x_angle += dt * (dotAngle - filterdata.x_bias); filterdata.x_bias += 0.014; filterdata.P_00 += -dt * (filterdata.P_10 + filterdata.P_01) + filterdata.Q_angle * dt; filterdata.P_01 += -dt * filterdata.P_11; filterdata.P_10 += -dt * filterdata.P_11; filterdata.P_11 += +filterdata.Q_gyro * dt; } //================================================ // FUNC : Update2() //------------------------------------------------ // 함수설명 : Measurement Update - 칼만이득 계산 후 공분산 업데이트 // 인 수 값 : Kalman 구조체와 dotAngle(가속도 센서로 얻은 각-pitch or roll) // 리 턴 값 : 보정 후의 각을 리턴 double Update2(struct Gyro1DKalman filterdata, const double Accel_x_y_z) { const double y = Accel_x_y_z - filterdata.x_angle; const double S = filterdata.P_00 + filterdata.R_angle; const double K_0 = filterdata.P_00 / S; const double K_1 = filterdata.P_10 / S; filterdata.x_angle += K_0 * y; filterdata.x_bias += K_1 * y; filterdata.P_00 -= K_0 * filterdata.P_00; filterdata.P_01 -= K_0 * filterdata.P_01; filterdata.P_10 -= K_1 * filterdata.P_00; filterdata.P_11 -= K_0 * filterdata.P_01; return filterdata.x_angle; } //================================================ // FUNC : NoiseFunc() //------------------------------------------------ // 함수설명 : 받은 값에 노이즈 제공 ( 노이즈 최대: +10 최저:-10) // 인 수 값 : 원본인 IMU1에서 얻은 값, 변환될 IMU2 or IMU3 // 리 턴 값 : IMU2 or IMU3 구조체 전체를 리턴 struct IMU NoiseFunc(int *originalP, struct IMU temp) { temp.packet[0] = originalP[0]; //counter temp.packet[1] = originalP[1] + ((rand() % 20) - 10); //mag temp.packet[2] = originalP[2] + ((rand() % 20) - 10); temp.packet[3] = originalP[3] + ((rand() % 20) - 10); temp.packet[4] = originalP[4] + ((rand() % 4) - 2);//acc temp.packet[5] = originalP[5] + ((rand() % 4) - 2); temp.packet[6] = originalP[6] + ((rand() % 4) - 2); temp.packet[7] = originalP[7] + ((rand() % 10) - 5);//gyro temp.packet[8] = originalP[8] + ((rand() % 10) - 5); temp.packet[9] = originalP[9] + ((rand() % 10) - 5); return temp; } //================================================ // FUNC : Get_offset_sum() //------------------------------------------------ // 함수설명 : 각 IMU의 10초간의 offset의 합을 얻어냄 // 인 수 값 : IMU 구조체 // 리 턴 값 : 연산 후 IMU 구조체 그대로 리턴 struct IMU Get_offset_sum(struct IMU temp) { temp.Gyro_offset_sum[0] += temp.packet[7]; // Gyro pitch temp.Gyro_offset_sum[1] += temp.packet[8]; // Gyro roll temp.Gyro_offset_sum[2] += temp.packet[9]; // Gyro yaw temp.Accel_offset_sum[0] += temp.packet[4]; // accel x temp.Accel_offset_sum[1] += temp.packet[5]; // accel y temp.Accel_offset_sum[2] += temp.packet[6]; // accel z return temp; } //================================================ // FUNC : Get_offset_avg() //------------------------------------------------ // 함수설명 : 각 IMU의 10초간의 offset의 합을 평균냄 // 인 수 값 : IMU 구조체 // 리 턴 값 : 연산 후 IMU 구조체 그대로 리턴 struct IMU Get_offset_avg(struct IMU temp) { temp.Gyro_offset_avg[0] = (double)temp.Gyro_offset_sum[0] / 200; temp.Gyro_offset_avg[1] = (double)temp.Gyro_offset_sum[1] / 200; temp.Gyro_offset_avg[2] = (double)temp.Gyro_offset_sum[2] / 200; temp.Accel_offset_avg[0] = (double)temp.Accel_offset_sum[0] / 200; temp.Accel_offset_avg[1] = (double)temp.Accel_offset_sum[1] / 200; temp.Accel_offset_avg[2] = (double)temp.Accel_offset_sum[2] / 200; return temp; } //================================================ // FUNC : 이하 Voting System //------------------------------------------------ // IMU에서 오는 input값의 범위를 체크하여 에러인지 확인한다. bool ReleaseErr(double d) { if (d <= 180 && d >= -180) return true; else return false; } //각 값들의 차이를 측정한다. 가장 작은 차이의 두값을 얻어내기 위함 double FormalMajorityVoter(double e, double f) { double g; g = e - f; return abs(g); } // 데이터 값의 차이가 가장 작은 것을 선택한다. double SelData(double a, double b, double c) { double tempAA; // tempAA = min(a, b, c); if (a <= b) { if (a <= c) tempAA = a; else tempAA = c; } else if (b <= c) { if (b <= a) tempAA = b; else tempAA = a; } else if (c <= a) { if (c <= b) tempAA = c; else tempAA = b; } return tempAA; } // 선택된 2개의 input값의 평균을 얻는다. double MeanVoter(double a, double b) { return (a + b) / 2.0; } // Voting System함수 double UavVoter(double a, double b, double c) // IMU에서 3개의 input값을 받는다. { double tempA, tempB, tempC; // IMU에서 얻어온 값들간의 차이 값을 저장할 임시 변수 double selData; // 각 값들의 차이값 중 가장 작은 차이값을 저장할 변수 double result; // voting system의 최종 결과값을 저장 ReleaseErr(a); // 에러체크 ReleaseErr(b); ReleaseErr(c); tempA = FormalMajorityVoter(a, b); // IMU에서 얻어온 값의 차이값(절대값)을 저장 tempB = FormalMajorityVoter(a, c); tempC = FormalMajorityVoter(b, c); selData = SelData(tempA, tempB, tempC); // 가장 작은 차이값을 선택 // 가장 작은 값의 차이를 보이는 2개의 IMU input값들의 평균을 구한다. if (tempA == selData) result = MeanVoter(a, b); else if (tempB == selData) result = MeanVoter(a, c); else if (tempC == selData) result = MeanVoter(b, c); return result; } // 여기까지 Voting System Functions. //================================================ //================================================ // FUNC : ControlImage() //------------------------------------------------ // 함수설명 : IMU에서 입력 받은 값을 통해서 이미지파일을 컨트롤 하는 함수 // 인 수 값 : x는 pitch, y는 roll, yaw(아직 미정) // 리 턴 값 : 없 음 void ControlImage(double x, double y, double z) // x는 pitch, y는 roll, yaw(아직 미정) { theta[3] = theta[0]; theta[4] = theta[1]; theta[5] = theta[2]; // 새롭게 얻어온 값과 현재 좌표계의 각도와의 차만큼 전역좌표계 z축을 회전 시킨다. theta[0] = (-theta[3]) + x; // theta[1] = (-theta[4]) + y; // theta[2] = (-theta[5]) + z; // theta[0] = x; // theta[1] = y; theta[2] = z; glRotatef(theta[0], 1.0, 0.0, 0.0); // z축 방향으로 회전 theta[1]만큼 회전 glRotatef(theta[1], 0.0, 1.0, 0.0); // z축 방향으로 회전 theta[1]만큼 회전 glRotatef(theta[2], 0.0, 0.0, 1.0); // z축 방향으로 회전 theta[1]만큼 회전 glutPostRedisplay(); // z축 반대방향으로 회전 theta[1]만큼 회전, 전역좌표계를 회전하기 전 상태로 돌리기 위해 glRotatef(theta[3], -1.0, 0.0, 0.0); glRotatef(theta[4], 0.0, -1.0, 0.0); glRotatef(theta[5], 0.0, 0.0, -1.0); } //================================================ // FUNC : SpinControl() //------------------------------------------------ // 함수설명 : 무한으로 반복되면서 IMU에서 값을 얻어 ControlImage함수로 값을 보냄 // 인 수 값 : 의미 없음 // 리 턴 값 : 없 음 ////////////// 회전값이 360도를 넘어가지 않도록 하는 함수 (float의 최대,최소값을 넘기는것을 방지) int a = 1; void SpinControl(int Value) { //---[ 메모리 블럭을 0으로 만듬 ] ZeroMemory(buf, sizeof(buf)); retval = ReadFile(hComm, buf, BUFSIZE, &BytesRead, NULL); if (retval == 0) err_quit("ReadFile()"); if (BytesRead == 0) printf("[오류] 응답이 없습니다!\n"); // continue; //---[ 받은 데이터 출력 ] buf[BytesRead] = '\0'; // ---[ 한 패킷 단위로 데이터를 파싱해서 변환하여 저장 ] // Count = 0 // Magnetic x=1, y=2, z=3 // Accelometer x=4, y=5, z=6 // Zyro pitch=7, roll=8, yaw=9 IMU1.packet[0] = (int)buf[1] * 256 + (int)buf[2]; IMU1.packet[1] = (int)buf[3] * 256 + (int)buf[4]; IMU1.packet[2] = (int)buf[5] * 256 + (int)buf[6]; IMU1.packet[3] = (int)buf[7] * 256 + (int)buf[8]; IMU1.packet[4] = (int)buf[9] * 256 + (int)buf[10]; IMU1.packet[5] = (int)buf[11] * 256 + (int)buf[12]; IMU1.packet[6] = (int)buf[13] * 256 + (int)buf[14]; IMU1.packet[7] = (int)buf[15] * 256 + (int)buf[16]; IMU1.packet[8] = (int)buf[17] * 256 + (int)buf[18]; IMU1.packet[9] = (int)buf[19] * 256 + (int)buf[20]; //노이즈 생성 - IMU2, IMU3 IMU2 = NoiseFunc(IMU1.packet, IMU2); IMU3 = NoiseFunc(IMU1.packet, IMU3); // -- [is_start가 아직 0이고, 10초가 아직 안 지났다면 수행] if (TenSec < 200) { IMU1 = Get_offset_sum(IMU1); IMU2 = Get_offset_sum(IMU2); IMU3 = Get_offset_sum(IMU3); TenSec++; if (TenSec == 199) { IMU1 = Get_offset_avg(IMU1); IMU2 = Get_offset_avg(IMU2); IMU3 = Get_offset_avg(IMU3); } } else { // 10개의 값을 테스트용으로 출력 printf("===================================\n"); printf(" count mag_x mag_y mag_z acc_x acc_y acc_z pitch roll yaw\n"); printf(" Packet : "); for (int k = 0; k < 10; k++) printf("%5d ", IMU1.packet[k]); printf("\n\n"); printf("--- IMU1 ---\n"); //최종 Gyro, Accel, KalmanFilter에 넣어보기 IMU1 = Gyro(IMU1); IMU1 = Accel(IMU1); Update1(filter_roll_1, IMU1.G_roll, dt); IMU1.LK_roll = Update2(filter_roll_1, IMU1.A_roll); printf("Kalman_Roll : %f \n\n", IMU1.LK_roll); printf("--- IMU2 ---\n"); IMU2 = Gyro(IMU2); IMU2 = Accel(IMU2); Update1(filter_roll_2, IMU2.G_roll, dt); IMU2.LK_roll = Update2(filter_roll_2, IMU2.A_roll); printf("Kalman_Roll : %f \n\n", IMU2.LK_roll); printf("--- IMU3 ---\n"); IMU3 = Gyro(IMU3); IMU3 = Accel(IMU3); Update1(filter_roll_3, IMU3.G_roll, dt); IMU3.LK_roll = Update2(filter_roll_3, IMU3.A_roll); printf("Kalman_Roll : %f \n\n", IMU3.LK_roll); Final_Roll = UavVoter(IMU1.LK_roll, IMU2.LK_roll, IMU3.LK_roll); printf("Final Roll is !!!! %f \n", Final_Roll); printf("===================================\n\n\n"); } ControlImage(0, 0, IMU1.G_roll); glutTimerFunc(9, SpinControl, 1); // 120hz 이므로 약0.008초 } //================================================ // FUNC : main() //------------------------------------------------ // 함수설명 : 프로그램 시작함수 void main(int argc, char *argv[]) { //---< STEP 1 : 포트 열기 >----------------------- // : 포트열기 함수 - CreateFile() // : 인수1 - 포트이름 문자열 // : 그밖의 인수 - 직렬 포트를 다룰때는 아래와 // 동일하게 설정함 // : 리턴값 - 파일열기 성공하면 핸들값 리턴 // 실패하면 INVALID_HANDIL_VALUE값 리턴 // GetLastError()에 에러메세지 보냄 //------------------------------------------------ hComm = CreateFile("COM2", GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); if (hComm == INVALID_HANDLE_VALUE) err_quit("CreateFile()"); //---< STEP 2 : 포트 설정값 변경 >---------------- // : 포트설정값 얻기함수 - GetCommState() // : 포트설정값 변경함수 - SetCommState() // : 성공->0이 아닌값, 실패->0 //------------------------------------------------ DCB dcb; if (!GetCommState(hComm, &dcb)) err_quit("GetCommState()"); //---[ 포트 설정값 변경 ] // : 115200bps, no parity(패리티), 8bit(데이타비트), 1 stop bit(정지비트) dcb.BaudRate = CBR_115200; dcb.fParity = FALSE; dcb.fNull = FALSE; dcb.ByteSize = 8; dcb.Parity = NOPARITY; dcb.StopBits = ONESTOPBIT; if (!SetCommState(hComm, &dcb)) err_quit("SetCommState()"); //---< STEP 3 : 읽기와 쓰기 타임아웃 설정 >------- // : 이 함수는 읽거나 쓰기위해 무한정 대기하는 것을 방지함 // 타임아웃이 경과되면 함수가 무조건 리턴함 // : 타임아웃 얻기함수 - GetTimeouts() // : 타임아웃 변경함수 - SetTimeouts() // : 성공->0이 아닌값, 실패-> 0 // : -Multiplier, -Constant값이 둘다 0이면 // 정해진 바이트를 쓰거나 읽을 때까지 무한정 대기함 // : 설정단위는 ms임 // : 읽기 타임아웃 -> n*ReadTotalTimeoutMultiplier + ReadTotalTimeoutConstant // : 쓰기 타임아웃 -> n*WriteTotalTimeoutMultiplier + WriteTotlaTimeoutConstant //------------------------------------------------ COMMTIMEOUTS timeouts; timeouts.ReadIntervalTimeout = 0; //---[ 여기서는 512바이트 보내므로 읽기 타임아웃은 512ms임 ] timeouts.ReadTotalTimeoutMultiplier = 22; timeouts.ReadTotalTimeoutConstant = 0; timeouts.WriteTotalTimeoutMultiplier = 0; timeouts.WriteTotalTimeoutConstant = 0; if (!SetCommTimeouts(hComm, &timeouts)) err_quit("SetCommTimeouts()"); //srand: 난수 생성을 위해. srand((unsigned)time(NULL)); /////////////////////////////======== 오픈지엘 if (argc <= 1) { printf("\n%s\n\n", "Usage : TextureDLG3_Consol.exe [BMPFileName.bmp]"); exit(1); } else if (argc > 2) { printf("\nToo Many Arguments!\nArgument is Only one\n"); exit(1); } else { glutInit(&argc, argv); glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH); glutInitWindowSize(1250, 750); glutInitWindowPosition(0, 0); glClearColor(1.0, 1.0, 1.0, 1.0); InitVisibility(); glutCreateWindow("Linear Kalman Filter"); glutReshapeFunc(MyReshape); glutDisplayFunc(MyDisplay); if (LoadGLTextures(argv[1])) { glEnable(GL_TEXTURE_2D); glShadeModel(GL_SMOOTH); glClearDepth(1.0); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); glutTimerFunc(50, SpinControl, 1); } // end of if glutMainLoop( ); } // end of else }