Quadricoptère Arduino

À propos de ce projet

Ce n'est pas seulement un quadricoptère... c'est une machine open source !

Maintenant, les questions viennent, où et comment puis-je obtenir le code du quadricoptère ? La réponse est donc Multiwii.

MultiWii est un logiciel de contrôleur de vol très populaire pour les multi-rotors DIY avec une grande communauté. Il prend en charge divers multicoptères dotés de fonctionnalités avancées telles que le contrôle Bluetooth par votre smartphone, l'écran OLED, le baromètre, le magnétomètre, le maintien de la position GPS et le retour à la maison, les bandes LED et bien d'autres. Alors construisons notre contrôleur de vol en utilisant Arduino !

Étape 1 :Conception du contrôleur de vol

Voici les schémas de la carte contrôleur de vol. vous pouvez en fabriquer un sur votre PCB à usage général ou commander un PCB auprès du fabricant comme je l'ai fait.

Connexions ESC

• D3 <

• D9 <

• D10 <

• D11 <

Connexions du module Bluetooth

• TX <

• RX <

Connexions MPU-6050

• A4 <

• A5 <

Indicateur LED

• D8 <

Connexions du récepteur

• D2 <

• D4 <<Élérons

• D5 <

• D6 <

• D7 <

Étape 2 : Construire un cadre

J'ai acheté un cadre DJI 450 et j'ai attaché mes moteurs et tout ce qu'il contient. Vous pouvez voir la vidéo sur la façon dont je l'ai fait.

Étape 3 :Fixation du contrôleur de vol sur le cadre

