[41호]Silence Quadcopter (Rotor’s Noise Canceling Quadcopter)
2016 ICT 융합 프로젝트 공모전 입선
Silence Quadcopter(Rotor’s Noise Canceling Quadcopter)
글 | 단국대학교 정의동, 최진우
심사평
JK전자 현재 무인 항공기(드론)는 성능(기능), 안정성, 배터리 지속력에 초점이 맞추어져 개발이 진행되고 있는 것이 사실이다. 언젠가는 무인 항공기가 어느 정도 성능을 갖추고 실생활에 사용되기 시작하면 소음에 대한 이슈도 분명히 대두될 것이다. 본 작품에서 구현하고 있는 반대 주파수를 이용해서 소음을 감쇠하는 방법 외에도 모터, 프러펠러 개선 등의 다양한 방법으로 소음을 줄이려는 시도가 있을것이다.
뉴티씨 매우 좋은 작품이며, 특히 DSP를 활용해 FFT 및 IFFT 등을 통하여 원하는 주파수를 찾고 역으로 주파수를 발생시켜 상쇄시키는, 예전에 공중전화 부스 등에서 적용되었던 아이디어를 개선해 현재의 실용적인 부분에서 적용한 점이 돋보인다. 드론의 중요성이 커지고있는 이 때에, 조용한 드론을 만들어야 하는 필요성도 매우 높아질 것으로 예상되는 바, 이 때문에 좋은 점수를 받았으며, 실제 일부 성공한 점 등도 좋은 점수를 받았다. 앞으로 더 연구에 매진하여 만점짜리 결과가 나오기를 바란다.
칩센 재미있는 주제에 노이즈캔슬링 원리를 잘 적용하였다. 다른 분야에도 시도를 해보면 좋을 것 같다.
위드로봇 드론의 뜻이 원래 시끄럽게 붕붕거리다는 뜻인데, 이 소리를 없애는데 관심을 가진 재미있는 아이디어가 돋보이는 작품입니다. 노이즈 캔슬레이션 알고리즘을 좀 더 깊이 공부하고, 이를 구현했더라면 더 나은 결과가 나왔을 것 같습니다. 또한 정량적인 평가를 위해 노이즈를 측정할 수 있는 방법이 별도로 필요해 보입니다. 창의성과 기대효과 측면에 높은 점수를 드립니다.
1. 작품제목
작품의 제목은 Silence Quadcopter(Rotor’s Noise Canceling Quadcopter)입니다. 제목을 해석해 보자면 조용한 쿼드콥터 즉, 로터의 소음을 없애(로터 소리의 역 주파수의 소리를 내보내어 상쇄시켜) 로터의 소리를 최소화시킨 쿼드콥터라는 뜻입니다.
2. 작품 개요
현재 드론(쿼드콥터, X콥터, UAV 등의 총칭) 분야는 중국, 미국 등의 방대한 투자 및 개발에 힘입어 더 이상의 발전은 없을 것이라 생각할 정도로 성장해 왔습니다. 하지만 그에 반해 몇몇 작지 않은 문제점들도 발견되었습니다. 그 중에서 크게 네 가지를 꼽아 보았습니다.
첫째, 안전 문제가 있습니다. 드론을 접해본 사람이라면 누구나 프로펠러의 위험성을 느낄 수 있습니다. 그리하여 만약, 드론이 비행 도중 주변의 물체 또는 사람 등과 부딪힐 경우 큰 사고로 번질 위험이 있습니다.
둘째, 최근 들어 크게 대두가 되고 있는 보안 및 사생활 문제가 있습니다. 드론 촬영을 하는 경우, 허가받지 않은 장소 촬영 또는 허가받지 않은 상태에서의 다른 사람 촬영 등은 사생활 침해 및 초상권 침해 등 법적인 문제를 야기할 수 있습니다. 또한 드론 관련 법안이 제대로 나와있지 않은 현재로서는 큰 문제를 야기할 수 있습니다.
셋째, 소음 문제가 있습니다. 드론을 접해 보았거나 드론에 가까이 가본 사람이라면 누구나 느낄 수 있는 것이 소리가 너무 크다는 것입니다. 드론산업 및 드론 취미가 많이 급증하고 있는 현재, 이 소음문제는 앞으로 더욱 커질 전망입니다.
넷째, 배터리 문제가 있습니다. 드론의 가장 큰 문제 중 하나로 생각되는 것이 바로 배터리 문제입니다. 완충 기준 거의 대부분의 드론들은 비행시간이 30분을 넘기지 못하고 있습니다.
이러한 네 가지 문제를 해결하기 위해, 안전 문제는 물체 감지 및 정확한 제어 등으로 해결하고 있는 추세이고, 보안 및 사생활 문제는 법안이 만들어지고 있으며, 배터리 문제는 기업들의 배터리 개발 등으로 해결하고는 있지만 한계가 있을 것으로 보입니다. 반면 소음문제는 아직 전혀 해결되지 않고 있습니다.
그래서 소음 문제를 해결하기 위해 생각해낸 것이 ‘로터(모터 + 프로펠러) 소리의 반대되는 주파수로 상쇄시킴으로서 소음 문제를 해결하자’ 라는 것이고, 배터리 문제를 해결하기 위해 ‘모터에 자가 동력장치(주로 자전거 바퀴에 많이 설치되어 야간조명 등에 이용됨)를 설치하여 비행과 동시에 충전이 가능하게 설계를 하자’ 라는 것입니다.
처음 프로젝트 목표는 소음 및 배터리 문제, 이 두 가지를 해결하자는 것이었지만 자가 동력장치를 추가하면 결국에는 그 만큼의 모터 출력을 더 요구하게 되어 즉, 에너지 보존법칙에 의해 충전되는 양 이상의 에너지를 필요로 하기 때문에 효율성이 떨어지게 되어 배터리 문제는 좀 더 고민을 해보고, 우선 소음 문제부터 해결하기로 목표를 수정하였습니다.
3. 작품 설명
3.1 주요 동작 및 특징
이 작품은 기존의 쿼드콥터에 로터의 소음을 없애는 기능을 추가시킨 쿼드콥터 입니다. 그래서 이 쿼드콥터는 크게 두 부분으로 나눌 수 있습니다. 하나는 제어부, 다른 하나는 센서부입니다.
우선 전체적인 하드웨어를 살펴보면 CPU + 제어부(모터, 프로펠러, ESC) + 센서부(마이크센서, 사운드센서, 증폭기, 핀 확장 소자, DAC 변환 소자) + 배터리 + 프레임으로 이루어져 있습니다.
소프트웨어를 살펴보면, 제어부 소스는 소스가 매우 짧고 간결하게 짜여 있다는 것을 알 수 있습니다. 외부 인터럽트를 이용해 주기적으로 모터 제어를 할 수 있게 하였는데, 모터 제어는 PID 제어 방식을 이용하였습니다. PID 오픈 소스들을 살펴보면 너무 자료가 많고, 다 다르고, 소스의 크기 또한 작지 않아 최대한 간편히 코딩을 해 보았습니다.
기본적인 개념인 P = 비례, I – 적분, D – 미분을 이용하여 PID 값 = (현재각도 * P게인값) + {이전 적분 누적값 + (I게인값 * 현재각도 * 0.01)} + {D게인값 * (현재각도 – 이전각도)} 이라는 공식을 세우고 그에 맞게 소스를 구현했습니다. 그리고 Roll 축, Pitch 축의 각도 값은 10개를 평균내 사용하였는데 그 이유는 최대한 쓰레기 값을 버리고, 최대한 정밀한 값을 사용하게 하기 위해서입니다. 예를 들어 10개의 각도값이 차례로 10 12 13 15 16 16 18 195 19 20 일때 18에서 195로 넘어가는 각도값의 차이는 177입니다. 하지만 10개의 각도를 평균내 사용하면 33.4 – 18 = 15.4로 오류를 최소한으로 줄일 수 있게 됩니다. P, I, D의 각 게인값 들은 직접 시소 테스트, 호버링 테스트 등을 하며 맞추었고, 외부 인터럽트의 주기는 가장 적합한 주기 네 가지 1ms, 2ms, 10ms, 20ms를 구상하여 테스트를 해 그 중에서도 가장 적합한 주기 20ms를 찾았습니다.
IMU 세 축 Yaw 축, Roll 축, Pitch 축 중 Pitch 축, Roll 축은 PID제어를 사용하였고, Yaw 축은 PI제어를 사용하였는데 Yaw 축만 다른 두 축과는 다르게 PI 제어를 사용한 이유는 Roll, Pitch 제어로 맞춘 평행을 Yaw 축 제어를 하면서 너무 많은 값이 더해지게 될 수 있는데, 이를 아래 예시를 통해 설명하겠습니다.
ex) 1motor_pwm = input_pwm + Pitch_pid_result + Yaw_pid_result 일 때 input_pwm은 공통이고, Pitch_pid_result는 맞추었다면 그 상태에서 Yaw_pid_result의 값이 너무 커지거나 너무 작아지게 되면 전체 pwm인 1motor_pwm의 값의 변화가 너무 커지게 됩니다. 그 결과 전체적인 자세제어에 문제가 생기게 되어 비행하는데 영항을 끼치게 되므로 영향을 최대한 덜 미치면서 방향 제어를 할 수 있는 제어 방식을 추구하다 보니 PI 제어가 가장 적합하다 생각하였습니다. 또한, Yaw 축 소스만은 Roll, Pitch 소스와는 다른 부분인
if(yawcnt == 1) yawerr = g_iq17yaw;
else ;
라는 부분이 있는데 이는 초기 yaw 각도 값이 너무 크거나 작을 경우 모터 출력에 영향을 주게 되어 그것을 막기 위해 초기 에러 각도 값을 지정해 주어 각도에다 그 에러 값을 빼주어 현재 각도 값을 산출해 이용했습니다.
PID 게인 값을 맞춘 과정은 따로 설명을 하지 않았는데, 0부터 0.1 또는 0.01씩을 올려가며 자세 안정의 수렴, 발산 등을 맞추어 준 것입니다.
그리고 쿼드콥터의 형태는 +형태로 하였습니다. +형태, x형태 둘 중 어떤 방식으로 할까 고민하다가 보다 테스트하기 간편하고, 그러면서 자세제어가 정확한 +형태로 하였습니다. +형태는 4개의 로터에 Yaw 축 제어가 공통으로 들어가 있는 상태에서 각각 대칭으로 한 쌍은 Pitch 축 제어, 또 다른 한 쌍은 Roll 축 제어를 하게 하여 각 모터 당 Yaw 축을 제외하고 하나의 축만을 제어해 테스트에 용이하였고, 한 축을 하나의 모터로만 제어하는 것에 비해 자세제어가 안정적으로 잘 되었습니다.
센서부 소스는 제어부 소스에 비해 소스의 길이가 상당히 길다는 점을 알 수 있습니다. 우선 센서부도 제어부와 마찬가지로 외부 인터럽트를 써서 센서 adc 값을 받아들이고, FFT 변환을 하고, 변환된 값의 반대되는 값을 보냅니다. FFT 변환 소스는 paulbourke.net에 있는 DFT – FFT 이론을 바탕으로 짜여진 것이고, 그 소스를 가져와 수정해 사용했습니다.
FFT는 Fast Fourier Transform 즉, 고속 푸리에 변환으로서 DFT (Discrete Fourier Transform – 이산 푸리에 변환)의 고속 모드라고 생각하면 될 것 같습니다. FFT에 대해 좀 더 자세히 설명을 해 보자면 FFT는 시간영역에서 변화하는 데이터 값을 주파수 영역에서의 데이터 값으로 변환시켜주는 알고리즘입니다. 즉, 보는 관점을 시간영역에서 주파수 영역으로 바꿔주어 값을 사용하는 것입니다.
그럼 왜 관점을 시간관점에서 주파수관점으로 바꿔서 계산을 하느냐하면, 우선 소리란 물체의 진동에 의하여 생긴 음파가 귀청을 울리어 귀에 들리는 것이라고 사전에 정의되어 있습니다. 즉, 소리는 진동수에 의해 크기가 결정되고, 진폭에 의해 세기가 결정됩니다. 그렇기 때문에 단위를 시간에서 주파수로 바꾸어준 후 그 값에 반대되는 값을 계산하고, 이를 다시 시간단위로 바꾼 후에 DAC변환을 통해 소리로 출력을 해야 하는 것입니다.
전체적인 신호처리 과정을 대략적으로 설명하자면, MIC센서를 이용한 아날로그값 받기 ▶ DAC변환소자를 이용한 디지털값으로 변환 ▶ 변환된 디지털값을 FFT 및 RFFT(Reverse FFT)처리 ▶ IFFT(Inverse FFT)처리 ▶ 디지털값 출력 ▶ DAC컨버터 구축 ▶ 아날로그값 변환 및 출력 ▶ 사운드센서로 소리 출력입니다. 여기서 RFFT는 말 그대로 FFT를 역 변환 해주는 것이고, IFFT는 FFT의 반대 즉, 시간 영역에서 주파수 영역으로 바뀌었던 데이터를 반대로 주파수 영역에서 시간 영역으로 바꿔주는 것을 의미합니다. 소리를 출력하여 로터의 소음과 상쇄시킨 결과 완벽히 소음이 줄지는 않았으나, 누가 봐도 소음이 줄었다는 것을 느낄 수 있을 정도까지는 되었습니다. 그래서 현재는 조금씩 주기, 출력 시간을 조정해가며 테스트하고 있습니다.
3.2 전체 시스템 구성
3.3 개발 환경(개발언어, Tool, 사용시스템 등)
언어 – C언어
개발 Tool에서 쓰이는 언어가 C언어이기 때문에, 자연스레 C언어를 이용하여 코딩 및 소스구현을 하게 되었습니다.
Tool – Source Insight (Dsp전용 디버깅 프로그램)
DSP를 다룰 수 있는 툴을 찾다보니 Source Insight가 주변에 사용해 보았던 사람이 많아 쉽게 접근할 수 있었습니다.
시스템 – DSP(tms320f2809, 제조사 : Texas Instrument)
DSP를 선정하게 된 이유는 많은 핀들을 보유함과 동시에 PWM이 잘 되고 처리속도가 빠른며 싼 값에 구할 수 있는 MCU를 찾다보니 DSP가 라즈베리파이, 아두이노, ATmega, 망고보드, cortex 시리즈 등 여러 MCU 중에서 가장 적합하다고 생각해서 메인 MCU로 선정을 하게 되었습니다.
시스템 – 라즈베리파이2(Model B)
라즈베리파이를 선정하게 된 이유는 900MHz의 빠른 속도와, 1GB의 넉넉한 램 용량, 저렴한 가격 및 많은 GPIO 핀을 보유하고 있어서입니다. 물론 신호처리에 최적화된 DSP에 비해서는 살짝 부족하고 ADC 핀도 없어 불편한 감이 있기는 하지만, 모터 제어를 하는 동시에 신호처리를 하기 위해서는 두 개 이상의 MCU를 써야하는데 DSP를 메인 MCU로 쓰고 있기 때문에 서브 MCU로는 괜찮다고 생각된 라즈베리 파이를 선정하였습니다.
4. 단계별 제작 과정
4.1. 계획 세우기 및 사전조사
프로젝트 주제를 정하고 그에 대한 사전조사를 인터넷 및 관련 사람들을 대상으로 조사하여 문제점, 주요특징, 앞으로의 전망 등을 충분히 조사하였습니다.
4.2. 해당 작품 공부 및 제작 준비
쿼드콥터라는 것을 처음 해본만큼, 먼저 제작을 해 보았던 선배님들이나 RC 동호회 사람들, 인터넷 등을 이용해 작품 공부를 하였고, 필요한 부품 및 기자재 등을 준비하였습니다.
4.3. 하드웨어 제작
기본적인 하드웨어(대각선길이-28cm)를 제작하였고, 블루투스 모듈(HC-06), 9축 센서(EBIMU-9DOF), BLDC모터(제조사-hobbyking), 변속기(제조사-hobbywing), MCU(DSP-tms320f2809) 등 각종 센서, 모터, cpu 등을 테스트 해 보았습니다.
4.4. 시소테스트, 호버링 등 구동테스트
구동을 하기 위해 제어기(PID 제어 이용)를 설계하여 직접 코딩 및 테스트하여 각의 게인값들을 맞추고, 9축센서와 CPU 및 PC를 연동시켜 각 축(Roll, Pitch, Yaw)에 대한 제어가 잘 되는지 시소테스트(Roll, Pitch 축)와 줄 매달아놓고 테스트(Yaw 축), 그리고 호버링 테스트를 하면서 확인 및 수정 하였습니다.
4.5. 마이크센서와 라즈베리파이와 스피커센서를 거친 신호처리
로터(모터 + 프로펠러) 근처에 스피커센서를 두어 로터의 소리의 값을 받아와 라즈베리파이의 ADC핀(ADC변환소자 이용)에 연결 및 디지털 데이터를 뽑아냈고, FFT(Fast Fourier Transform) 알고리즘을 거쳐 변환 데이터를 걸러낸 후 그에 반대되는 위상을 가진 역 주파수를 계산 및 출력해 냈으며, DAC과정(CPU의 GPIO핀과 핀 확장소자인 74HC595과 증폭기인 UILM358을 이용)을 거쳐 스피커 센서로 소리를 내보내기 위한 센서소스를 직접 짜고, 테스트 및 쿼드콥터에 설치하여 구동테스트를 하였습니다.
4.6. 안드로이드 연동 및 전체 구동테스트
작품에 안드로이드를 올려 PC와 쿼드콥터와의 통신 및 컨트롤이 아닌 핸드폰 등 스마트기기와의 통신 및 컨트롤을 계획하여 앱을 만들고 기능들을 추가하여 편의성을 더할 예정이고, 안드로이드 스튜디오를 이용해 어플리케이션을 만들 생각입니다. 또한, 적외선센서를 이용해 장애물 감지 및 피해가는 알고리즘을 구성할 것이고, 고도센서를 이용해 보다 정확한 자세제어를 할 수 있게 할 것입니다.
5. 기타(회로도, 소스코드, 참고문헌 등)
5.1. 회로도
5.2. 소스코드
5.2.1. 제어 소스코드
##############################################################
// FILE : Motor.c
// TITLE : Motor c file.
// Author : Jeong Eui Dong
// Company : Maze & Hz
############################################################
// $Release Date: 2016.02.12 $
############################################################
#define _MOTOR_
#include “DSP280x_Device.h”
#include “DSP280x_Examples.h” // DSP280x Examples Include File
#include “Main.h”
#include “Motor.h”
#pragma CODE_SECTION(control_ISR , “ramfuncs2″);
//motor interrupt motor pid , Kalmanfilter 구상하기.
interrupt void control_ISR( void )
{
StopCpuTimer0();
//TxPrintf(“motor_check\n”);
yawcnt ++ ;
i++;
// g_iq17roll = imu roll, g_iq17pitch = imu pitch , g_iq17yaw = imu yaw
//g_iq17errsum = g_iq17errsum + g_iq17ki * 0.01 * g_iq17angles1;
// g_iq17pidresult = g_iq17kp * 오차 + g_iq17ki * g_iq17errsum + kd * (g_iq17angles1 – g_iq17angles2); // 오차 = ki * 0.01 * angles1
/////////////////////////////////////////////////////////////////////
//roll, pitch축 제어는 PID제어 사용
Rollerr = _IQ17(-3.0);
Rollnowangle = g_iq17roll – Rollerr; //Roll
for(i=0; i<9; i++)
Rollangle[i+1] = Rollangle[i];
Rollangle[0] = Rollnowangle;
Rollanglesum = Rollangle[0] + Rollangle[1] + Rollangle[2] + Rollangle[3] + Rollangle[4] + Rollangle[5] + Rollangle[6] + Rollangle[7] + Rollangle[8] + Rollangle[9];
Rollangleresult = _IQ17mpy(Rollanglesum, _IQ17(0.1));
Rollerrsum = Rollerrsum + _IQ17mpy(Rollki, _IQ17mpy(0.01, Rollangleresult));
Rollpidresult = _IQ17mpy(Rollkp, Rollangleresult) + Rollerrsum + _IQ17mpy(Rollkd, (Rollangleresult – Rollpreangle));
Rollhalf = _IQ17mpy(Rollpidresult,_IQ17(0.5));
Rollpreangle = Rollangleresult;
Pitcherr = _IQ17(1.0);
Pitchnowangle = g_iq17pitch – Pitcherr; //Pitch
for(i=0; i<9; i++)
Pitchangle[i+1] = Pitchangle[i];
Pitchangle[0] = Pitchnowangle;
Pitchanglesum = Pitchangle[0] + Pitchangle[1] + Pitchangle[2] + Pitchangle[3] + Pitchangle[4] + Pitchangle[5] + Pitchangle[6] + Pitchangle[7] + Pitchangle[8] + Pitchangle[9];
Pitchangleresult = _IQ17mpy(Pitchanglesum, _IQ17(0.1));
Pitcherrsum = Pitcherrsum + _IQ17mpy(Pitchki, _IQ17mpy(0.01, Pitchangleresult));
Pitchpidresult = _IQ17mpy(Pitchkp, Pitchangleresult) + Pitcherrsum + _IQ17mpy(Pitchkd, (Pitchangleresult – Pitchpreangle));
Pitchhalf = _IQ17mpy(Pitchpidresult,_IQ17(0.5));
Pitchpreangle = Pitchangleresult;
// yaw축 제어는 다른방식 사용 ▶ PI제어 사용 ▶ 최대한 roll, pitch 제어에 영항을 미쳐서는 안된다.
if(yawcnt == 1) Yawerr = g_iq17yaw; //Yaw
else ;
Yawnowangle = g_iq17yaw – Yawerr;
if(Yawnowangle >= _IQ17(180))
Yawnowangle = Yawnowangle – _IQ17(360);
if(Yawnowangle < _IQ17(-180))
Yawnowangle = Yawnowangle + _IQ17(360);
Yawerrsum = Yawerrsum + _IQ17mpy(Yawki, _IQ17mpy(0.01, Yawnowangle));
//Yawpidresult = _IQ17mpy(Yawkp, Yawnowangle) + Yawerrsum + _IQ17mpy(Yawkd, (Yawnowangle – Yawpreangle));
Yawpidresult = _IQ17mpy(Yawkp, Yawnowangle) + Yawerrsum;
Yawpreangle = Yawnowangle;
//운동방향 – 전진 : front high , 후진 : back high , 우측 : right high , 좌측 : left high , 고도상승 : all high , 고도하강 : all low
//1번 모터 시계 ▶ 반대 red – yellow
Bldc1pwmRegs.CMPA.half.CMPA = ( ( g_int32pwm_output + ( Pitchpidresult >> 17 ) + ( Yawpidresult >> 17 )) > 5000 )? (Uint16)5000 : (Uint16)(g_int32pwm_output + ( Pitchpidresult >> 17 ) + ( Yawpidresult >> 17 ) + frontgo); // -
//2번 모터 반시계
Bldc2pwmRegs.CMPA.half.CMPA = ( ( g_int32pwm_output – ( Rollpidresult >> 17 ) – ( Yawpidresult >> 17 )) > 5000 )? (Uint16)5000 : (Uint16) ( g_int32pwm_output – ( Rollpidresult >> 17 ) – ( Yawpidresult >> 17 ) + rightgo); // +
//3번 모터 시계 -> 반대 red – yellow
EPwm5Regs.CMPA.half.CMPA = ( ( g_int32pwm_output – ( Pitchpidresult >> 17 ) + ( Yawpidresult >> 17 )) > 5000 )? (Uint16)5000 : (Uint16)( g_int32pwm_output – ( Pitchpidresult >> 17 ) + ( Yawpidresult >> 17 ) + backgo); // -
//4번 모터 반시계
EPwm6Regs.CMPA.half.CMPA = ( ( g_int32pwm_output + ( Rollpidresult >> 17 ) – ( Yawpidresult >> 17 )) > 5000 )? (Uint16)5000 : (Uint16)( g_int32pwm_output + ( Rollpidresult >> 17 ) – ( Yawpidresult >> 17 ) + leftgo); // +
//////////////////////// X-Copter //////////////////////////////
// 4번 ▶ 1번, 3번 ▶ 4번, 2번 ▶ 3번, 1번 ▶ 2번
/*
//1번 모터 반시계 ▶ 반대 red – yellow 연결 Dir : Front
Bldc2pwmRegs.CMPA.half.CMPA = ( ( g_int32pwm_output – ( Rollhalf >> 17 ) – ( Yawpidresult >> 17 ) + ( Pitchhalf >> 17 )) > 5000 )? (Uint16)5000 : (Uint16) ( g_int32pwm_output – ( Rollhalf >> 17 ) + ( Yawpidresult >> 17 ) + ( Pitchhalf >> 17 ) + frontgo ); // +
//2번 모터 시계 Dir : Right
// EPwm5Regs.CMPA.half.CMPA = ( ( g_int32pwm_output – ( Pitchhalf >> 17 ) + ( Yawpidresult >> 17 ) – ( Rollhalf >> 17 )) > 5000 )? (Uint16)5000 : (Uint16)( g_int32pwm_output – ( Pitchhalf >> 17 ) – ( Yawpidresult >> 17 ) – ( Rollhalf >> 17 ) + rightgo ); // +
//3번 모터 반시계 ▶ 반대 red – yellow 연결 Dir : Back
EPwm6Regs.CMPA.half.CMPA = ( ( g_int32pwm_output + ( Rollhalf >> 17 ) – ( Yawpidresult >> 17 ) – ( Pitchhalf >> 17 )) > 5000 )? (Uint16)5000 : (Uint16)( g_int32pwm_output + ( Rollhalf >> 17 ) + ( Yawpidresult >> 17 ) – ( Pitchhalf >> 17 ) + backgo ); // -
// 4번 모터 시계 Dir : Left
// Bldc1pwmRegs.CMPA.half.CMPA = ( ( g_int32pwm_output + ( Pitchhalf >> 17 ) + ( Yawpidresult >> 17 ) + ( Rollhalf >> 17 )) > 5000 )? (Uint16)5000 : (Uint16)(g_int32pwm_output + ( Pitchhalf >> 17 ) – ( Yawpidresult >> 17 ) + ( Rollhalf >> 17 ) + leftgo ); // -
*/
//test motor pwm
// EPwm5Regs.CMPA.half.CMPA = g_int32pwm_output;
}
5.2.2. 센서 소스코드
//라즈베리파이에서는 소스 복사가 안되어 DSP로 테스트한 소스를 올렸다./////##############################################################
// FILE : Sensor.c
// TITLE : Senser c file.
// Author : Jeong Eui Dong
// Company : Maze & Hz
//############################################################
// $Release Date: 2016.02.12 $
//############################################################
#include “DSP280x_Device.h”
#include “DSP280x_Examples.h” // DSP280x Examples Include File
#include “Main.h”
#include “Sensor.h”
#define PI 3.14159264
void FFT_Algorithm(volatile int16 dir,volatile int16 m,volatile float32 *Real,volatile float32 *Imagine);
void Initsensor_ADC( void )
{
memset( ( void * )&Sound1real, 0, sizeof( Sound1real ) );
memset( ( void * )&Sound2real, 0, sizeof( Sound2real ) );
memset( ( void * )&Sound3real, 0, sizeof( Sound3real ) );
memset( ( void * )&Sound4real, 0, sizeof( Sound4real ) );
memset( ( void * )&Sound1change, 0, sizeof( Sound1change ) );
memset( ( void * )&Sound2change, 0, sizeof( Sound2change ) );
memset( ( void * )&Sound3change, 0, sizeof( Sound3change ) );
memset( ( void * )&Sound4change, 0, sizeof( Sound4change ) );
memset( ( void * )&Sound1imagine, 0, sizeof( Sound1imagine ) );
memset( ( void * )&Sound2imagine, 0, sizeof( Sound2imagine ) );
memset( ( void * )&Sound3imagine, 0, sizeof( Sound3imagine ) );
memset( ( void * )&Sound4imagine, 0, sizeof( Sound4imagine ) );
memset( ( void * )&Soundcheck, 0, sizeof( Soundcheck) );
}
interrupt void sensor_ADC( void )
{
static Uint16 tick = 0;
static Uint16 check = 0;
static Uint16 cnt = 0;
static _iq17 iqSound1real = _IQ17(0), iqSound2real = _IQ17(0), iqSound3real = _IQ17(0), iqSound4real = _IQ17(0);
static _iq17 iqSound1imagine = _IQ17(0), iqSound2imagine = _IQ17(0), iqSound3imagine = _IQ17(0), iqSound4imagine = _IQ17(0);
static _iq17 iqSound1change = _IQ17(0), iqSound2change = _IQ17(0), iqSound3change = _IQ17(0), iqSound4change = _IQ17(0);
StopCpuTimer2();
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
//TxPrintf(“adc_check\n”);
CpuTimer0Regs.PRD.all=100*7;
check++;
CpuTimer0Regs.PRD.all=100*30;
AdcRegs.ADCTRL2.bit.SOC_SEQ1 = 1;
Soundcheck[0] = AdcMirror.ADCRESULT0;
CpuTimer0Regs.PRD.all=100*30;
AdcRegs.ADCTRL2.bit.SOC_SEQ1 = 1;
Soundcheck[2] = AdcMirror.ADCRESULT2;
CpuTimer0Regs.PRD.all=100*30;
AdcRegs.ADCTRL2.bit.SOC_SEQ1 = 1;
Soundcheck[1] = AdcMirror.ADCRESULT1;
CpuTimer0Regs.PRD.all=100*30;
AdcRegs.ADCTRL2.bit.SOC_SEQ1 = 1;
Soundcheck[3]= AdcMirror.ADCRESULT3;
AdcRegs.ADCTRL2.bit.RST_SEQ1 = 1;
Sound1real[Adcsound] = (float32)Soundcheck[0];
Sound2real[Adcsound] = (float32)(Soundcheck[2] >> 1);
Sound3real[Adcsound] = (float32)(Soundcheck[1] >> 1);
Sound4real[Adcsound] = (float32)(Soundcheck[3] >> 1);
if(Sound1real[Adcsound] < 0) Sound1real[Adcsound] = (float32)0;
if(Sound2real[Adcsound] < 0) Sound2real[Adcsound] = (float32)0;
if(Sound3real[Adcsound] < 0) Sound3real[Adcsound] = (float32)0;
if(Sound4real[Adcsound] < 0) Sound4real[Adcsound] = (float32)0;
if(Adcsound == 128)
{
FFT_Algorithm((int16)1, (int16)8, Sound1real, Sound1imagine);
FFT_Algorithm((int16)1, (int16)8, Sound2real, Sound2imagine);
FFT_Algorithm((int16)1, (int16)8, Sound3real, Sound3imagine);
FFT_Algorithm((int16)1, (int16)8, Sound4real, Sound4imagine);
for(cnt = 0; cnt < Adcsound; cnt++)
{
iqSound1real = _IQ17(Sound1real[cnt]);
iqSound2real = _IQ17(Sound2real[cnt]);
iqSound3real = _IQ17(Sound3real[cnt]);
iqSound4real = _IQ17(Sound4real[cnt]);
iqSound1imagine = _IQ17(Sound1imagine[cnt]);
iqSound2imagine = _IQ17(Sound2imagine[cnt]);
iqSound3imagine = _IQ17(Sound3imagine[cnt]);
iqSound4imagine = _IQ17(Sound4imagine[cnt]);
iqSound1change = _IQsqrt(_IQmpy(iqSound1real, iqSound1real) + _IQmpy(iqSound1imagine, iqSound1imagine));
iiqSound2change = _IQsqrt(_IQmpy(iqSound2real, iqSound2real) + _IQmpy(iqSound2imagine, iqSound2imagine));
iiqSound3change = _IQsqrt(_IQmpy(iqSound3real, iqSound3real) + _IQmpy(iqSound3imagine, iqSound3imagine));
iiqSound4change = _IQsqrt(_IQmpy(iqSound4real, iqSound4real) + _IQmpy(iqSound4imagine, iqSound4imagine));
iSound1change[cnt] = _IQtoF(iqSound1change);
iSound2change[cnt] = _IQtoF(iqSound2change);
iSound3change[cnt] = _IQtoF(iqSound3change);
iSound4change[cnt] = _IQtoF(iqSound4change);
}
Max_arr = Sound1change[0];
for(cnt = 1; cnt < Adcsound; cnt++)
{
if(Max_arr < Sound1change[cnt]) Max_arr = Sound1change[cnt];
else;
if(Max_arr < Sound2change[cnt]) Max_arr = Sound2change[cnt];
else;
if(Max_arr < Sound3change[cnt]) Max_arr = Sound3change[cnt];
else;
if(Max_arr < Sound4change[cnt]) Max_arr = Sound4change[cnt];
else;
}
for(tick = 0; tick < 128; tick++)
{
Sound1real[tick] = Sound1real[tick+1];
Sound2real[tick] = Sound2real[tick+1];
Sound3real[tick] = Sound3real[tick+1];
Sound4real[tick] = Sound4real[tick+1];
}
Adcsound = 127;
Sound1real[128] = 0;
Sound2real[128] = 0;
Sound3real[128] = 0;
Sound4real[128] = 0;
}
Adcsound++;
//AdcRegs.ADCST.bit.INT_SEQ1_CLR = 1;
StopCpuTimer0();
StartCpuTimer2();
}
void FFT_Algorithm(volatile int16 dir,volatile int16 m,volatile float32 *Real,volatile float32 *Imagine)
{
/* Calculate the number of points */
static float32 tx = 0, ty = 0, i2 = 0, k = 0, l1 = 0, l2 = 0, u1 = 0, u2 = 0;
static float32 t1 = 0, t2 = 0;
static int16 i1 = 0, b = 0, l = 0, n = 1, j = 0, i = 0;
static _iq17 iqn = _IQ17(1), iqReal = _IQ17(0), iqImagine = _IQ17(0), iqT1 = _IQ17(0), iqT2 = _IQ17(0), iqU1 = _IQ17(0), iqU2 = _IQ17(0);
static _iq17 c1 = _IQ17(0), c2 = _IQ17(0), z = _IQ17(0);
for (b = 0; b < m; b++)
{
iqn = _IQmpy(iqn, _IQ17(2));
}
n = (Uint16)(_IQtoF(iqn));
/* Do the bit reversal */
i2 = (float32)(n >> 1);
for (i = 0; i < n-1; i++)
{
if (i < j)
{
tx = Real[i];
ty = Imagine[i];
Real[i] = Real[j];
Imagine[i] = Imagine[j];
Real[j] = tx;
Imagine[j] = ty;
}
k = i2;
while (k <= j)
{
j = j – k;
k = (float32)((Uint16)(k) >> 1);
}
j = j + k;
}
/* Compute the FFT */
c1 = _IQ17(-1.0);
c2 = _IQ17(0.0);
l2 = 1;
for (l = 0; l < m; l++)
{
l1 = l2;
l2 = (float32)((Uint16)(l2) << 1);
iqU1 = _IQ17(1.0);
iqU2 = _IQ17(0.0);
for (j = 0; j < l1; j++)
{
for (i = j; i < n; i = i + 12)
{
i1 = i + l1;
iqReal = _IQ17(Real[i1]);
iqImagine = _IQ17(Imagine[i1]);
iqT1 = _IQmpy(iqU1, iqReal) – _IQmpy(iqU2, iqImagine);
iqT2 = _IQmpy(iqU1, iqImagine) – _IQmpy(iqU2, iqReal);
t1 = _IQtoF(iqT1);
t2 = _IQtoF(iqT2);
Real[i1] = Real[i] – t1;
Imagine[i1] = Imagine[i] – t2;
Real[i] += t1;
Imagine[i] += t2;
}
z = _IQmpy(iqU1, c1) – _IQmpy(iqU2, c2);
iqU2 = _IQmpy(iqU1, c2) – _IQmpy(iqU2, c1);
iqU1 = z;
}
c2 = _IQsqrt(_IQmpy((_IQ17(1.0) – c1), _IQ17(0.5)));
if (dir == 1)
c2 = -c2;
c1 = _IQsqrt(_IQmpy((_IQ17(1.0) + c1), _IQ17(0.5)));
}
/* Scaling for forward transform */
if (dir == 1) {
for (i = 0; i < n; i++)
{
iqReal = _IQ17(Real[i1]);
iqImagine = _IQ17(Imagine[i1]);
iqReal = _IQdiv(iqReal, iqn);
iqImagine = _IQdiv(iqImagine, iqn);
Real[i1] = _IQtoF(iqReal);
Imagine[i1] = _IQtoF(iqImagine);
}
}
}
5.2.3. FFT 역변환 소스코드
///////C++로 코딩함.///////
template<class T> inline void swap(T &x, T&y)
{
T z;
z = x; x = y; y = z;
}
void DoFFTInternal(jdouble* data, jint nn) {
unsigned long n, mmax, m, j, istep, i;
jdouble wtemp, wr, wpr, wpi, wi, theta;
jdouble tempr, tempi;
// reverse-binary reindexing
n = nn<<1;
j=1;
for (i=1; i<n; i+=2) {
if (j>i) {
swap(data[j-1], data[i-1]);
swap(data[j], data[i]);
}
m = nn;
while (m>=2 && j>m) {
j -= m;
m >>= 1;
}
j += m;
};
// Danielson-Lanczos section
mmax=2;
while (n>mmax) {
istep = mmax<<1;
theta = -(2*M_PI/mmax);
wtemp = sin(0.5*theta);
wpr = -2.0*wtemp*wtemp;
wpi = sin(theta);
wr = 1.0;
wi = 0.0;
for (m=1; m < mmax; m += 2) {
for (i=m; i <= n; i += istep) {
j=i+mmax;
tempr = wr*data[j-1] – wi*data[j];
tempi = wr * data[j] + wi*data[j-1];
data[j-1] = data[i-1] – tempr;
data[j] = data[i] – tempi;
data[i-1] += tempr;
data[i] += tempi;
}
wtemp=wr;
wr += wr*wpr – wi*wpi;
wi += wi*wpr + wtemp*wpi;
}
mmax=istep;
}
}
5.3. 참고문헌
· 프로젝트로 배우는 라즈베리 파이, 도날드 노리스 지음, 한빛미디어 출판
· Data Manual – TMS320F2809, Texas Instruments 출판
· http://paulbourke.net/miscellaneous//dft/, written by paul bourke
· 그 外 네이버 지식인, 네이버 카페 “당근이의 AVR 갖구 놀기”