Arduino Quadcopter

Über dieses Projekt

Es ist nicht nur ein Quadcopter ... es ist eine Open-Source-Maschine!

Jetzt kommen die Fragen, wo und wie bekomme ich den Code für den Quadcopter? Die Antwort ist also Multiwii.

MultiWii ist eine sehr beliebte Flugsteuerungssoftware für DIY-Multirotoren mit einer großen Community. Es unterstützt verschiedene Multikopter mit erweiterten Funktionen wie Bluetooth-Steuerung durch Ihr Smartphone, OLED-Display, Barometer, Magnetometer, GPS-Positionshaltung und Rückkehr nach Hause, LED-Streifen und vieles mehr. Also bauen wir unseren Flugcontroller mit Arduino!

Schritt 1:Design des Flugreglers

Hier sind die Schaltpläne für die Flugsteuerungsplatine. Sie können eine auf Ihrer Allzweck-Leiterplatte herstellen oder eine Leiterplatte wie ich beim Hersteller bestellen.

ESC-Verbindungen

• D3 <

• D9 <

• D10 <

• D11 <

Verbindungen des Bluetooth-Moduls

• TX <

• RX <

MPU-6050-Anschlüsse

• A4 <

• A5 <

LED-Indicator

• D8 <

Empfängeranschlüsse

• D2 <

• D4 <

• D5 <

• D6 <

• D7 <

Schritt 2:Erstellen eines Rahmens

Ich habe mir einen DJI 450 Rahmen gekauft und meine Motoren und alles daran befestigt. Sie können das Video sehen, wie ich es gemacht habe.

Schritt 3:Anbringen des Flugreglers am Rahmen

