2단계:프레임 만들기
DJI 450 프레임을 구입하고 모터와 모든 것을 부착했습니다. 제가 어떻게 했는지 영상을 보실 수 있습니다.
3단계:프레임에 비행 컨트롤러 부착
그런 다음 회로도와 같이 마지막으로 esc와 수신기를 보드에 부착하면 모든 것이 완료됩니다!
<섹션 클래스="섹션 컨테이너 섹션 축소 가능" id="코드"> 코드
<울> MultiWii.ino
MultiWii.inoC/C++
#include "Arduino.h#include "config.h#include "def.h#include "types.h#include "GPS.h#include "Serial.h#include "센서. h#include "MultiWii.h#include "EEPROM.h#include #if GPS//다른 GPS 기능을 위한 함수 프로토타입//이것은 아마도 gps.h 파일로 갈 수 있지만, gps.cpp에 로컬 정적 무효 GPS_bearing(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* 방위);정적 무효 GPS_distance_cm(int32_tlon* lat1, int12* int3 ,uint32_t* dist);정적 무효 GPS_calc_velocity(무효);정적 무효 GPS_calc_location_error( int32_t* target_lat, int32_t* target_lng, int32_t* gps_lat, int32_t* gps_low6_int32_t* gps_low6_int32_t* gps_low_lng_정적 무효);정적 무효 GPS_calc_poshold );정적 무효 GPS_calc_nav_rate(uint16_t max_speed);int32_t wrap_18000(int32_t ang);정적 부울 check_missed_wp( 무효); 무효 GPS_calc_longitude_scaling(int32_t 위도);정적 무효 GPS_update_crosstrack( 무효) p_36000(int32_t ang);// Leadig 필터 - TODO:C++ 대신 일반 C로 다시 작성// 설정 gps lag#if defined(UBLOX) || defined (MTK_BINARY19)#define GPS_LAG 0.5f //UBLOX GPS는 MTK 및 other#else#define GPS_LAG 1.0f보다 지연이 더 작습니다. //MTK GPS는 1초 지연이 있다고 가정합니다.#endif static int32_t GPS_coord_lead[2]; // 리드 필터링된 GPS 좌표 클래스 LeadFilter {public:LeadFilter() :_last_velocity(0) { } // CLI에서 최소 및 최대 라디오 값 설정 int32_t get_position(int32_t pos, int16_t vel, float lag_in_seconds =1.0); 무효 clear() { _last_velocity =0; }개인:int16_t _last_velocity;};int32_t LeadFilter::get_position(int32_t pos, int16_t vel, float lag_in_seconds){ int16_t accel_contribution =(vel - _last_velocity) * lag_in_seconds * int16_t vel_contribution =vel * lag_in_seconds; // 다음 반복을 위한 속도 저장 _last_velocity =vel; 반환 위치 + vel_contribution + accel_contribution;}LeadFilter xLeadFilter; // 긴 GPS 지연 필터 LeadFilter yLeadFilter; // 위도 GPS 지연 필터 typedef struct PID_PARAM_ { float kP; 플로트 KI; 플로트 kD; 플로트 아이맥스; } PID_PARAM; PID_PARAM posholdPID_PARAM;PID_PARAM poshold_ratePID_PARAM;PID_PARAM navPID_PARAM;typedef 구조체 PID_ { 부동 적분기; // 적분기 값 int32_t last_input; // 도함수의 마지막 입력 float lastderivative; // 저역 통과 필터의 마지막 도함수 float 출력; float 미분;} PID;PID posholdPID[2];PID poshold_ratePID[2];PID navPID[2];int32_t get_P(int32_t error, struct PID_PARAM_* pid) { return (float)error * pid->kP;}int32_t get_I (int32_t 오류, float* dt, struct PID_* pid, struct PID_PARAM_* pid_param) { pid->적분기 +=((float) 오류 * pid_param->kI) * *dt; pid->적분기 =제약(pid->적분기,-pid_param->Imax,pid_param->Imax); return pid->integrator;} int32_t get_D(int32_t input, float* dt, struct PID_* pid, struct PID_PARAM_* pid_param) { // 밀리초 단위의 dt pid->derivative =(input - pid->last_input) / *dt; /// 미분 계산을 위한 저역 통과 필터 컷 주파수. 플로트 필터 =7.9577e-3; // "1 / ( 2 * PI * f_cut )"으로 설정; // _filter의 예:// f_cut =10Hz -> _filter =15.9155e-3 // f_cut =15Hz -> _filter =10.6103e-3 // f_cut =20Hz -> _filter =7.9577e-3 // f_cut =25Hz -> _filter =6.3662e-3 // f_cut =30Hz -> _filter =5.3052e-3 // 이산 저역 통과 필터, // 컨트롤러를 미치게 할 수 있는 고주파수 노이즈를 차단합니다. pid-> 도함수 =pid->마지막 도함수 + (*dt / ( 필터 + *dt)) * (pid-> 도함수 - pid->마지막 도함수); // 업데이트 상태 pid->last_input =입력; pid->lastderivative =pid->derivative; // 파생 구성 요소에 추가 return pid_param->kD * pid->derivative;}void reset_PID(struct PID_* pid) { pid->integrator =0; pid->last_input =0; pid->lastderivative =0;}#define _X 1#define _Y 0#define RADX100 0.000174532925 uint8_t land_detect; //땅(외부)정적 감지 uint32_t land_settle_timer;uint8_t GPS_Frame; // 유효한 GPS_Frame이 감지되었고 데이터가 탐색을 위해 준비되었습니다. 계산 static float dTnav; // 탐색 계산을 위한 델타 시간(밀리초), 모든 양호한 GPS로 업데이트됨 readstatic int16_t actual_speed[2] ={0,0};static float GPS_scaleLonDown; // 이것은 우리가 극쪽으로 갈 때 축소되는 경도를 상쇄하는 데 사용됩니다. // 원하는 이동 속도와 실제 이동 속도 간의 차이 // GPS 판독 후 업데이트됨 - 5-10hzstatic int16_t rate_error[2];static int32_t 오류[2];정적 int32_t GPS_WP[2]; //현재 사용 중인 WPstatic int32_t GPS_FROM[2]; //정확한 추적을 위한 이전 경유지 다음int32_t target_bearing; // 헬리콥터에서 "next_WP" 위치까지의 각도(도)입니다. * 100static int32_t original_target_bearing; // deg * 100, next_WP가 설정되었을 때 next_WP에 대한 원래 각도, 또한 WPstatic int16_t crosstrack_error를 전달할 때 확인하는 데 사용됩니다. // 콥터를 최적의 pathuint32_t wp_distance로 되돌리기 위해 target_bearing에 적용된 각도 보정의 양; // cmstatic 단위로 평면과 next_WP 사이의 거리 uint16_t waypoint_speed_gov; // 탐색을 시작할 때 느린 속도로 감기에 사용됨;////////////////////////////////////// //////////////////////////////////////////////// 이동 평균 필터 변수//#define GPS_FILTER_VECTOR_LENGTH 5정적 uint8_t GPS_filter_index =0;정적 int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];정적 int22_t GPS_filter_sum[2];정적 int32_t GPS_read32]정적 int32_t GPS_read32; //소수점 없는 위도(lat/10 000 000)static uint16_t fraction3[2];static int16_t nav_takeoff_bearing; // 집에 도착할 때 이륙 방향으로 회전하는 데 사용되는 이륙 시 베어링 저장(1deg =1)//주 탐색 프로세서 및 상태 엔진// TODO:처리 부담을 완화하기 위해 진행 상태 추가 uint8_t GPS_Compute(void) { unsigned char axis; uint32_t 거리; // 콥터에 dist를 저장할 임시 변수 int32_t dir; // 콥터에 dir을 저장할 임시 변수 static uint32_t nav_loopTimer; // 유효한 프레임이 있는지 확인하고 그렇지 않은 경우 즉시 반환 if (GPS_Frame ==0) return 0; 그렇지 않으면 GPS_Frame =0; // 홈 위치를 확인하고 설정되지 않은 경우 설정 if (f.GPS_FIX &&GPS_numSat>=5) { #if !defined(DONT_RESET_HOME_AT_ARM) if (!f.ARMED) {f.GPS_FIX_HOME =0;} #endif if (!f.GPS_FIX_HOME &&f.ARMED) { GPS_reset_home_position(); } // 이동 평균 필터를 GPS 데이터에 적용 if (GPS_conf.filtering) { GPS_filter_index =(GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH; (축 =0; 축<2; 축++) { GPS_read[축] =GPS_coord[축]; // 필터링되지 않은 최신 데이터는 GPS_latitude 및 GPS_longitude에 있습니다. GPS_degree[axis] =GPS_read[axis] / 10000000; // 합계가 int32_t에 맞는지 확인하기 위해 차수를 얻습니다. // 우리가 차수 선에 얼마나 가까웠습니까? 도의 분수에서 처음 세 자리 // 나중에 도선에 가까우면 확인하는 데 사용합니다. 그렇다면 평균을 비활성화합니다. fraction3[axis] =(GPS_read[axis]- GPS_degree[axis]*10000000 ) / 10000; GPS_filter_sum[축] -=GPS_filter[축][GPS_filter_index]; GPS_filter[축][GPS_filter_index] =GPS_read[축] - (GPS_도[축]*10000000); GPS_filter_sum[축] +=GPS_filter[축][GPS_filter_index]; GPS_filtered[축] =GPS_filter_sum[축] / GPS_FILTER_VECTOR_LENGTH + (GPS_도[축]*10000000); if ( NAV_state ==NAV_STATE_HOLD_INFINIT || NAV_state ==NAV_STATE_HOLD_TIMED) { //우리는 poshold 모드에서만 gps 평균을 사용합니다... if ( fraction3[axis]>1 &&fraction3[axis]<999 ) GPS_coord[axis] =GPS_filtered[ 중심선]; } } } //dTnav 계산 //x,y 속도 및 탐색 pid를 계산하는 시간 dTnav =(float)(millis() - nav_loopTimer)/ 1000.0; nav_loopTimer =밀리(); // 잘못된 GPS로 인한 런업 방지 dTnav =min(dTnav, 1.0); //gui 및 기타 항목에 대한 거리 및 방위를 지속적으로 계산 - 집에서 헬리콥터까지 GPS_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dir); GPS_distance_cm(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dist); GPS_distanceToHome =dist/100; GPS_directionToHome =dir/100; if (!f.GPS_FIX_HOME) { //홈 설정이 없으면 아무 것도 표시하지 않습니다. GPS_distanceToHome =0; GPS_directionToHome =0; } //펜스 설정을 확인하고 필요한 경우 RTH를 실행 //TODO:자동 착륙 if ((GPS_conf.fence> 0) &&(GPS_conf.fence 5000) NAV_state =NAV_STATE_LAND_START_DESCENT; 부서지다; 경우 NAV_STATE_LAND_START_DESCENT:GPS_calc_poshold(); // 착지 위치 유지 f.THROTTLE_IGNORED =1; //스로트 스틱 입력 무시 f.GPS_BARO_MODE =1; //BARO 모드를 제어합니다. land_detect =0; //토지 감지기 재설정 f.LAND_COMPLETED =0; f.LAND_IN_PROGRESS =1; // 랜드 프로세스 플래그 NAV_state =NAV_STATE_LAND_IN_PROGRESS; 부서지다; 경우 NAV_STATE_LAND_IN_PROGRESS:GPS_calc_poshold(); check_land(); // 토지 탐지기 호출 if (f.LAND_COMPLETED) { nav_timer_stop =millis() + 5000; NAV_state =NAV_STATE_LANDED; } 부서지다; case NAV_STATE_LANDED:// 땅이 감지된 후 THROTTLE 스틱이 최소이거나 5초가 지난 경우 해제합니다. if (rcData[THROTTLE] 0) { //0에 도달하지 않은 경우 점프를 수행합니다. next_step =Mission_step.parameter1; NAV_상태 =NAV_STATE_PROCESS_NEXT; jump_times--; } 부서지다; case NAV_STATE_PROCESS_NEXT://다음 미션 단계 처리 NAV_error =NAV_ERROR_NONE; if (!recallWP(next_step)) { abort_mission(NAV_ERROR_WP_CRC); } else { switch(mission_step.action) { // Waypoiny 및 Hold 명령은 모두 항로 상태로 시작하며 LAND 명령도 포함합니다. case MISSION_WAYPOINT:case MISSION_HOLD_TIME:case MISSION_HOLD_UNLIM:case MISSION_LAND:set_new_altitude(mission_step.altitude); GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_prev[LAT], &GPS_prev[LON]); if ((wp_distance/100)>=GPS_conf.safe_wp_distance) abort_mission(NAV_ERROR_TOOFAR); 그렇지 않으면 NAV_state =NAV_STATE_WP_ENROUTE; GPS_prev[LAT] =Mission_step.pos[LAT]; //정확한 경로 계산을 위해 wp 좌표 저장 GPS_prev[LON] =Mission_step.pos[LON]; 부서지다; 경우 MISSION_RTH:f.GPS_head_set =0; if (GPS_conf.rth_altitude ==0 &&Mission_step.altitude ==0) //구성 및 Mission_step alt가 0이면 set_new_altitude(alt.EstAlt); // RTH는 실제 고도에서 반환 else { uint32_t rth_alt; if (mission_step.altitude ==0) rth_alt =GPS_conf.rth_altitude * 100; //미션 스텝의 고도가 우선합니다. else rth_alt =Mission_step.altitude; if (alt.EstAlt 0 &&mission_step.parameter1 GPS_conf.nav_max_altitude*100) _new_alt =GPS_conf.nav_max_altitude * 100; if(_new_alt ==alt.EstAlt){ force_new_altitude(_new_alt); 반품; } // 현재 위치의 고도에서 시작하여 점차적으로 alt를 변경합니다. alt_to_hold =alt.EstAlt; // 델타 시간 계산용 alt_change_timer =millis(); // 목표 고도를 저장합니다 target_altitude =_new_alt; // 고도 적분기 재설정 alt_change =0; // 원래 고도를 저장합니다 original_altitude =alt.EstAlt; // 목표 고도에 도달했는지 판단하기 위해 if(target_altitude> original_altitude){ // 아래에 있고 위로 올라갑니다 alt_change_flag =ASCENDING; } else if(target_altitude =target_altitude) alt_change_flag =REACHED_ALT; // 목표물을 지나서 명령하면 안 됩니다. if(alt_to_hold>=target_altitude) return target_altitude; } else if (alt_change_flag ==DESCENDING) { // 위, 아래로 if(alt.EstAlt <=target_altitude) alt_change_flag =REACHED_ALT; // 목표물을 지나서 명령하면 안 됩니다. if(alt_to_hold <=target_altitude) return target_altitude; } // 목표 고도에 도달했다면 목표 alt를 반환합니다. if(alt_change_flag ==REACHED_ALT) return target_altitude; int32_t diff =abs(alt_to_hold - target_altitude); // scale은 경과 시간에서 원하는 비율을 생성하는 방법입니다. // scale이 작을수록 더 빠른 비율을 의미합니다. int8_t _scale =4; if (alt_to_hold > 4 =64cm/s descent 기본적으로 int32_t change =(millis() - alt_change_timer)>> _규모; if(alt_change_flag ==오름차순){ alt_change +=변경; } else { alt_change -=변경; } // 델타 시간 생성용 alt_change_timer =millis(); 반환 original_altitude + alt_change;}////////////////////////////////////////////// ///////////////////////////////////////////PID 기반 GPS 탐색 기능//저자 :EOSBandi//Arducopter 팀의 코드 및 아이디어 기반:Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen//Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni//원래 제약 조건 변수에서는 작동하지 않습니다.int16_t constrain_int16(int16_t amt, int16_t low, int16_t high) { return ((amt)<(low)?(low):((amt)>(high)?(high):(amt))); }///////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////// 우리가 poles// 웨이포인트 설정당 한 번 계산해도 괜찮습니다. multicopter//void GPS_calc_longitude_scaling(int32_t lat) { GPS_scaleLonDown =cos(lat * 1.0e-7f * 0.01745329251f);} ////////////////////////////////////////////////////// /////////////////////////// ////////// 탐색, 필요한 변수 재설정 및 초기 값 계산을 위한 웨이포인트 설정//void GPS_set_next_wp(int32_t* lat_to, int32_t* lon_to, int32_t* lat_from, int32_t* lon_from) { GPS_WP[LAT] =*lat_to; GPS_WP[LON] =*lon_to; GPS_FROM[LAT] =*lat_from; GPS_FROM[LON] =*lon_from; GPS_calc_longitude_scaling(*lat_to); GPS_bearing(&GPS_FROM[LAT],&GPS_FROM[LON],&GPS_WP[LAT],&GPS_WP[LON],&target_bearing); GPS_distance_cm(&GPS_FROM[LAT],&GPS_FROM[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance); GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_FROM[LAT],&GPS_FROM[LON]); waypoint_speed_gov =GPS_conf.nav_speed_min; original_target_bearing =target_bearing;}////////////////////////////////////////////// //////////////////////////////////////// 목적지를 놓쳤는지 확인// 정적 부울 check_missed_wp(무효) { int32_t 임시; 임시 =target_bearing - original_target_bearing; 온도 =랩_18000(온도); 반환(abs(temp)> 10000); // 웨이포인트를 100도 통과}//////////////////////////////////////// /////////////////////////////////////////////// 두 사이의 거리 가져오기 cm 단위의 포인트// pos1에서 pos2까지 방위를 가져오고 1deg =100을 반환합니다. int32_t off_y =(*lat2 - *lat1) / GPS_scaleLonDown; *베어링 =9000 + atan2(-off_y, off_x) * 5729.57795f; //출력 레디안을 100xdeg로 변환 if (*bearing <0) *bearing +=36000;}void GPS_distance_cm(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2,uint32_t float)(*lat2 - *lat1); // 위도의 1/10 000 000도 차이 float dLon =(float)(*lon2 - *lon1) * GPS_scaleLonDown; //x *dist =sqrt(sq(dLat) + sq(dLon)) * 1.11318845f;}//***************************** ************************************************** ****************************// calc_velocity_and_filtered_position - GPS 위치에서 계산된 경도 및 위도 방향의 속도 // 및 가속도계 데이터// lon_speed cm/s로 표시됩니다. 양수는 cm/s로 표시되는 동쪽// 위도 속도 이동을 의미합니다. 북쪽으로 이동할 때 양수// 참고:GPS 위치를 사용하여 속도를 묻는 대신 GPS 위치를 직접 사용합니다. // 이것이 1.5m/s 미만에서 더 정확하기 때문입니다.// 참고:위치가 리드 필터를 사용하여 투영되더라도 속도는 변경되지 않은 GPS 위치에서 // 계산됩니다. 우리는 속도에 영향을 미치는 리드 필터의 노이즈를 원하지 않습니다.//**************************************** ************************************************** ****************정적 무효 GPS_calc_velocity(무효){ 정적 int16_t speed_old[2] ={0,0}; 정적 int32_t 마지막[2] ={0,0}; 정적 uint8_t 초기화 =0; if (초기화) { float tmp =1.0/dTnav; 실제 속도[_X] =(float)(GPS_coord[LON] - 마지막[LON]) * GPS_scaleLonDown * tmp; 실제 속도[_Y] =(float)(GPS_coord[LAT] - 마지막[LAT]) * tmp; //TODO:가능한 GPS 신호 저하에 대한 비현실적인 속도 변경 및 신호 탐색 확인 if (!GPS_conf.lead_filter) { actual_speed[_X] =(actual_speed[_X] + speed_old[_X]) / 2; 실제 속도[_Y] =(실제 속도[_Y] + 이전 속도[_Y]) / 2; speed_old[_X] =실제_속도[_X]; speed_old[_Y] =실제_속도[_Y]; } } 초기화=1; 마지막[LON] =GPS_coord[LON]; 마지막[LAT] =GPS_coord[LAT]; if (GPS_conf.lead_filter) { GPS_coord_lead[LON] =xLeadFilter.get_position(GPS_coord[LON], real_speed[_X], GPS_LAG); GPS_coord_lead[LAT] =yLeadFilter.get_position(GPS_coord[LAT], 실제_속도[_Y], GPS_LAG); }}/////////////////////////////////////////////////// ////////////////////////////////////// 두 GPS 좌표 사이의 위치 오차 계산// 왜냐하면 우리는 위도와 경도를 사용하여 거리 오류를 수행하고 있습니다. 여기에 간단한 차트가 있습니다. // 100 =1m// 1000 =11m =36피트// 1800 =19.80m =60피트// 3000 =33m// 10000 =111m//정적 무효 GPS_calc_location_error( int32_t* target_lat, int32_t* target_lng, int32_t* gps_lat, int32_t* gps_lng ) { error[LON] =(float)(*target_lng - *gps_lng) * GPS_scaleLonDown; // X 오류 오류[LAT] =*target_lat - *gps_lat; // Y 오류}//////////////////////////////////////////////// //////////////////////////////////////// x 및 y에서 nav_lat 및 nav_lon 계산 오류 및 속도//// TODO:더 빠른 poshold lockstatic void GPS_calc_poshold(void) { int32_t d; int32_t target_speed; uint8_t 축; for (axis=0;axis<2;axis++) { target_speed =get_P(오류[축], &posholdPID_PARAM); // 위도/경도 오차에서 원하는 속도 계산 target_speed =constrain(target_speed,-100,100); // poshold 모드에서 목표 속도를 1m/s로 제한하면 폭주를 방지하는 데 도움이 됩니다. rate_error[axis] =target_speed - actual_speed[axis]; // 속도 오차 계산 nav[axis] =get_P(rate_error[axis], &poshold_ratePID_PARAM) +get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d =get_D(오류[축], &dTnav, &poshold_ratePID[축], &poshold_ratePID_PARAM); d =제약(d, -2000, 2000); // 노이즈 제거 if(abs(actual_speed[axis]) <50) d =0; 탐색[축] +=d; // nav[축] =제약(nav[축], -NAV_BANK_MAX, NAV_BANK_MAX); nav[축] =constrain_int16(nav[축], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID[축].integrator =poshold_ratePID[축].integrator; }}/////////////////////////////////////////////////// ///////////////////////////////////// RTH와 같은 장거리 비행에 대해 원하는 nav_lat 및 nav_lon 계산 및 WP//정적 무효 GPS_calc_nav_rate(uint16_t max_speed) { float trig[2]; int32_t target_speed[2]; int32_t 기울기; uint8_t 축; GPS_update_crosstrack(); int16_t cross_speed =crosstrack_error * (GPS_conf.crosstrack_gain / 100.0); // 괜찮은지 확인? cross_speed =제약(cross_speed,-200,200); cross_speed =-교차 속도; 부동 온도 =(9000l - target_bearing) * RADX100; trig[_X] =코스(온도); 삼각[_Y] =죄(온도); target_speed[_X] =max_speed * trig[_X] - 교차 속도 * trig[_Y]; target_speed[_Y] =cross_speed * trig[_X] + max_speed * trig[_Y]; for (axis=0;axis<2;axis++) { rate_error[축] =target_speed[축] - 실제 속도[축]; rate_error[축] =제약(rate_error[축],-1000,1000); nav[축] =get_P(rate_error[축], &navPID_PARAM) +get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +get_D(rate_error[축], &dTnav, &navPID[축], &navPID_PA // nav[축] =제약(nav[축],-NAV_BANK_MAX,NAV_BANK_MAX); nav[축] =constrain_int16(nav[축], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID[축].integrator =navPID[축].integrator; }}static void GPS_update_crosstrack(void) { // Crosstrack Error // ---------------- // 너무 멀거나 너무 가까우면 float 추적을 하지 않습니다. temp =(target_bearing - original_target_bearing) * RADX100; crosstrack_error =sin(temp) * wp_distance; // 미터가 트랙 라인에서 벗어났습니다.}////////////////////////////////////////// ////////////////////////////////////////////// 탐색할 때 원하는 속도 결정 웨이포인트를 향하여 느린 // 탐색을 시작할 때 속도 증가도 구현합니다. //// | waypoint_speed_gov){ waypoint_speed_gov +=(int)(100.0 * dTnav); // increase at .5/ms max_speed =waypoint_speed_gov; } return max_speed;}////////////////////////////////////////////////////////////////////////////////////// Utilities//int32_t wrap_36000(int32_t ang) { if (ang> 36000) ang -=36000; if (ang <0) ang +=36000; return ang;}/* * EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution * with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased * resolution also increased precision of nav calculations*/#define DIGIT_TO_VAL(_x) (_x - '0')uint32_t GPS_coord_to_degrees(char* s) { char *p, *q; uint8_t deg =0, min =0; unsigned int frac_min =0; uint8_t i; // scan for decimal point or end of field for (p =s; isdigit(*p); p++); q =s; // convert degrees while ((p - q)> 2) { if (deg) deg *=10; deg +=DIGIT_TO_VAL(*q++); } // convert minutes while (p>
q) { if (min) min *=10; min +=DIGIT_TO_VAL(*q++); } // convert fractional minutes // expect up to four digits, result is in // ten-thousandths of a minute if (*p =='.') { q =p + 1; for (i =0; i <4; i++) { frac_min *=10; if (isdigit(*q)) frac_min +=*q++ - '0'; } } return deg * 10000000UL + (min * 1000000UL + frac_min*100UL) / 6;}// helper functions uint16_t grab_fields(char* src, uint8_t mult) { // convert string to uint16 uint8_t i; uint16_t tmp =0; for(i=0; src[i]!=0; i++) { if(src[i] =='.') { i++; if(mult==0) break; else src[i+mult] =0; } tmp *=10; if(src[i]>='0' &&src[i] <='9') tmp +=src[i]-'0'; } return tmp;}uint8_t hex_c(uint8_t n) { // convert '0'..'9','A'..'F' to 0..15 n -='0'; if(n>9) n -=7; n &=0x0F; return n;} //************************************************************************// Common GPS functions //void init_RTH() { f.GPS_mode =GPS_MODE_RTH; // Set GPS_mode to RTH f.GPS_BARO_MODE =true; GPS_hold[LAT] =GPS_coord[LAT]; //All RTH starts with a poshold GPS_hold[LON] =GPS_coord[LON]; //This allows to raise to rth altitude GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON], &GPS_hold[LAT], &GPS_hold[LON]); NAV_paused_at =0; if (GPS_conf.rth_altitude ==0) set_new_altitude(alt.EstAlt); //Return at actual altitude else { // RTH altitude is defined, but we use it only if we are below it if (alt.EstAlt =5) { GPS_home[LAT] =GPS_coord[LAT]; GPS_home[LON] =GPS_coord[LON]; GPS_calc_longitude_scaling(GPS_coord[LAT]); //need an initial value for distance and bearing calc nav_takeoff_bearing =att.heading; //save takeoff heading //TODO:Set ground altitude f.GPS_FIX_HOME =1; }}//reset navigation (stop the navigation processor, and clear nav)void GPS_reset_nav(void) { uint8_t i; for(i=0;i<2;i++) { nav[i] =0; reset_PID(&posholdPID[i]); reset_PID(&poshold_ratePID[i]); reset_PID(&navPID[i]); NAV_state =NAV_STATE_NONE; //invalidate JUMP counter jump_times =-10; //reset next step counter next_step =1; //Clear poi GPS_poi[LAT] =0; GPS_poi[LON] =0; f.GPS_head_set =0; }}//Get the relevant P I D values and set the PID controllers void GPS_set_pids(void) { posholdPID_PARAM.kP =(float)conf.pid[PIDPOS].P8/100.0; posholdPID_PARAM.kI =(float)conf.pid[PIDPOS].I8/100.0; posholdPID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; poshold_ratePID_PARAM.kP =(float)conf.pid[PIDPOSR].P8/10.0; poshold_ratePID_PARAM.kI =(float)conf.pid[PIDPOSR].I8/100.0; poshold_ratePID_PARAM.kD =(float)conf.pid[PIDPOSR].D8/1000.0; poshold_ratePID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; navPID_PARAM.kP =(float)conf.pid[PIDNAVR].P8/10.0; navPID_PARAM.kI =(float)conf.pid[PIDNAVR].I8/100.0; navPID_PARAM.kD =(float)conf.pid[PIDNAVR].D8/1000.0; navPID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; }//It was moved here since even i2cgps code needs itint32_t wrap_18000(int32_t ang) { if (ang> 18000) ang -=36000; if (ang <-18000) ang +=36000; return ang;}/**************************************************************************************//**************************************************************************************/...This file has been truncated, please download it to see its full contents.
회로도