Ensuite, fixez enfin l'esc et le récepteur sur la carte comme indiqué sur les schémas et tout est fait !

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 "Capteurs. h"#include "MultiWii.h"#include "EEPROM.h"#include #if GPS//Prototypes de fonctions pour d'autres fonctions GPS//Ceux-ci pourraient peut-être aller dans le fichier gps.h, mais ce sont local au vide statique gps.cpp GPS_bearing(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* relèvement);static void GPS_distance_cm(int32_t* lat1, int32_t* lon1, int2, int32_t2* lat ,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_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);// Filtre Leadig - A FAIRE :réécrire en C normal au lieu de C++// Configurer le décalage gps#if défini(UBLOX) || défini (MTK_BINARY19)#define GPS_LAG 0.5f //UBLOX GPS a un décalage plus petit que MTK et other#else#define GPS_LAG 1.0f //Nous supposons que MTK GPS a un décalage de 1 sec#endif static int32_t GPS_coord_lead[2]; // Classe de coordonnées GPS filtrée par plomb LeadFilter {public:LeadFilter() :_last_velocity(0) { } // configuration des valeurs radio min et max dans la 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; // stocker la vitesse pour la prochaine itération _last_velocity =vel; return pos + vel_contribution + accel_contribution;}LeadFilter xLeadFilter; // Filtre de retard GPS long LeadFilter yLeadFilter; // Lat GPS lag filter typedef struct PID_PARAM_ { float kP; flotteur kI; flotteur kD; flotteur Imax; } PID_PARAM ; PID_PARAM posholdPID_PARAM;PID_PARAM poshold_ratePID_PARAM;PID_PARAM navPID_PARAM;typedef struct PID_ { float integrator; // valeur de l'intégrateur int32_t last_input; // dernière entrée pour la dérivée float lastderivative; // dernière dérivée pour la sortie flottante du filtre passe-bas ; dérivé flottant;} 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 (erreur int32_t, float* dt, struct PID_* pid, struct PID_PARAM_* pid_param) { pid->integrator +=((float)error * pid_param->kI) * *dt; pid->intégrateur =contrainte(pid->intégrateur,-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 en millisecondes pid->derivative =(input - pid->last_input) / *dt; /// Fréquence de coupure du filtre passe-bas pour le calcul de la dérivée. filtre à flotteur =7,9577e-3 ; // Définir sur "1 / ( 2 * PI * f_cut )" ; // Exemples pour _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 // filtre passe-bas discret, supprime le // bruit haute fréquence qui peut rendre le contrôleur fou pid-> dérivé =pid->derrivative + (*dt / ( filtre + *dt)) * (pid->dérivé - pid->lastderivative); // mettre à jour l'état pid->last_input =input; pid->derrivative =pid->dérivé ; // ajouter un composant dérivé 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; //Détecter la terre (externe)statique uint32_t land_settle_timer;uint8_t GPS_Frame; // un GPS_Frame valide a été détecté et les données sont prêtes pour le calcul de navigationstatic float dTnav; // Delta Time en millisecondes pour les calculs de navigation, mis à jour avec chaque bon GPS readstatic int16_t actual_speed[2] ={0,0};static float GPS_scaleLonDown; // ceci est utilisé pour compenser la diminution de la longitude à mesure que nous nous dirigeons vers les pôles// La différence entre le taux de déplacement souhaité et le taux de déplacement réel// mis à jour après la lecture GPS - 5-10hzstatic int16_t rate_error[2];static int32_t erreur[2];statique int32_t GPS_WP[2]; //Actuellement utilisé WPstatic int32_t GPS_FROM[2]; //le waypoint précédent pour un suivi précis de la trajectoire int32_t target_bearing ; // Il s'agit de l'angle entre l'hélicoptère et l'emplacement "next_WP" en degrés * 100static int32_t original_target_bearing; // deg * 100, L'angle d'origine par rapport au next_WP lorsque le next_WP a été défini, également utilisé pour vérifier lorsque nous passons un WPstatic int16_t crosstrack_error; // La quantité de correction d'angle appliquée à target_bearing pour ramener l'hélicoptère sur son pathuint32_t optimal wp_distance; // distance entre l'avion et next_WP en cmstatic uint16_t waypoint_speed_gov; // utilisé pour la vitesse lente au démarrage de la navigation ; //////////////////////////////////// ////////////////////////////////////////////// moyenne mobile variables de filtre//#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];filter_read[2];static int32_t GPS_filter_sum[2];static int32_t GPS_read[2];filterdegret_[2];static int32_t GPS_filter_sum[2];static int32_t GPS_read[2];; //le degré lat lon sans aucune décimale (lat/10 000 000)static uint16_t fraction3[2];static int16_t nav_takeoff_bearing; // enregistre le relèvement au décollage (1deg =1) utilisé pour pivoter dans la direction de décollage lorsqu'il arrive à la maison // Processeur de navigation principal et moteur d'état // TODO :ajouter des états de traitement pour alléger la charge de traitement uint8_t GPS_Compute (void) { axe de char non signé; uint32_t dist ; //variable temporaire pour stocker dist à copter int32_t dir ; // variable temporaire pour stocker le répertoire dans le copter statique uint32_t nav_loopTimer ; //vérifiez que nous avons un cadre valide, sinon retournez immédiatement si (GPS_Frame ==0) retournez 0; sinon GPS_Frame =0 ; //vérifier la position d'origine et la définir si elle n'a pas été définie 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(); } //Appliquer le filtre de moyenne mobile aux données GPS si (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]; //les dernières données non filtrées sont en GPS_latitude et GPS_longitude GPS_degree[axis] =GPS_read[axis] / 10000000 ; // obtient le degré pour s'assurer que la somme correspond à l'int32_t // À quelle distance sommes-nous d'une ligne de degré ? ce sont les trois premiers chiffres des fractions de degré // plus tard, nous l'utilisons pour vérifier si nous sommes proches d'une ligne de degré, si oui, désactiver la moyenne, 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) { // nous utilisons la moyenne GPS uniquement en mode poshold... if ( fraction3[axis]>1 &&fraction3[axis]<999 ) GPS_coord[axis] =GPS_filtered[ axe]; } } } //calcul dTnav //Temps de calcul de la vitesse x,y et des pids de navigation dTnav =(float)(millis() - nav_loopTimer)/ 1000.0; nav_loopTimer =millis(); // empêche le démarrage d'un mauvais GPS dTnav =min(dTnav, 1.0); //calculer la distance et les relèvements pour l'interface graphique et d'autres éléments en continu - De la maison à l'hélicoptère 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) { //Si nous n'avons pas de domicile défini, n'affiche rien GPS_distanceToHome =0; GPS_directionToHome =0; } //Vérifiez le réglage de la clôture et exécutez RTH si nécessaire //À FAIRE :atterrissage automatique si ((GPS_conf.fence> 0) &&(GPS_conf.fence  5000) NAV_state =NAV_STATE_LAND_START_DESCENT; Pause; cas NAV_STATE_LAND_START_DESCENT :GPS_calc_poshold(); // Atterrir en position hold f.THROTTLE_IGNORED =1; //Ignorer l'entrée du manche des gaz f.GPS_BARO_MODE =1; //Prendre le contrôle du mode BARO land_detect =0; //Réinitialiser le détecteur de terrain f.LAND_COMPLETED =0; f.LAND_IN_PROGRESS =1 ; // Signalisation du processus terrestre NAV_state =NAV_STATE_LAND_IN_PROGRESS; Pause; cas NAV_STATE_LAND_IN_PROGRESS :GPS_calc_poshold(); check_land(); //Appelle le détecteur de terrain if (f.LAND_COMPLETED) { nav_timer_stop =millis() + 5000 ; NAV_state =NAV_STATE_LANDED ; } Pause; case NAV_STATE_LANDED :// Désarmer si le manche des gaz est au minimum ou 5 secondes après l'atterrissage détecté if (rcData[THROTTLE] 0) { //si zéro non atteint faire un saut next_step =mission_step.parameter1; NAV_state =NAV_STATE_PROCESS_NEXT ; jump_times--; } Pause; case NAV_STATE_PROCESS_NEXT ://Traitement de la prochaine étape de la mission NAV_error =NAV_ERROR_NONE ; if (!recallWP(next_step)) { abort_mission(NAV_ERROR_WP_CRC); } else { switch(mission_step.action) { // Les commandes Waypoiny et hold commencent toutes par un statut en route, elles incluent également la commande 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); sinon NAV_state =NAV_STATE_WP_ENROUTE ; GPS_prev[LAT] =mission_step.pos[LAT] ; //Enregistrer les coordonnées wp pour un calcul d'itinéraire précis GPS_prev[LON] =mission_step.pos[LON]; Pause; cas MISSION_RTH :f.GPS_head_set =0 ; if (GPS_conf.rth_altitude ==0 &&mission_step.altitude ==0) //si config et mission_step alt est à zéro set_new_altitude(alt.EstAlt); // RTH revient à l'altitude réelle else { uint32_t rth_alt; if (mission_step.altitude ==0) rth_alt =GPS_conf.rth_altitude * 100; //l'altitude dans l'étape de la mission est prioritaire sinon 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); retourner; } // Nous commençons à l'altitude de l'emplacement actuel et modifions progressivement alt alt_to_hold =alt.EstAlt; // pour calculer le temps delta alt_change_timer =millis(); // enregistre l'altitude cible target_altitude =_new_alt; // réinitialiser notre intégrateur d'altitude alt_change =0; // enregistre l'altitude d'origine original_altitude =alt.EstAlt; // pour décider si nous avons atteint l'altitude cible if(target_altitude> original_altitude){ // nous sommes en dessous, en montant alt_change_flag =ASCENDING; } else if(target_altitude =target_altitude) alt_change_flag =REACHED_ALT; // nous ne devrions pas commander au-delà de notre cible if(alt_to_hold>=target_altitude) return target_altitude; } else if (alt_change_flag ==DESCENDING) { // nous sommes au-dessus, en descendant if(alt.EstAlt <=target_altitude) alt_change_flag =REACHED_ALT; // nous ne devrions pas commander au-delà de notre cible if(alt_to_hold <=target_altitude) return target_altitude; } // si nous avons atteint notre altitude cible, retourne la cible alt if(alt_change_flag ==REACHED_ALT) return target_altitude; int32_t diff =abs(alt_to_hold - target_altitude); // l'échelle est la façon dont nous générons un taux souhaité à partir du temps écoulé // une échelle plus petite signifie des taux plus rapides int8_t _scale =4; if (alt_to_hold > 4 =descente à 64 cm/s par défaut int32_t change =(millis() - alt_change_timer)>> _échelle ; if(alt_change_flag ==ASCENDING){ alt_change +=changer ; } else { alt_change -=changer; } // pour générer le temps delta alt_change_timer =millis(); return original_altitude + alt_change;}////////////////////////////////////////// /////////////////////////////////////// Fonctions de navigation GPS basées sur PID//Auteur :EOSBandi//Basé sur le code et les idées de l'équipe Arducopter :Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen//Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni//contrainte originale ne fonctionne pas avec les variablesint16_t constrain_int16(int16_t amt, int16_t low, int16_t high) { return ((amt)<(low)?(low):((amt)>(high)?(high):(amt))); }////////////////////////////////////////////// ////////////////////////////////////////////////////// pôles// Il est correct de calculer cela une fois par réglage de point de cheminement, car cela change un peu à la portée d'un multicoptère//void GPS_calc_longitude_scaling(int32_t lat) { GPS_scaleLonDown =cos(lat * 1.0e-7f * 0.01745329251f);}/ ////////////////////////////////////////////////////////////// //////////////////////// ////////// Définit le waypoint à parcourir, réinitialise les variables nécessaires et calcule les valeurs initiales//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;}//////////////////////////////////////////////////////////// ////////////////////////////////////// Vérifiez si nous avons manqué la destination d'une manière ou d'une autre// static bool check_missed_wp(void) { int32_t temp; temp =target_bearing - original_target_bearing; temp =wrap_18000(temp); retour (abs(temp)> 10000); // nous avons dépassé le waypoint de 100 degrés}////////////////////////////////////// /////////////////////////////////////////// Obtenir la distance entre deux points en cm// Obtenir le relèvement de pos1 à pos2, renvoie un 1deg =100 precisionvoid GPS_bearing(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* relèvement) { int32_t off_x =*lon2 - *lon1; int32_t off_y =(*lat2 - *lat1) / GPS_scaleLonDown; *roulement =9000 + atan2(-off_y, off_x) * 5729.57795f; //Convertir les redians de sortie en 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 =( flottant)(*lat2 - *lat1) ; // différence de latitude en 1/10 000 000 degrés float dLon =(float)(*lon2 - *lon1) * GPS_scaleLonDown; //x *dist =sqrt(sq(dLat) + sq(dLon)) * 1.11318845f;}//****************************** ******************************************************** *******************************// calc_velocity_and_filtered_position - vitesse dans les directions lon et lat calculée à partir de la position GPS// et des données de l'accéléromètre// lon_speed exprimé en cm/s. les nombres positifs signifient se déplacer vers l'est // vitesse_lat exprimée en cm/s. nombres positifs lors du déplacement vers le nord// Remarque :nous utilisons directement les emplacements GPS pour calculer la vitesse au lieu de demander au GPS la vitesse car// c'est plus précis en dessous de 1,5 m/s// Remarque :même si les positions sont projetées à l'aide d'un filtre à plomb, les vitesses sont calculées // à partir des emplacements GPS non modifiés. Nous ne voulons pas que le bruit de notre filtre principal affecte la vélocité//****************************************** ******************************************************** **************** static void GPS_calc_velocity(void){ static int16_t speed_old[2] ={0,0} ; statique int32_t last[2] ={0,0} ; statique 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:Vérifiez les changements de vitesse irréalistes et la navigation du signal sur la dégradation du signal GPS posibble if (!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] =actual_speed[_X] ; speed_old[_Y] =actual_speed[_Y] ; } } init=1; dernier[LON] =GPS_coord[LON] ; dernier[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], actual_speed[_Y], GPS_LAG); }}///////////////////////////////////////////// //////////////////////////////////// Calculer une erreur de localisation entre deux coordonnées GPS// Parce que nous utilisent lat et lon pour faire nos erreurs de distance voici un tableau rapide:// 100 =1 m // 1000 =11 m =36 pieds // 1800 =19,80 m =60 pieds // 3000 =33 m // 10 000 =111 m // statique 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; // Erreur X erreur[LAT] =*target_lat - *gps_lat; // Erreur Y}////////////////////////////////////////// /////////////////////////////////////// Calculer nav_lat et nav_lon à partir des x et y erreur et la vitesse //// A FAIRE :vérifier que la contrainte de vitesse cible poshold peut être augmentée pour un lockstatic poshold plus rapide void GPS_calc_poshold(void) { int32_t d; int32_t target_speed; uint8_t axe ; for (axis=0;axis<2;axis++) { target_speed =get_P(error[axis], &posholdPID_PARAM); // calcule la vitesse souhaitée à partir de l'erreur lat/lon target_speed =constrain(target_speed,-100,100); // Contraindre la vitesse cible en mode poshold à 1 m/s, cela permet d'éviter les emballements. rate_error[axis] =target_speed - actual_speed[axis]; // calcule l'erreur de vitesse 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[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM); d =contrainte(d, -2000, 2000); // se débarrasser du bruit if(abs(actual_speed[axis]) <50) d =0; nav[axe] +=d; // nav[axe] =contrainte(nav[axe], -NAV_BANK_MAX, NAV_BANK_MAX); nav[axe] =constrain_int16(nav[axe], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID[axe].integrator =poshold_ratePID[axe].integrator ; }}///////////////////////////////////////////// //////////////////////////////////// Calculez les nav_lat et nav_lon souhaités pour les vols à distance tels que RTH et WP//static void GPS_calc_nav_rate( uint16_t max_speed) { float trig[2]; int32_t target_speed[2] ; int32_t inclinaison ; uint8_t axe ; GPS_update_crosstrack(); int16_t cross_speed =crosstrack_error * (GPS_conf.crosstrack_gain / 100.0); //vérifie que tout va bien ? vitesse_croisée =contrainte(vitesse_croisée,-200,200); cross_speed =-cross_speed; float temp =(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] ; taux_erreur[axe] =contrainte(taux_erreur[axe],-1000,1000); nav[axe] =get_P(taux_erreur[axe], &navPID_PARAM) +get_I(taux_erreur[axe], &dTnav, &navPID[axe], &navPID_PARAM) +get_D(taux_erreur[axe], &dTnav, &navPID[axe], &navPID_PARAM) // nav[axe] =contrainte(nav[axe],-NAV_BANK_MAX,NAV_BANK_MAX); nav[axe] =constrain_int16(nav[axe], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID[axis].integrator =navPID[axis].integrator ; }}static void GPS_update_crosstrack(void) { // Erreur de crosstrack // ---------------- // Si nous sommes trop loin ou trop près, nous ne faisons pas de suivi après le flotteur temp =(target_bearing - original_target_bearing) * RADX100; crosstrack_error =sin(temp) * wp_distance; // Mètres nous sommes hors ligne}////////////////////////////////////////////////////// ////////////////////////////////////////// Déterminer la vitesse souhaitée lors de la navigation vers un waypoint, implémentez également une // rampe de vitesse lente au démarrage d'une navigation//// | 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.

Schémas