Dann endlich den Regler und den Empfänger wie in den Schaltplänen gezeigt auf der Platine befestigen und alles ist fertig!

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 "Sensoren. h"#include "MultiWii.h"#include "EEPROM.h"#include #if GPS//Funktionsprototypen für andere GPS-Funktionen//Diese könnten vielleicht in die gps.h-Datei gehen, aber diese sind local to the gps.cpp static void GPS_pearing(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t*peil);static void GPS_distance_cm(int32_t* lat1, int32_t* lon1, int32_t* lat2, int2 ,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 uint_int16_speed );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:in normales C statt in C++ umschreiben// GPS-Lag einrichten#if define(UBLOX) || define (MTK_BINARY19)#define GPS_LAG 0.5f //UBLOX GPS hat eine kleinere Verzögerung als MTK and other#else#define GPS_LAG 1.0f //Wir gehen davon aus, dass MTK GPS eine Verzögerung von 1 Sekunde hat#endif static int32_t GPS_coord_lead[2]; // Bleigefilterte GPS-Koordinatenklasse LeadFilter {public:LeadFilter() :_last_velocity(0) { } // Min- und Max-Radiowerte in CLI einrichten int32_t get_position(int32_t pos, int16_t vel, float lag_in_seconds =1.0); Void clear () { _last_velocity =0; }privat: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; // Geschwindigkeit für die nächste Iteration speichern _last_velocity =vel; return pos + vel_contribution + accel_contribution;}LeadFilter xLeadFilter; // Langes GPS-Lag-Filter LeadFilter yLeadFilter; // Lat GPS-Lagfilter typedef struct PID_PARAM_ { float kP; Schwimmer kI; Schwimmer kD; Schwimmer Imax; } PID_PARAM; PID_PARAM posholdPID_PARAM;PID_PARAM poshold_ratePID_PARAM;PID_PARAM navPID_PARAM;typedef struct PID_ { float integrator; // Integratorwert int32_t last_input; // letzte Eingabe für die Ableitung float lastderivative; // letzte Ableitung für die Float-Ausgabe des Tiefpassfilters; Float-Derivat;} 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 error, float* dt, struct PID_* pid, struct PID_PARAM_* pid_param) { pid->integrator +=((float)error * pid_param->kI) * *dt; pid->integrator =Constraint(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 Millisekunden pid->derivative =(input - pid->last_input) / *dt; /// Tiefpassfilter-Schnittfrequenz für die Ableitungsberechnung. Schwimmerfilter =7,9577e-3; // Auf "1 / ( 2 * PI * f_cut )" setzen; // Beispiele für _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 // diskreter Tiefpassfilter, schneidet das // hochfrequente Rauschen heraus, das den Controller verrückt machen kann pid-> Ableitung =pid -> letzte Ableitung + (*dt / (Filter + *dt)) * (pid -> Ableitung - pid -> letzte Ableitung); // Status aktualisieren pid->last_input =input; pid->lastderivative =pid->derivative; // Ableitungskomponente hinzufügen 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; //Erkenne Land (extern)statisch uint32_t land_settle_timer;uint8_t GPS_Frame; // ein gültiger GPS_Frame wurde erkannt und die Daten sind bereit für die Nav-Berechnungstatic float dTnav; // Delta Time in Millisekunden für Navigationsberechnungen, aktualisiert mit jedem guten GPS readstatic int16_t current_speed[2] ={0,0};static float GPS_scaleLonDown; // Dies wird verwendet, um den schrumpfenden Längengrad auszugleichen, wenn wir uns den Polen nähern // Die Differenz zwischen der gewünschten Reisegeschwindigkeit und der tatsächlichen Reisegeschwindigkeit // aktualisiert nach dem GPS-Lesen - 5-10hzstatic int16_t rate_error[2];static int32_t error[2];statisch int32_t GPS_WP[2]; //Derzeit verwendetes WPstatic int32_t GPS_FROM[2]; //der vorhergehende Wegpunkt für eine genaue Spurverfolgungint32_t target_pearing; // Dies ist der Winkel vom Copter zur "next_WP" Position in Grad * 100static int32_t original_target_pearing; // deg * 100, Der ursprüngliche Winkel zum next_WP, wenn der next_WP gesetzt wurde, Wird auch verwendet, um zu überprüfen, wann wir einen WPstatic übergeben int16_t crosstrack_error; // Die Winkelkorrektur, die auf target_pearing angewendet wird, um den Copter wieder auf seinen optimalen pathuint32_t wp_distance zu bringen; // Abstand zwischen Flugzeug und next_WP in cmstatic uint16_t waypoint_speed_gov; // verwendet für langsames Aufziehen beim Starten der Navigation; ///////////////////////////////////// //////////////////////////////////////////////// gleitender Durchschnitt Filtervariablen//#define GPS_FILTER_VECTOR_LENGTH 5statisch uint8_t GPS_filter_index =0;statisch int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];statisch int32_t GPS_filter_sum[2];statisch int32_t GPS_read[2];statisch int32_t GPS_t_filtered2]; //der lat-lon-Grad ohne Dezimalstellen (lat/10 000 000)static uint16_t Fraction3[2];static int16_t nav_takeoff_pearing; // speichert die Peilung beim Start (1deg =1), die verwendet wird, um bei der Ankunft zu Hause in die Startrichtung zu drehen // Hauptnavigationsprozessor und Zustandsmaschine // TODO:Verarbeitungszustände hinzufügen, um die Verarbeitungsbelastung zu verringern uint8_t GPS_Compute(void) { unsigned char axis; uint32_t dist; //Temp-Variable zum Speichern von Dist to Copter int32_t dir; //Temp-Variable zum Speichern des Verzeichnisses im Copter statisch uint32_t nav_loopTimer; //überprüfen, ob wir einen gültigen Frame haben, wenn nicht, dann sofort zurückkehren if (GPS_Frame ==0) return 0; sonst GPS_Frame =0; // Home-Position prüfen und setzen, falls nicht gesetzt 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(); } //Gleitende Durchschnittsfilter auf GPS-Daten anwenden if (GPS_conf.filtering) { GPS_filter_index =(GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH; for (Achse =0; Achse<2; Achse++) { GPS_read[Achse] =GPS_coord[Achse]; // neueste ungefilterte Daten sind in GPS_latitude und GPS_longitude GPS_degree[axis] =GPS_read[axis] / 10000000; // Holen Sie sich den Grad, um sicherzustellen, dass die Summe zu int32_t passt // Wie nahe sind wir an einer Gradlinie? es sind die ersten drei Ziffern aus den Bruchteilen von Grad // später verwenden wir es, um zu überprüfen, ob wir uns in der Nähe einer Gradlinie befinden, wenn ja, deaktivieren Sie die Mittelwertbildung, Fraction3[Achse] =(GPS_read[Achse]- GPS_degree[Achse]*10000000 ) / 10000; GPS_filter_sum[Achse] -=GPS_filter[Achse][GPS_filter_index]; GPS_filter[Achse][GPS_filter_index] =GPS_read[Achse] – (GPS_Grad[Achse]*10000000); GPS_filter_sum[Achse] +=GPS_filter[Achse][GPS_filter_index]; GPS_filtered[Achse] =GPS_filter_sum[Achse] / GPS_FILTER_VECTOR_LENGTH + (GPS_Grad[Achse]*10000000); if ( NAV_state ==NAV_STATE_HOLD_INFINIT || NAV_state ==NAV_STATE_HOLD_TIMED) { //Wir verwenden GPS-Mittelung nur im Poshold-Modus... if ( Fraction3[Achse]>1 &&Fraktion3[Achse]<999 ) GPS_coord[Achse] =GPS_gefiltert[[ Achse]; } } } // dTnav-Berechnung // Zeit für die Berechnung der XY-Geschwindigkeit und der Navigations-Pids dTnav =(float) (millis () - nav_loopTimer) / 1000.0; nav_loopTimer =millis(); // Hochlauf von schlechtem GPS verhindern dTnav =min (dTnav, 1.0); // Berechnen Sie kontinuierlich Entfernung und Peilung für GUI und andere Dinge - Von zu Hause zum Copter GPS_pearing(&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) {//Wenn wir kein Home-Set haben, zeigen Sie nichts an GPS_distanceToHome =0; GPS_directionToHome =0; } //Zauneinstellung prüfen und ggf. RTH ausführen //TODO:Autolanding if ((GPS_conf.fence> 0) &&(GPS_conf.fence  5000) NAV_state =NAV_STATE_LAND_START_DESCENT; brechen; Fall NAV_STATE_LAND_START_DESCENT:GPS_calc_poshold(); //Land in Position halten f.THROTTLE_IGNORED =1; // Gashebeleingabe ignorieren f.GPS_BARO_MODE =1; //Übernehmen Sie die Kontrolle über den BARO-Modus land_detect =0; // Landdetektor zurücksetzen f.LAND_COMPLETED =0; f.LAND_IN_PROGRESS =1; // Flag Landprozess NAV_state =NAV_STATE_LAND_IN_PROGRESS; brechen; Fall NAV_STATE_LAND_IN_PROGRESS:GPS_calc_poshold(); check_land(); // Landdetektor anrufen if (f.LAND_COMPLETED) { nav_timer_stop =millis () + 5000; NAV_state =NAV_STATE_LANDED; } brechen; case NAV_STATE_LANDED:// Unscharf, wenn der THROTTLE-Stick auf Minimum ist oder 5 Sek. nach dem Landen erkannt wurde if (rcData[THROTTLE] 0) {//wenn Null nicht erreicht ist, mache einen Sprung next_step =mission_step.parameter1; NAV_state =NAV_STATE_PROCESS_NEXT; jump_times--; } brechen; Fall NAV_STATE_PROCESS_NEXT://Bearbeitung des nächsten Missionsschritts NAV_error =NAV_ERROR_NONE; if (!recallWP(next_step)) { abort_mission(NAV_ERROR_WP_CRC); } else { switch(mission_step.action) { //Waypoiny- und Hold-Befehle beginnen alle mit einem Enroute-Status es beinhaltet auch den LAND-Befehl 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); sonst NAV_state =NAV_STATE_WP_ENROUTE; GPS_prev[LAT] =mission_step.pos[LAT]; //Wp-Koordinaten für genaue Routenberechnung speichern GPS_prev[LON] =mission_step.pos[LON]; brechen; Fall MISSION_RTH:f.GPS_head_set =0; if (GPS_conf.rth_altitude ==0 &&mission_step.altitude ==0) //wenn config und mission_step alt gleich Null sind set_new_altitude(alt.EstAlt); // RTH gibt die aktuelle Höhe zurück else { uint32_t rth_alt; if (mission_step.altitude ==0) rth_alt =GPS_conf.rth_altitude * 100; //Höhe im Missionsschritt hat Priorität 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); Rückkehr; } // Wir beginnen auf der aktuellen Positionshöhe und ändern allmählich alt alt_to_hold =alt.EstAlt; // zur Berechnung der Deltazeit alt_change_timer =millis(); // Zielhöhe speichern target_altitude =_new_alt; // unseren Höhenintegrator zurücksetzen alt_change =0; // speichere die ursprüngliche Höhe original_altitude =alt.EstAlt; // um zu entscheiden, ob wir die Zielhöhe erreicht haben if(target_altitude> original_altitude){ // wir sind unten und gehen nach oben alt_change_flag =ASCENDING; } else if(target_altitude =target_altitude) alt_change_flag =REACHED_ALT; // wir sollten nicht über unser Ziel hinauskommandieren if(alt_to_hold>=target_altitude) return target_altitude; } else if (alt_change_flag ==DESCENDING) { // wir sind oben, gehen runter if(alt.EstAlt <=target_altitude) alt_change_flag =REACHED_ALT; // wir sollten nicht an unserem Ziel vorbeikommandieren if(alt_to_hold <=target_altitude) return target_altitude; } // wenn wir unsere Zielhöhe erreicht haben, geben Sie das Ziel zurück alt if(alt_change_flag ==REACHED_ALT) return target_altitude; int32_t diff =abs(alt_to_hold - target_altitude); // scale ist, wie wir aus der verstrichenen Zeit eine gewünschte Rate generieren // eine kleinere Skala bedeutet schnellere Rate int8_t _scale =4; if (alt_to_hold > 4 =64 cm/s Abstieg standardmäßig int32_t change =(millis() - alt_change_timer)>> _Skala; if(alt_change_flag ==ASCENDING){ alt_change +=change; } else { alt_change -=ändern; } // zum Generieren der Deltazeit alt_change_timer =millis(); return original_altitude + alt_change;}////////////////////////////////////////// /////////////////////////////////////////PID-basierte GPS-Navigationsfunktionen//Autor :EOSBandi//Basierend auf Code und Ideen des Arducopter-Teams:Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen//Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni//originale Einschränkung funktioniert nicht mit Variablenint16_t Constrain_int16(int16_t amt, int16_t low, int16_t high) { return ((amt)<(low)?(low):((amt)>(high)?(high):(amt))); }////////////////////////////////////////////// //////////////////////////////////// dies wird verwendet, um den schrumpfenden Längengrad auszugleichen, wenn wir in Richtung gehen poles// Es ist in Ordnung, dies einmal pro Wegpunkteinstellung zu berechnen, da es sich in Reichweite eines Multicopters ein wenig ändert //void GPS_calc_longitude_scaling(int32_t lat) { GPS_scaleLonDown =cos(lat * 1.0e-7f * 0.01745329251f);}/ //////////////////////////////////////////////// ////////////////////////// ////////// Setzt den Wegpunkt zum Navigieren, setzt notwendige Variablen zurück und berechnet Anfangswerte //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_pearing(&GPS_FROM[LAT],&GPS_FROM[LON],&GPS_WP[LAT],&GPS_WP[LON],&target_pearing); 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; ursprüngliches_ziel_lager =ziel_lager;}/////////////////////////////////////////// //////////////////////////////////////// Überprüfen Sie, ob wir das Ziel irgendwie verpasst haben// static bool check_missed_wp(void) { int32_t temp; temp =Ziel_Lager - Original_Ziel_Lager; temp =Wrap_18000 (temp); zurück (abs(temp)> 10000); // wir haben den Wegpunkt um 100 Grad passiert}/////////////////////////////////////// ///////////////////////////////////////////// Distanz zwischen zwei ermitteln Punkte in cm // Peilung von pos1 zu pos2 erhalten, gibt 1deg =100 zurück. int32_t off_y =(*lat2 - *lat1) / GPS_scaleLonDown; *Lager =9000 + atan2(-off_y, off_x) * 5729.57795f; // Die Ausgabe-Rediane in 100xdeg umwandeln if (*peilung <0) *peilung +=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); // Breitengraddifferenz in 1/10 000 000 Grad float dLon =(float)(*lon2 - *lon1) * GPS_scaleLonDown; //x *dist =sqrt(sq(dLat) + sq(dLon)) * 1.11318845f;}//**************************** ************************************************* **************************** // calc_velocity_and_filtered_position - Geschwindigkeit in Längen- und Breitenrichtung berechnet aus GPS-Position // und Beschleunigungsmesserdaten // lon_speed ausgedrückt in cm/s. positive Zahlen bedeuten, sich nach Osten zu bewegen // lat_speed, ausgedrückt in cm/s. positive Zahlen, wenn man sich nach Norden bewegt // Hinweis:Wir verwenden GPS-Positionen direkt zur Berechnung der Geschwindigkeit, anstatt GPS nach der Geschwindigkeit zu fragen, da dies unter 1,5 m/s genauer ist // Hinweis:Obwohl die Positionen mit einem Bleifilter projiziert werden, Die Geschwindigkeiten werden // aus den unveränderten GPS-Standorten berechnet. Wir möchten nicht, dass Rauschen von unserem Lead-Filter die Geschwindigkeit beeinflusst //************************************ ************************************************* ****************statische Leere GPS_calc_velocity(void){statische 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; current_speed[_X] =(float)(GPS_coord[LON] - last[LON]) * GPS_scaleLonDown * tmp; current_speed[_Y] =(float)(GPS_coord[LAT] - last[LAT]) * tmp; // TODO:Überprüfen Sie unrealistische Geschwindigkeitsänderungen und Signalnavigation auf mögliche Verschlechterung des GPS-Signals if (!GPS_conf.lead_filter) {actual_speed[_X] =(actual_speed[_X] + speed_old[_X]) / 2; Ist_Geschwindigkeit[_Y] =(Ist_Geschwindigkeit[_Y] + Geschwindigkeit_Alt[_Y]) / 2; speed_old[_X] =aktuelle_speed[_X]; speed_old[_Y] =aktuelle_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], current_speed[_X], GPS_LAG); GPS_coord_lead[LAT] =yLeadFilter.get_position(GPS_coord[LAT], current_speed[_Y], GPS_LAG); }}///////////////////////////////////////////// ///////////////////////////////////// Berechnen eines Positionsfehlers zwischen zwei GPS-Koordinaten // Weil wir verwenden lat und lon, um unsere Distanzfehler zu machen, hier ist eine kurze Tabelle: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 Fehler error[LAT] =*target_lat - *gps_lat; // Y-Fehler}//////////////////////////////////////////// //////////////////////////////////////// Berechne nav_lat und nav_lon aus x und y error und die Geschwindigkeit //// TODO:Überprüfen Sie, ob die poshold-Zielgeschwindigkeitsbeschränkung für schnellere poshold lockstatic erhöht werden kann void GPS_calc_poshold(void) { int32_t d; int32_t Zielgeschwindigkeit; uint8_t-Achse; for (axis=0;axis<2;axis++) { target_speed =get_P(error[axis], &posholdPID_PARAM); // Berechnen Sie die gewünschte Geschwindigkeit aus dem Lat / Lon-Fehler target_speed =Constraint (target_speed,-100,100); // Beschränken Sie die Zielgeschwindigkeit im Poshold-Modus auf 1 m/s, um Ausreißer zu vermeiden. rate_error[axis] =target_speed - current_speed[axis]; // Geschwindigkeitsfehler berechnen nav[Achse] =get_P(rate_error[Achse], &poshold_ratePID_PARAM) +get_I(rate_error[Achse] + error[Achse], &dTnav, &poshold_ratePID[Achse], &poshold_ratePID_PARAM); d =get_D(Fehler[Achse], &dTnav, &poshold_ratePID[Achse], &poshold_ratePID_PARAM); d =Beschränkung (d, -2000, 2000); // Rauschen beseitigen if (abs (actual_speed [axis]) <50) d =0; Navi[Achse] +=d; // nav[Achse] =einschränken(nav[Achse], -NAV_BANK_MAX, NAV_BANK_MAX); nav[Achse] =Constraint_int16(nav[Achse], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID[Achse].integrator =poshold_ratePID[Achse].integrator; }}///////////////////////////////////////////// ///////////////////////////////////// Berechnen des gewünschten nav_lat und nav_lon für Distanzflüge wie RTH und WP // static void GPS_calc_nav_rate (uint16_t max_speed) {float trig[2]; int32_t Zielgeschwindigkeit[2]; int32_t Neigung; uint8_t-Achse; GPS_update_crosstrack(); int16_t cross_speed =crosstrack_error * (GPS_conf.crosstrack_gain / 100.0); //Überprüfen, ob es in Ordnung ist? cross_speed =beschränken (cross_speed,-200,200); cross_speed =-cross_speed; Schwimmertemp =(9000l - Ziel_Lager) * 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] - current_speed[axis]; rate_error[Achse] =einschränken(rate_error[Achse],-1000,1000); nav[Achse] =get_P(rate_error[Achse], &navPID_PARAM) +get_I(rate_error[Achse], &dTnav, &navPID[Achse], &navPID_PARAM) +get_D(rate_error[Achse], &dTnav, &navPID[Achse], &navPID_PARAM); // nav[Achse] =einschränken(nav[Achse],-NAV_BANK_MAX,NAV_BANK_MAX); nav[Achse] =Constraint_int16(nav[Achse], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID[Achse].integrator =navPID[Achse].integrator; }}static void GPS_update_crosstrack(void) {// Crosstrack Error // ---------------- // Wenn wir zu weit weg oder zu nahe sind, verfolgen wir nicht den Float temp =(Ziel_Lager - Original_Ziel_Lager) * RADX100; crosstrack_error =sin(temp) * wp_distance; // Meter sind wir außerhalb der Spur}//////////////////////////////////////// //////////////////////////////////////////// Gewünschte Geschwindigkeit beim Navigieren ermitteln Richtung Wegpunkt, auch langsamen // Geschwindigkeitsanstieg beim Start einer Navigation implementieren //// | 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.

Schaltpläne