Arduino Quadcopter

Code

• MultiWii.ino

MultiWii.inoC/C++

#include "Arduino.h"#include "config.h"#include "def.h"#include "types.h"#include "GPS.h"#include "Serial.h"#include "Sensors. h"#include "MultiWii.h"#include "EEPROM.h"#include #if GPS//Functie-prototypes voor andere GPS-functies//Deze zouden misschien naar het gps.h-bestand kunnen gaan, maar dit zijn lokaal naar de 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* lat2, int232 ,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);static uintt* target_lng, int32_t* gps_lat, int32_t* gps_lng );static void GPS_calc_poshold(void);static uinttool );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:herschrijven naar normale C in plaats van C++// GPS lag instellen#if gedefinieerd(UBLOX) || gedefinieerd (MTK_BINARY19)#define GPS_LAG 0.5f //UBLOX GPS heeft een kleinere vertraging dan MTK en andere#else#define GPS_LAG 1.0f //We nemen aan dat MTK GPS een vertraging van 1 sec heeft#endif static int32_t GPS_coord_lead[2]; // Lead gefilterde gps-coördinatenklasse LeadFilter {public:LeadFilter () :_last_velocity (0) {} // min en max radiowaarden instellen in CLI int32_t get_position (int32_t pos, int16_t vel, float lag_in_seconds =1.0); void clear() { _last_velocity =0; }private: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; // winkel snelheid voor volgende iteratie _last_velocity =vel; return pos + vel_contribution + accel_contribution;}LeadFilter xLeadFilter; // Lange GPS-vertragingsfilter LeadFilter yLeadFilter; // Lat GPS lag filter typedef struct PID_PARAM_ { float kP; vlotter kI; vlotter kD; vlotter Imax; } PID_PARAM; PID_PARAM posholdPID_PARAM;PID_PARAM poshold_ratePID_PARAM;PID_PARAM navPID_PARAM;typedef struct PID_ { float integrator; // integratorwaarde int32_t last_input; // laatste invoer voor afgeleide float lastderivative; // laatste afgeleide voor laagdoorlaatfilter vlotteruitgang; float afgeleide;} PID;PID posholdPID[2];PID poshold_ratePID[2];PID navPID[2];int32_t get_P(int32_t fout, struct PID_PARAM_* pid) {return (float) error * pid->kP;}int32_t get_I (int32_t error, float* dt, struct PID_* pid, struct PID_PARAM_* pid_param) {pid->integrator +=((float) error * pid_param->kI) * *dt; pid->integrator =constrain(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 in milliseconden pid->derivative =(input - pid->last_input) / *dt; /// Laagdoorlaatfilter-afsnijfrequentie voor afgeleide berekening. vlotterfilter =7.9577e-3; // Stel in op "1 / (2 * PI * f_cut)"; // Voorbeelden voor _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 // discrete laagdoorlaatfilter, verwijdert de // hoogfrequente ruis die de controller gek kan maken pid-> afgeleide =pid->laatsteafgeleide + (*dt / ( filter + *dt)) * (pid->afgeleide - pid->laatsteafgeleide); // update status pid->last_input =input; pid->laatstederivaat =pid->derivaat; // voeg de afgeleide component toe, retourneer 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; // Detecteer land (extern) statisch uint32_t land_settle_timer; uint8_t GPS_Frame; // er is een geldig GPS_Frame gedetecteerd en de gegevens zijn gereed voor nav-berekening, statische float dTnav; // Deltatijd in milliseconden voor navigatieberekeningen, bijgewerkt met elke goede GPS-readstatic int16_t actual_speed[2] ={0,0};static float GPS_scaleLonDown; // dit wordt gebruikt om de afnemende lengtegraad te compenseren als we naar de polen gaan // Het verschil tussen de gewenste reissnelheid en de werkelijke reissnelheid // bijgewerkt na GPS-uitlezing - 5-10hzstatic int16_t rate_error[2];static int32_t error[2];static int32_t GPS_WP[2]; //Momenteel gebruikte WPstatic int32_t GPS_FROM [2]; // het doorlaatbare waypoint voor nauwkeurig volgen van de trackint32_t target_bearing; // Dit is de hoek van de helikopter naar de "next_WP" locatie in graden * 100static int32_t original_target_bearing; // deg * 100, De originele hoek naar de next_WP toen de next_WP werd ingesteld, Wordt ook gebruikt om te controleren wanneer we een WPstatic int16_t crosstrack_error passeren; // De hoeveelheid hoekcorrectie toegepast op target_bearing om de helikopter terug te brengen op zijn optimale pathuint32_t wp_distance; // afstand tussen vliegtuig en next_WP in cmstatic uint16_t waypoint_speed_gov; // gebruikt om langzaam op te winden bij het starten van de navigatie;////////////////////////////////////// //////////////////////////////////////////////// voortschrijdend gemiddelde filtervariabelen//#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];static_int32_t GPSstatisch int32_t GPSstatisch int32_t GPSstatic int32_t GPSdegree; //de lat lon graad zonder decimalen (lat/10 000 000)static uint16_t fraction3[2];static int16_t nav_takeoff_bearing; // slaat de peiling op bij het opstijgen (1deg =1) die wordt gebruikt om te roteren naar de startrichting wanneer ze thuis aankomen // Hoofdnavigatieprocessor en statusengine // TODO:processtatussen toevoegen om de verwerkingslast te vergemakkelijken uint8_t GPS_Compute (void) { unsigned char axis; uint32_t dist; //temp variabele om dist op te slaan naar copter int32_t dir; //temp variabele om dir op te slaan naar copter static uint32_t nav_loopTimer; // controleer of we een geldig frame hebben, zo niet, keer dan onmiddellijk terug als (GPS_Frame ==0) retourneer 0; anders GPS_Frame =0; // controleer de startpositie en stel deze in als deze niet was ingesteld 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(); } //Pas voortschrijdend gemiddelde filter toe op GPS-gegevens als (GPS_conf.filtering) { GPS_filter_index =(GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH; for (axis =0; axis<2; axis++) { GPS_read[axis] =GPS_coord[axis]; //laatste ongefilterde gegevens zijn in GPS_latitude en GPS_longitude GPS_degree [axis] =GPS_read [axis] / 10000000; // haal de graad om er zeker van te zijn dat de som past bij de int32_t // Hoe dicht zijn we bij een graadlijn ? het zijn de eerste drie cijfers van de breuken van graden // later gebruiken we het om te controleren of we dicht bij een gradenlijn zijn, zo ja, middeling uitschakelen, fraction3[axis] =(GPS_read[axis]- GPS_degree[axis]*10000000 ) / 10000; GPS_filter_sum[axis] -=GPS_filter[axis][GPS_filter_index]; GPS_filter[axis][GPS_filter_index] =GPS_read[axis] - (GPS_degree[axis]*10000000); GPS_filter_sum[axis] +=GPS_filter[axis][GPS_filter_index]; GPS_filtered[axis] =GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000); if (NAV_state ==NAV_STATE_HOLD_INFINIT || NAV_state ==NAV_STATE_HOLD_TIMED) {// we gebruiken gps-gemiddelden alleen in poshold-modus... if (fractie3[axis]>1 &&fraction3[axis]<999) GPS_coord[axis] =GPS_gefilterd[ as]; } } } //dTnav-berekening //Tijd voor het berekenen van x,y-snelheid en navigatie-pids dTnav =(float)(millis() - nav_loopTimer)/ 1000.0; nav_loopTimer =millis(); // voorkom aanloop van slechte GPS dTnav =min (dTnav, 1.0); // bereken continu afstand en peilingen voor gui en andere dingen - Van huis naar helikopter GPS_lager(&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) {//Als we geen home set hebben, laat dan niets zien GPS_distanceToHome =0; GPS_directionToHome =0; } //Controleer de afrasteringsinstelling en voer indien nodig RTH uit //TODO:autolanding if ((GPS_conf.fence> 0) &&(GPS_conf.fence  5000) NAV_state =NAV_STATE_LAND_START_DESCENT; pauze; geval NAV_STATE_LAND_START_DESCENT:GPS_calc_poshold(); //Land in positie houden f.THROTTLE_IGNORED =1; // Negeer Throtte stick-invoer f.GPS_BARO_MODE =1; // Neem de controle over de BARO-modus land_detect =0; // Reset landdetector f.LAND_COMPLETED =0; f.LAND_IN_PROGRESS =1; // Markeer landproces NAV_state =NAV_STATE_LAND_IN_PROGRESS; pauze; geval NAV_STATE_LAND_IN_PROGRESS:GPS_calc_poshold(); check_land(); //Bel landdetector als (f.LAND_COMPLETED) { nav_timer_stop =millis () + 5000; NAV_staat =NAV_STATE_LANDED; } pauze; case NAV_STATE_LANDED:// Ontwapenen als THROTTLE stick minimaal is of 5 sec voorbij nadat land is gedetecteerd als (rcData[THROTTLE] 0) {//als nul niet wordt bereikt, spring dan next_step =mission_step.parameter1; NAV_state =NAV_STATE_PROCESS_NEXT; jump_times--; } pauze; case NAV_STATE_PROCESS_NEXT:// Volgende missiestap wordt verwerkt NAV_error =NAV_ERROR_NONE; if (!recallWP (next_step)) { abort_mission (NAV_ERROR_WP_CRC); } else { switch (mission_step.action) { // Waypoiny en hold-commando's beginnen allemaal met een enroute-status, het bevat ook het LAND-commando 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); anders NAV_state =NAV_STATE_WP_ENROUTE; GPS_prev[LAT] =mission_step.pos[LAT]; // Bewaar wp-coördinaten voor nauwkeurige routeberekening GPS_prev[LON] =mission_step.pos[LON]; pauze; geval MISSION_RTH:f.GPS_head_set =0; if (GPS_conf.rth_altitude ==0 &&mission_step.altitude ==0) //if config en mission_step alt is nul set_new_altitude(alt.EstAlt); // RTH keert terug op de werkelijke hoogte else { uint32_t rth_alt; if (mission_step.altitude ==0) rth_alt =GPS_conf.rth_altitude * 100; // hoogte in missiestap heeft prioriteit 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); opbrengst; } // We beginnen op de huidige locatiehoogte en veranderen geleidelijk alt alt_to_hold =alt.EstAlt; // voor het berekenen van de delta-tijd alt_change_timer =millis (); // sla de doelhoogte op target_altitude =_new_alt; // reset onze hoogte-integrator alt_change =0; // sla de originele hoogte op original_altitude =alt.EstAlt; // om te beslissen of we de beoogde hoogte hebben bereikt if(target_altitude> original_altitude){ // we zijn beneden, gaan omhoog alt_change_flag =ASCENDING; } else if(target_altitude =target_altitude) alt_change_flag =REACHED_ALT; // we zouden niet voorbij ons doel moeten gaan if (alt_to_hold>=target_altitude) return target_altitude; } else if (alt_change_flag ==AFDALEN) { // we zijn boven, gaan naar beneden if (alt.EstAlt <=target_altitude) alt_change_flag =REACHED_ALT; // we zouden niet voorbij ons doel moeten gaan if (alt_to_hold <=target_altitude) return target_altitude; } // als we onze doelhoogte hebben bereikt, retourneer het doel alt if(alt_change_flag ==REACHED_ALT) retourneer target_altitude; int32_t diff =abs(alt_to_hold - target_altitude); // schaal is hoe we een gewenste snelheid genereren uit de verstreken tijd // een kleinere schaal betekent snellere tarieven int8_t _scale =4; if (alt_to_hold > 4 =64 cm/s afdaling standaard int32_t change =(millis() - alt_change_timer)>> _schaal; if(alt_change_flag ==ASCENDING){ alt_change +=change; } else { alt_change -=verandering; } // voor het genereren van delta-tijd alt_change_timer =millis (); return original_altitude + alt_change;}//////////////////////////////////////////// //////////////////////////////////////////PID-gebaseerde GPS-navigatiefuncties//Auteur :EOSBandi//Gebaseerd op code en ideeën van het Arducopter-team:Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen//Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni//oorspronkelijke beperking werkt niet met variabelenint16_t constrain_int16(int16_t amt, int16_t low, int16_t high) { return ((amt)<(low)?(low):((amt)>(high)?(high):(amt))); }///////////////////////////////////////////////// ///////////////////////////////////// dit wordt gebruikt om de afnemende lengtegraad te compenseren als we naar de poles// Het is oké om dit één keer per waypoint-instelling te berekenen, aangezien het een beetje verandert binnen het bereik van een multicopter//void GPS_calc_longitude_scaling(int32_t lat) { GPS_scaleLonDown =cos(lat * 1.0e-7f * 0.01745329251f);}/ ////////////////////////////////////////////////// ////////////////////////// ////////// Stelt het waypoint in om te navigeren, reset noodzakelijke variabelen en berekent initiële waarden//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; origineel_doel_lager =doel_lager;}///////////////////////////////////////////// ///////////////////////////////////////// Controleer of we de bestemming op de een of andere manier hebben gemist// statische bool check_missed_wp (void) { int32_t temp; temp =target_bearing - originele_target_bearing; temp =wrap_18000(temp); retour (abs(temp)> 10000); // we zijn het waypoint 100 graden gepasseerd}//////////////////////////////////////// ////////////////////////////////////////////// Haal afstand tussen twee punten in cm// Krijg peiling van pos1 naar pos2, retourneert een 1deg =100 precisievoid GPS_lager (int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* peiling) { int32_t off_x =*lon2 - *lon1; int32_t off_y =(*lat2 - *lat1) / GPS_scaleLonDown; *lager =9000 + atan2(-off_y, off_x) * 5729.57795f; // Converteer de output-redianen naar 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); // breedtegraadverschil in 1/10 000 000 graden float dLon =(float)(*lon2 - *lon1) * GPS_scaleLonDown; //x *dist =sqrt(sq(dLat) + sq(dLon)) * 1.11318845f;}//************************* ********************************************** ******************************// calc_velocity_and_filtered_position - snelheid in lon- en lat-richtingen berekend op basis van GPS-positie// en versnellingsmetergegevens// lon_speed uitgedrukt in cm/s. positieve getallen betekenen bewegen naar het oosten// lat_speed uitgedrukt in cm/s. positieve getallen bij verplaatsing naar het noorden// Opmerking:we gebruiken gps-locaties rechtstreeks om de snelheid te berekenen in plaats van gps om snelheid te vragen omdat// dit nauwkeuriger is onder 1,5 m/s// Opmerking:hoewel de posities worden geprojecteerd met behulp van een loodfilter, de snelheden worden berekend// uit de ongewijzigde gps-locaties. We willen niet dat ruis van ons leadfilter de snelheid beïnvloedt//**************************************** ********************************************** ******************** statische leegte GPS_calc_velocity (void) { static int16_t speed_old [2] ={0,0}; statisch int32_t last[2] ={0,0}; statisch uint8_t init =0; if (init) { float tmp =1.0/dTnav; actual_speed[_X] =(float)(GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp; actual_speed[_Y] =(float)(GPS_coord[LAT] - last[LAT]) * tmp; // TODO:controleer onrealistische snelheidsveranderingen en signaalnavigatie over mogelijke verslechtering van het gps-signaal als (!GPS_conf.lead_filter) {actual_speed[_X] =(actual_speed[_X] + speed_old[_X]) / 2; actual_speed[_Y] =(actual_speed[_Y] + speed_old[_Y]) / 2; speed_old[_X] =actuele_speed[_X]; speed_old[_Y] =actuele_speed[_Y]; } } init=1; last[LON] =GPS_coord[LON]; last[LAT] =GPS_coord[LAT]; if (GPS_conf.lead_filter) { GPS_coord_lead[LON] =xLeadFilter.get_position (GPS_coord[LON], actuele_snelheid [_X], GPS_LAG); GPS_coord_lead[LAT] =yLeadFilter.get_position(GPS_coord[LAT], actuele_snelheid[_Y], GPS_LAG); }}//////////////////////////////////////////////// ////////////////////////////////////// Bereken een locatiefout tussen twee gps-coördinaten// Omdat we gebruiken lat en lon om onze afstandsfouten uit te voeren, hier is een snelle grafiek:// 100 =1 m// 1000 =11 m =36 voet// 1800 =19,80 m =60 voet// 3000 =33 m// 10000 =111 m//statisch 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-foutfout [LAT] =*target_lat - *gps_lat; // Y-fout}///////////////////////////////////////////// ///////////////////////////////////////// Bereken nav_lat en nav_lon van de x en y fout en de snelheid //// TODO:controleer of de poshold-doelsnelheidsbeperking kan worden verhoogd voor een snellere poshold lockstatic void GPS_calc_poshold(void) { int32_t d; int32_t target_speed; uint8_t as; for (axis=0;axis<2;axis++) { target_speed =get_P(error[axis], &posholdPID_PARAM); // bereken de gewenste snelheid uit de lat/lon-fout target_speed =constrain(target_speed,-100,100); // Beperk de doelsnelheid in de poshold-modus tot 1 m/s om weglopers te voorkomen. rate_error[axis] =target_speed - actual_speed[axis]; // bereken de snelheidsfout nav [as] =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(fout[as], &dTnav, &poshold_ratePID[as], &poshold_ratePID_PARAM); d =beperking (d, -2000, 2000); // verwijder ruis if(abs(actual_speed[axis]) <50) d =0; nav[as] +=d; // nav [as] =beperking (nav [as], -NAV_BANK_MAX, NAV_BANK_MAX); nav[as] =constrain_int16(nav[as], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID[as].integrator =poshold_ratePID[as].integrator; }}//////////////////////////////////////////////// ////////////////////////////////////// Bereken de gewenste nav_lat en nav_lon voor afstandsvluchten zoals RTH en WP//static void GPS_calc_nav_rate (uint16_t max_speed) { float trig [2]; int32_t target_speed[2]; int32_t kantelen; uint8_t as; GPS_update_crosstrack(); int16_t cross_speed =crosstrack_error * (GPS_conf.crosstrack_gain / 100.0); //controleer is het goed? cross_speed =beperking (cross_speed, -200.200); cross_speed =-cross_speed; vlottertemperatuur =(9000l - target_bearing) * RADX100; trig[_X] =cos(temp); trig[_Y] =sin(temp); target_speed[_X] =max_speed * trig[_X] - cross_speed * trig[_Y]; target_speed[_Y] =cross_speed * trig[_X] + max_speed * trig[_Y]; for (axis=0;axis<2;axis++) {rate_error[axis] =target_speed[axis] - actual_speed[axis]; rate_error[as] =constrain(rate_error[as],-1000,1000); nav[axis] =get_P(rate_error[axis], &navPID_PARAM) +get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID[axis]; // nav [as] =beperking (nav [as], -NAV_BANK_MAX, NAV_BANK_MAX); nav[as] =constrain_int16(nav[as], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID[as].integrator =navPID[as].integrator; }}static void GPS_update_crosstrack(void) { // Crosstrack Error // ---------------- // Als we te ver weg of te dichtbij zijn, volgen we geen track volgende float temp =(target_bearing - original_target_bearing) * RADX100; crosstrack_error =sin(temp) * wp_distance; // Meter we zijn van de baan af}///////////////////////////////////////// ////////////////////////////////////////////// Bepaal de gewenste snelheid tijdens het navigeren richting een waypoint, voer ook langzame // speed rampup uit bij het starten van een navigatie//// | 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.

Schema's