Arduino Quadcopter

Kode

• MultiWii.ino

MultiWii.inoC/C++

#include "Arduino.h"#include "config.h"#include "def.h"#include "types.h"#include "GPS.h"#include "Serial.h"#include "Sensor.h" h"#include "MultiWii.h"#include "EEPROM.h"#include #if GPS//Fungsi prototipe untuk fungsi GPS lainnya//Ini mungkin bisa masuk ke file gps.h, namun ini lokal ke gps.cpp static void GPS_bearing(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* bearing);static void GPS_distance_cm(int32_t* lat1, int32_t* lon1, int32_t* latt ,uint32_t* dist);static void GPS_calc_velocity(void);static void GPS_calc_location_error( int32_t* target_lat, int32_t* target_lng, int32_t* gps_lat, int32_t* gps_lng );static void GPS_calc_poshold(void);statis kecepatan GPS_uint );static void GPS_calc_nav_rate(uint16_t max_speed);int32_t wrap_18000(int32_t ang);static bool check_missed_wp(void);void GPS_calc_longitude_scaling(int32_t lat);static void GPS_update_crosstrack(void);int32_t wra p_36000(int32_t ang);// Leadig filter - TODO:menulis ulang ke C normal alih-alih C++// Mengatur lag gps#if didefinisikan(UBLOX) || didefinisikan (MTK_BINARY19)#define GPS_LAG 0.5f //UBLOX GPS memiliki lag yang lebih kecil dari MTK dan lainnya#else#define GPS_LAG 1.0f //Kami berasumsi bahwa GPS MTK memiliki lag 1 detik#endif static int32_t GPS_coord_lead[2]; // Lead filtered gps coordinatsclass LeadFilter {public:LeadFilter() :_last_velocity(0) { } // setup nilai radio min dan max di CLI int32_t get_position(int32_t pos, int16_t vel, float lag_in_seconds =1.0); void clear() { _last_velocity =0; }pribadi: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 * lag_in_seconds; int16_t vel_contribution =vel * lag_in_seconds; // simpan kecepatan untuk iterasi berikutnya _last_velocity =vel; kembali pos + vel_contribution + accel_contribution;}LeadFilter xLeadFilter; // Filter kelambatan GPS yang panjang LeadFilter yLeadFilter; // Lat GPS lag filter typedef struct PID_PARAM_ { float kP; mengapung kI; mengapung kD; mengambang Imaks; } PID_PARAM; PID_PARAM posholdPID_PARAM;PID_PARAM poshold_ratePID_PARAM;PID_PARAM navPID_PARAM;typedef struct PID_ { float integrator; // nilai integrator int32_t last_input; // input terakhir untuk derivatif float turunan terakhir; // turunan terakhir untuk keluaran float filter lolos rendah; turunan 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 (kesalahan int32_t, float* dt, struct PID_* pid, struct PID_PARAM_* pid_param) { pid->integrator +=((float)error * pid_param->kI) * *dt; pid->integrator =kendala(pid->integrator,-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 dalam milidetik pid->derivative =(input - pid->last_input) / *dt; /// Frekuensi potong filter lolos rendah untuk perhitungan turunan. filter pelampung =7.9577e-3; // Atur ke "1 / ( 2 * PI * f_cut )"; // Contoh untuk _filter:// f_cut =10 Hz -> _filter =15.9155e-3 // f_cut =15 Hz -> _filter =10.6103e-3 // f_cut =20 Hz -> _filter =7.9577e-3 // f_cut =25 Hz -> _filter =6.3662e-3 // f_cut =30 Hz -> _filter =5.3052e-3 // filter low pass diskrit, memotong // noise frekuensi tinggi yang dapat membuat controller gila pid-> turunan =pid->turunan terakhir + (*dt / ( filter + *dt)) * (pid->turunan - pid->turunan terakhir); // perbarui status pid->last_input =input; pid->turunan terakhir =pid->turunan; // tambahkan komponen turunan return pid_param->kD * pid->derivative;}void reset_PID(struct PID_* pid) { pid->integrator =0; pid->input_terakhir =0; pid->lastderivative =0;}#define _X 1#define _Y 0#define RADX100 0,000174532925 uint8_t land_detect; //Mendeteksi tanah (ekstern)statis uint32_t land_settle_timer;uint8_t GPS_Frame; // GPS_Frame yang valid terdeteksi, dan data siap untuk komputasi nav float dTnav; // Waktu Delta dalam milidetik untuk perhitungan navigasi, diperbarui dengan setiap GPS readstatic baik int16_t actual_speed[2] ={0,0};static float GPS_scaleLonDown; // ini digunakan untuk mengimbangi penyusutan garis bujur saat kita menuju kutub// Perbedaan antara kecepatan perjalanan yang diinginkan dan kecepatan perjalanan sebenarnya// diperbarui setelah GPS dibaca - 5-10hzstatic int16_t rate_error[2];static int32_t error[2];static int32_t GPS_WP[2]; //Saat ini digunakan WPstatic int32_t GPS_FROM[2]; //titik jalan tembus untuk trek yang tepat mengikuti int32_t target_bearing; // Ini adalah sudut dari helikopter ke lokasi "next_WP" dalam derajat * 100static int32_t original_target_bearing; // deg * 100, Sudut asli ke next_WP saat next_WP disetel, Juga digunakan untuk memeriksa saat kita melewati WPstatic int16_t crosstrack_error; // Jumlah koreksi sudut yang diterapkan pada target_bearing untuk membawa helikopter kembali ke pathuint32_t wp_distance yang optimal; // jarak antara plane dan next_WP dalam cmstatic uint16_t waypoint_speed_gov; // digunakan untuk kecepatan lambat saat memulai navigasi;//////////////////////////////////// ////////////////////////////////////////////////////////////////// moving average variabel filter//#define GPS_FILTER_VECTOR_LENGTH 5static uint8_t GPS_filter_index =0;static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];static int32_t GPS_filter_sum[2];static int32_t GPS_read[2];statis int32_t GPS_filtered[32_t]; //derajat bujur lintang tanpa desimal (lat/10 000 000)static uint16_t fraction3[2];static int16_t nav_takeoff_bearing; // menyimpan bantalan saat takeof (1deg =1) digunakan untuk memutar ke arah lepas landas ketika tiba di rumah//Prosesor navigasi utama dan mesin negara// TODO:menambahkan status proses untuk meringankan beban pemrosesan uint8_t GPS_Compute(void) { unsigned char axis; uint32_t dist; //variabel temp untuk menyimpan dist ke copter int32_t dir; //variabel temp untuk menyimpan dir ke copter static uint32_t nav_loopTimer; //periksa apakah kita memiliki frame yang valid, jika tidak maka segera kembalikan if (GPS_Frame ==0) return 0; lain GPS_Frame =0; //periksa posisi awal dan atur jika tidak disetel 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(); } //Terapkan filter rata-rata bergerak ke data GPS jika (GPS_conf.filtering) { GPS_filter_index =(GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH; for (sumbu =0; sumbu<2; sumbu++) { GPS_read[sumbu] =GPS_coord[sumbu]; //data terbaru tanpa filter ada di GPS_latitude dan GPS_longitude GPS_degree[axis] =GPS_read[axis] / 10000000; // dapatkan derajat untuk memastikan jumlahnya sesuai dengan int32_t // Seberapa dekat kita dengan garis derajat ? itu tiga digit pertama dari pecahan derajat // nanti kita gunakan untuk Memeriksa apakah kita dekat dengan garis derajat, jika ya, nonaktifkan rata-rata, fraction3[axis] =(GPS_read[axis]- GPS_degree[axis]*10000000 ) / 10.000; GPS_filter_sum[sumbu] -=GPS_filter[sumbu][GPS_filter_index]; GPS_filter[sumbu][GPS_filter_index] =GPS_read[sumbu] - (GPS_degree[sumbu]*10000000); GPS_filter_sum[sumbu] +=GPS_filter[sumbu][GPS_filter_index]; GPS_filtered[sumbu] =GPS_filter_sum[sumbu] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[sumbu]*10000000); if ( NAV_state ==NAV_STATE_HOLD_INFINIT || NAV_state ==NAV_STATE_HOLD_TIMED) { //kita menggunakan rata-rata gps hanya dalam mode poshold... if ( fraction3[axis]>1 &&fraction3[axis]<999 ) GPS_coord[axis] =GPS_filtered[ sumbu]; } } } //kalkulasi dTnav //Waktu untuk menghitung kecepatan x,y dan navigasi pids dTnav =(float)(millis() - nav_loopTimer)/ 1000.0; nav_loopTimer =milis(); // mencegah runup dari GPS yang buruk dTnav =min(dTnav, 1.0); //menghitung jarak dan bantalan untuk gui dan hal-hal lain secara terus menerus - Dari rumah ke copter 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) { //Jika kita tidak memiliki home set, jangan tampilkan apapun GPS_distanceToHome =0; GPS_directionToHome =0; } //Periksa pengaturan pagar dan jalankan RTH jika perlu //TODO:autolanding if ((GPS_conf.fence> 0) &&(GPS_conf.fence  5000) NAV_state =NAV_STATE_LAND_START_DESCENT; merusak; kasus NAV_STATE_LAND_START_DESCENT:GPS_calc_poshold(); //Tanah dalam posisi tahan f.THROTTLE_IGNORED =1; //Abaikan masukan stik Throtte f.GPS_BARO_MODE =1; //Mengendalikan mode BARO land_detect =0; //Reset detektor tanah f.LAND_COMPLETED =0; f.LAND_IN_PROGRESS =1; // Tandai proses tanah NAV_state =NAV_STATE_LAND_IN_PROGRESS; merusak; kasus NAV_STATE_LAND_IN_PROGRESS:GPS_calc_poshold(); cek_land(); //Panggil detektor tanah jika (f.LAND_COMPLETED) { nav_timer_stop =milis() + 5000; NAV_state =NAV_STATE_LANDED; } merusak; case NAV_STATE_LANDED:// Melucuti senjata jika THROTTLE stick minimal atau 5 detik setelah tanah terdeteksi jika (rcData[THROTTLE] 0) { //jika nol tidak tercapai lakukan lompatan next_step =mission_step.parameter1; NAV_state =NAV_STATE_PROCESS_NEXT; jump_times--; } merusak; case NAV_STATE_PROCESS_NEXT://Memproses langkah misi selanjutnya NAV_error =NAV_ERROR_NONE; if (!recallWP(langkah_berikutnya)) { abort_mission(NAV_ERROR_WP_CRC); } else { switch(mission_step.action) { //Waypoiny dan tahan perintah semua dimulai dengan status enroute itu termasuk perintah LAND juga 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); lain NAV_state =NAV_STATE_WP_ENROUTE; GPS_prev[LAT] =mission_step.pos[LAT]; //Simpan koordinat wp untuk perhitungan rute yang tepat GPS_prev[LON] =mission_step.pos[LON]; merusak; kasus MISSION_RTH:f.GPS_head_set =0; if (GPS_conf.rth_altitude ==0 &&mission_step.altitude ==0) //jika config dan mission_step alt adalah nol set_new_altitude(alt.EstAlt); // RTH kembali pada ketinggian sebenarnya else { uint32_t rth_alt; if (mission_step.altitude ==0) rth_alt =GPS_conf.rth_altitude * 100; //ketinggian dalam langkah misi memiliki prioritas lain 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); kembali; } // Kita mulai dari ketinggian lokasi saat ini dan secara bertahap mengubah alt alt_to_hold =alt.EstAlt; // untuk menghitung waktu delta alt_change_timer =milis(); // simpan ketinggian target target_altitude =_new_alt; // reset integrator ketinggian alt_change =0; // simpan ketinggian asli original_altitude =alt.EstAlt; // untuk memutuskan apakah kita telah mencapai ketinggian target if(target_altitude> original_altitude){ // kita berada di bawah, naik alt_change_flag =ASCENDING; } else if(target_altitude =target_altitude) alt_change_flag =REACHED_ALT; // kita tidak boleh memerintahkan melewati target kita jika(alt_to_hold>=target_altitude) return target_altitude; } else if (alt_change_flag ==DESCENDING) { // kita di atas, turun if(alt.EstAlt <=target_altitude) alt_change_flag =REACHED_ALT; // kita tidak boleh memerintahkan melewati target kita jika(alt_to_hold <=target_altitude) mengembalikan target_altitude; } // jika kita telah mencapai ketinggian target kita, kembalikan alt target if(alt_change_flag ==REACHED_ALT) return target_altitude; int32_t diff =abs(alt_to_hold - target_altitude); // skala adalah bagaimana kita menghasilkan tingkat yang diinginkan dari waktu yang telah berlalu // skala yang lebih kecil berarti tingkat yang lebih cepat int8_t _scale =4; if (alt_to_hold > 4 =64cm/s penurunan secara default int32_t change =(millis() - alt_change_timer)>> _skala; if(alt_change_flag ==NAIK){ alt_change +=ubah; } else { alt_ubah -=ubah; } // untuk menghasilkan waktu delta alt_change_timer =milis(); kembalikan original_altitude + alt_change;}//////////////////////////////////////////////////////////// ////////////////////////////////////////Fungsi navigasi GPS berbasis PID // Penulis :EOSBandi//Berdasarkan kode dan ide dari tim Arducopter:Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen//Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni//original constraint tidak bekerja dengan variabelint16_t constrain_int16(int16_t amt, int16_t rendah, int16_t tinggi) { return ((amt)<(rendah)?(rendah):((amt)>(tinggi)?(tinggi):(amt))); }///////////////////////////////////////////////////////////////// //////////////////////////////////// ini digunakan untuk mengimbangi garis bujur yang menyusut saat kita menuju poles// Tidak apa-apa untuk menghitung ini sekali per pengaturan titik jalan, karena ini sedikit berubah dalam jangkauan multicopter//void GPS_calc_longitude_scaling(int32_t lat) { GPS_scaleLonDown =cos(lat * 1.0e-7f * 0.01745329251f);}/ ////////////////////////////////////////////////////////////////// //////////////////////////////////// ////////// Menyetel titik jalan untuk bernavigasi, mengatur ulang variabel yang diperlukan dan menghitung nilai awal//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_ke; 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;}/////////////////////////////////////////////////////////// //////////////////////////////////////// Periksa apakah kita melewatkan tujuan entah bagaimana// static bool check_missed_wp(void) { int32_t temp; temp =target_bearing - original_target_bearing; suhu =bungkus_18000(suhu); kembali (abs(temp)> 10.000); // kita melewati titik jalan sebesar 100 derajat}/////////////////////////////////////////////////// //////////////////////////////////////////// Dapatkan jarak antara dua poin dalam cm// Dapatkan bantalan dari pos1 ke pos2, mengembalikan 1deg =100 presisivoid GPS_bearing(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* bantalan) { int32_t off_x =*lon2 - *lon1; int32_t off_y =(*lat2 - *lat1) / GPS_scaleLonDown; *bantalan =9000 + atan2(-off_y, off_x) * 5729.57795f; //Konversikan output redian ke 100xdeg if (*bearing <0) *bearing +=36000;}void GPS_distance_cm(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2,uint32_t* dist) { float dLat =( float)(*lat2 - *lat1); // perbedaan lintang dalam 1/10 000 000 derajat float dLon =(float)(*lon2 - *lon1) * GPS_scaleLonDown; //x *dist =sqrt(sq(dLat) + sq(dLon)) * 1.11318845f;}//************************* ************************************************** ****************************// calc_velocity_and_filtered_position - kecepatan dalam arah bujur dan lintang dihitung dari posisi GPS// dan data akselerometer// kecepatan_lon dinyatakan dalam cm/s. angka positif berarti bergerak ke timur// lat_speed dinyatakan dalam cm/s. angka positif saat bergerak ke utara// Catatan:kami menggunakan lokasi gps secara langsung untuk menghitung kecepatan alih-alih meminta kecepatan gps karena// ini lebih akurat di bawah 1,5m/s// Catatan:meskipun posisi diproyeksikan menggunakan filter timah, kecepatan dihitung // dari lokasi gps yang tidak berubah. Kami tidak ingin kebisingan dari filter timah kami memengaruhi kecepatan//************************************ ************************************************** ****************static void GPS_calc_velocity(void){ static int16_t speed_old[2] ={0,0}; static int32_t last[2] ={0,0}; uint8_t init statis =0; if (init) { float tmp =1.0/dTnav; aktual_speed[_X] =(float)(GPS_coord[LON] - terakhir[LON]) * GPS_scaleLonDown * tmp; aktual_kecepatan[_Y] =(mengambang)(GPS_coord[LAT] - terakhir[LAT]) * tmp; //TODO:Periksa perubahan kecepatan yang tidak realistis dan navigasi sinyal tentang kemungkinan penurunan sinyal gps if (!GPS_conf.lead_filter) { actual_speed[_X] =(actual_speed[_X] + speed_old[_X]) / 2; aktual_kecepatan[_Y] =(kecepatan_aktual[_Y] + kecepatan_lama[_Y]) / 2; speed_old[_X] =aktual_speed[_X]; speed_old[_Y] =aktual_speed[_Y]; } } init=1; terakhir[LON] =GPS_coord[LON]; terakhir[LAT] =GPS_coord[LAT]; if (GPS_conf.lead_filter) { GPS_coord_lead[LON] =xLeadFilter.get_position(GPS_coord[LON], actual_speed[_X], GPS_LAG); GPS_coord_lead[LAT] =yLeadFilter.get_position(GPS_coord[LAT], kecepatan_aktual[_Y], GPS_LAG); }}//////////////////////////////////////////////////////////////// ///////////////////////////////////// Menghitung kesalahan lokasi antara dua koordinat gps // Karena kita menggunakan lintang dan bujur untuk melakukan kesalahan jarak kami, berikut adalah bagan cepat:// 100 =1m// 1000 =11m =36 kaki// 1800 =19,80m =60 kaki// 3000 =33m// 10000 =111m//statis void 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 Kesalahan kesalahan[LAT] =*target_lat - *gps_lat; // Kesalahan Y}////////////////////////////////////////////////////////////// /////////////////////////////////////// Hitung nav_lat dan nav_lon dari x dan y kesalahan dan kecepatan //// TODO:periksa apakah batasan kecepatan target poshold dapat ditingkatkan untuk snappier poshold lockstatic void GPS_calc_poshold(void) { int32_t d; int32_t target_speed; sumbu uint8_t; for (sumbu=0;sumbu<2;sumbu++) { target_speed =get_P(error[sumbu], &posholdPID_PARAM); // menghitung kecepatan yang diinginkan dari lat/lon error target_speed =constrain(target_speed,-100.100); // Batasi kecepatan target dalam mode poshold menjadi 1m/s. Ini membantu menghindari pelarian.. rate_error[axis] =target_speed - actual_speed[axis]; // hitung kecepatan error 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(error[sumbu], &dTnav, &poshold_ratePID[sumbu], &poshold_ratePID_PARAM); d =kendala(d, -2000, 2000); // hilangkan noise if(abs(actual_speed[axis]) <50) d =0; nav[sumbu] +=d; // nav[sumbu] =kendala(nav[sumbu], -NAV_BANK_MAX, NAV_BANK_MAX); nav[sumbu] =constrain_int16(nav[sumbu], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID[sumbu].integrator =poshold_ratePID[sumbu].integrator; }}//////////////////////////////////////////////////////////////// //////////////////////////////////// Hitung nav_lat dan nav_lon yang diinginkan untuk terbang jarak jauh seperti RTH dan WP//static void GPS_calc_nav_rate( uint16_t max_speed) { float trig[2]; int32_t target_speed[2]; kemiringan int32_t; sumbu uint8_t; GPS_update_crosstrack(); int16_t cross_speed =crosstrack_error * (GPS_conf.crosstrack_gain / 100.0); //periksa apakah baik-baik saja? cross_speed =kendala(cross_speed,-200.200); lintas_kecepatan =-lintas_kecepatan; float temp =(9000l - target_bearing) * RADX100; trigonometri[_X] =cos(temp); trigonometri[_Y] =sin(temp); target_speed[_X] =max_speed * trigonometri[_X] - cross_speed * trigonometri[_Y]; target_speed[_Y] =cross_speed * trigonometri[_X] + max_speed * trigonometri[_Y]; for (sumbu=0;sumbu<2;sumbu++) { rate_error[sumbu] =target_speed[sumbu] - aktual_kecepatan[sumbu]; rate_error[axis] =kendala(rate_error[axis],-1000,1000); nav[sumbu] =get_P(rate_error[axis], &navPID_PARAM) +get_I(rate_error[axis], &dTnav, &navPID[sumbu], &navPID_PARAM) +get_D(rate_error[sumbu], &dTnav, &navPID[sumbu], &navPID_PARAM); // nav[sumbu] =kendala(nav[sumbu],-NAV_BANK_MAX,NAV_BANK_MAX); nav[sumbu] =constrain_int16(nav[sumbu], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID[sumbu].integrator =navPID[sumbu].integrator; }}static void GPS_update_crosstrack(void) { // Crosstrack Error // ---------------- // Jika terlalu jauh atau terlalu dekat kita tidak melakukan track following float temp =(target_bearing - original_target_bearing) * RADX100; crosstrack_error =sin(temp) * wp_distance; // Meter kita keluar jalur}//////////////////////////////////////////////////// /////////////////////////////////////////// Tentukan kecepatan yang diinginkan saat bernavigasi menuju titik jalan, terapkan juga lambat // peningkatan kecepatan saat memulai navigasi//// | 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.

Skema