Differences

This shows you the differences between two versions of the page.

Link to this comparison view

px4-analyse-navigator [2018/10/26 01:25]
natwati created
px4-analyse-navigator [2018/10/26 01:26] (current)
natwati
Line 1: Line 1:
-====== ​test ======+====== ​Navigator ​====== 
 +Dieses Modul ist für das autonome Fliegen (z.B. Mission-Mode) zuständig. Es veröffentlicht eine Dreiergruppe aus Navigationszielen (letzter, aktueller und nächster Wegpunkt) als uORB „position_setpoint_triplet_s“. 
 +Dieses wird dann vom Positions-Controller für die Navigation verwendet.
  
 +Hier der entsprechende [[https://​github.com/​PX4/​Firmware/​tree/​v1.8.0/​src/​modules/​fw_pos_control_l1|Sourcecode]].
 +
 +==== Constructor ====
 +
 +<code c++ navigator_main.cpp>​
 + ModuleParams(nullptr),​
 + _loop_perf(perf_alloc(PC_ELAPSED,​ "​navigator"​)),​
 + _geofence(this),​
 +
 + _navigation_mode_array[0] = &​_mission;​
 + _navigation_mode_array[1] = &​_loiter;​
 + _navigation_mode_array[2] = &_rtl;
 + _navigation_mode_array[3] = &​_dataLinkLoss;​
 + _navigation_mode_array[4] = &​_engineFailure;​
 + _navigation_mode_array[5] = &​_gpsFailure;​
 + _navigation_mode_array[6] = &​_rcLoss;​
 + _navigation_mode_array[7] = &​_takeoff;​
 + _navigation_mode_array[8] = &_land;
 + _navigation_mode_array[9] = &​_precland;​
 + _navigation_mode_array[10] = &​_follow_target;​
 +</​code>​
 +
 +Im Konstruktor werden ein performance counter sowie das geofencing modul instanziert.
 +
 +Außerdem wird für jeden Mode eine spezifische Klasse instanziert und diese dem _navigation_mode_array hinzugefügt. Alle Modi implementieren die Basisklassen ModuleParams und MissionBlock (und damit auch desen Basisklasse NavigatorMode).
 +
 +ModuleParams ist die Basisklasse für all Module oder Klassen die configuration parameter einsetzen. Sie dient lediglich dazu einen Baum aus Eltern- und Kindknoten aufzubauen um über diesen Parameteränderungen weiterzugeben. Ändert sich ein Parameter wird die update() Funktion der Klasse aufgerufen.
 +
 +Land, Takeoff und EngineFailure benutzen keine Parameter und implementieren daher ModuleParams nicht.
 +
 +
 +==== Hilfsfunktionen ====
 +
 +Der Navigator implementiert einige Funktionen um die subscribten ORBs abzufragen. Für einige Objekte implementiert er die Möglichkeit das Auslesen des Wertes auch dann durchzuführen wenn sich der Wert nicht geändert hat.
 +<code c++ navigator_main.h>​
 + // update subscriptions
 + void fw_pos_ctrl_status_update(bool force = false);
 + void global_position_update();​
 + void gps_position_update();​
 + void home_position_update(bool force = false);
 + void local_position_update();​
 + void params_update();​
 + void vehicle_land_detected_update();​
 + void vehicle_status_update();​
 +</​code>​
 +
 +==== Initialisierung / Eingabewerte ====
 +
 +Der Navigator versucht die Geofence-Parameter aus einer Datei zu lesen falls diese vorhanden ist.
 +
 +Der Navigator nutzt die folgenden ORBs.
 +<code c++ navigator_main.cpp>​
 + /* do subscriptions */
 + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));​
 + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));​
 + _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));​
 + _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));​
 + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));​
 + _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));​
 + _home_pos_sub = orb_subscribe(ORB_ID(home_position));​
 + _offboard_mission_sub = orb_subscribe(ORB_ID(mission));​
 + _param_update_sub = orb_subscribe(ORB_ID(parameter_update));​
 + _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));​
 + _traffic_sub = orb_subscribe(ORB_ID(transponder_report));​
 +</​code>​
 +
 +**vehicle_global_position** und **vehicle_local_position** sind vom position_estimator_inav bereitgestellte Lageinformationen.
 +
 +**vehicle_gps_position** ist die GPS Position des Flugzeugs und kommt entweder vom Simulator oder vom GPS Treiber.
 +
 +**fw_pos_ctrl_status** ist ein Rückkanal des Fixed Wing Position Controllers an den Navigator. Hierbei werden lediglich einige landungsspezfische Parameter und der Akzeptanzradius für die Wegpunkte verwendet.
 +
 +**vehicle_status** gibt vom Controller aus an in welchem Navigation State sich das Flugzeug befindet:
 +<code c++ vehicle_status.msg>​
 +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode
 +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode
 +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode
 +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
 +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
 +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
 +uint8 NAVIGATION_STATE_AUTO_RCRECOVER = 6 # RC recover mode
 +uint8 NAVIGATION_STATE_AUTO_RTGS = 7 # Auto return to groundstation on data link loss
 +uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure
 +uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down)
 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
 +uint8 NAVIGATION_STATE_UNUSED = 11 # Free slot
 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
 +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
 +uint8 NAVIGATION_STATE_OFFBOARD = 14
 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode
 +uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "​flip"​) mode
 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff
 +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land
 +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow
 +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target
 +</​code>​
 +
 +**vehicle_land_detected** gibt an ob das Flugzeug gelandet ist.
 +
 +**home_position** wird durch den Commander gesetzt.
 +
 +**mission** enthält die hochgeladene Mission und wird entweder vom Mavlink oder vom Commander gesetzt. Das setzen im Commander geschieht nur einmalig zum Start, damit eine Mission auch dann genutzt werden kann wenn Mavlink nicht startet.
 +
 +**parameter_update** informiert naheliegenderweise über eine Änderung der Parameter.
 +
 +**vehicle_command** wird vom Mavlink Empfänger (mavlink_receiver) gesetzt und enthält die MAV_CMD-Nachrichten welche er nicht direkt selbst bearbeitet. In bestimmten Fällen sendet der Navigator sich auch selbst Nachrichten z.B. um die Landung abzubrechen. Alle Missionsitems die nicht den folgenden Befehlen entsprechen werden als vehicle_command veröffentlicht und nicht durch mission.cpp abgearbeitet.
 +
 +<code c++ mission_block.cpp>​
 +bool MissionBlock::​item_contains_position(const mission_item_s &item)
 +{
 + return item.nav_cmd == NAV_CMD_WAYPOINT ||
 +        ​item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
 +        ​item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 +        ​item.nav_cmd == NAV_CMD_LAND ||
 +        ​item.nav_cmd == NAV_CMD_TAKEOFF ||
 +        ​item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
 +        ​item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
 +        ​item.nav_cmd == NAV_CMD_VTOL_LAND ||
 +        ​item.nav_cmd == NAV_CMD_DO_FOLLOW_REPOSITION;​
 +}
 +</​code>​
 +
 +**transponder_report** dient lediglich der Kollisionsvermeidung für Drohnen mit Transpondern.
 +==== Main Loop ====
 +
 +Der Main Loop wird durchlaufen sobald neue Daten für den ORB vehicle_local_position vorliegen. Die Funktion ist so konfiguriert das sie höchstens alle 50 ms ein Update bekommt, aber auch ausgeführt wird, wenn nach 1000 ms kein Update vorliegt. In letzterem Fall wird die Main Loop nochmals mit der letzten vehicle_local_position durchlaufen.
 +
 +Am Anfang jedes Durchlaufs werden die folgenden Werte aktualisiert:​
 +  * GPS Position
 +  * Globale Position
 +  * Parameter
 +  * Vehicle Status
 +  * Vehicle Land Detected
 +  * Fixedwing Position Control Status (fw_pos_ctrl_status)
 +  * Home Position
 +  * Vehicle Command
 +
 +Zeile 312-515 Beschäftigen sich mit der Behandlung neuer vehicle_command.
 +
 +
 +Zeile 516-519 Es wird eine Kollisionsvermeidung für Drohnen mit Transponder geprüft.
 +
 +
 +Hier wird beispielsweise das Komando CMD_DO_REPOSITION beahndelt indem das _reposition_triplet aktualisiert und gültig gesetzt wird:​\\ ​
 +Als vorheriger Setpoint des Triplets wird die aktuelle Position (lat, lon, alt) und Ausrichtung (yaw) verwendet. Als aktueller Setpoint des Triplets werden die Parameter 4-7 des übertragenen Befehls (siehe [[https://​mavlink.io/​en/​messages/​common.html#​MAV_CMD_DO_REPOSITION|MAV_CMD_DO_REPOSITION]]) verwendet. Speed und Throttle werden auf die Rückgabewerte der Funktionen get_cruising_speed() und get_cruising_throttle() gesetzt. Es wird immer der LOITER-Typ (mit loiter_radius und direction 1) verwendet.
 +
 +Zeile 520-553 reagieren auf potentielle Geofencing Überschreitungen.
 +
 +Zeile 554-723 legt fest in welchem Modus die Navigation betrieben wird. Dies geschiebt basierend auf dem am Anfang der Schleife ausgelesenen ORB vehicle_status.
 +
 +Zeile 724-738 invalidiert die Position-Setpoint-Dreiergruppe (_pos_sp_triplet) falls sich der Navigations-Modus geändert hat.
 +
 +Zeile 739-745 führt die Aktivitätsstatemachines aller Modi aus.
 +
 +Zeile 746-757 Wenn das Flugzeug gelandet ist und noch keinen Takeoff Punkt erhalten hat wird der Momentane Wegpunkt des Triplets gültig geschalten und der Wegpunkttyp auf IDLE gesetzt.
 +
 +Zeile 758-763 Wenn kein Navigations-Modus gesetzt ist wird das Triplet einmalig ungültig gesetzt.
 +
 +Zeile 764-769 Wenn das Triplet in der Schleife aktualisiert wurde wird es jetzt publiziert.
 +
 +Zeile 770-773 Wenn das mission_result in der Schleife aktualisiert wurde wird es jetzt publiziert.
 +
 +Zeile 774-776 Die Main Loop beginnt von vorne.
 +
 +
 +==== Deinitialisierung ====
 +
 +Der Navigator unsubscribed alle subscribten ORBS (außer transponder_report).
 +
 +==== Weitere Funktionen ====
 +
 +<code c++ navigator.h>​
 + static int task_spawn(int argc, char *argv[]);
 + static Navigator *instantiate(int argc, char *argv[]);
 + static int custom_command(int argc, char *argv[]);
 + static int print_usage(const char *reason = nullptr);
 + void run() override;
 + int print_status() override;
 +</​code>​
 +
 +Es werden einige Grundfunktionen für Module implementiert. Eine Erklärung dazu findet sich in [[https://​github.com/​PX4/​Firmware/​tree/​v1.8.0/​src/​platforms/​px4_module.h|px4_module.h]]
 +
 +<code c++ navigator.h>​
 + float get_cruising_speed();​
 +</​code>​
 +Liest den angestrebten Cruising-Speed der Mission aus.
 +
 +<code c++ navigator.h>​
 + void set_cruising_speed(float speed = -1.0f);
 +</​code>​
 +Setzt den Cruising-Speed auf einen übergebenen Wert.
 +
 +
 +<code c++ navigator.h>​
 +void reset_cruising_speed();​
 +</​code>​
 +Setzt den Cruising-Speed auf den default Wert zurück.
 +
 +
 +<code c++ navigator.h>​
 +void get_cruising_throttle();​
 +</​code>​
 +Liefert das Cruising-Throotle für die aktuelle Mission zurück.
 +
 +
 +<code c++ navigator.h>​
 +void set_cruising_throttle(float throttle = -1.0f) { _mission_throttle = throttle; }
 +</​code>​
 +Setzt das Cruising-Throotle für die aktuelle Mission.
 +
 +
 +<code c++ navigator.h>​
 +bool abort_landing();​
 +</​code>​
 +Liefert zurück ob der Position Controller den Landevorgang abbrechen möchte (_fw_pos_ctrl_status.abort_landing).
 +
 +<code c++ navigator.h>​
 +bool force_vtol();​
 +</​code>​
 +Liefert den Wert von Force-VTOL zurück.
 +
 +<code c++ navigator.h>​
 +float get_default_acceptance_radius();​
 +</​code>​
 +Liefert den in den Parametern hinterlegten Akzeptanzradius zurück, bei dessen unterschreiten ein Wegpunkt als erreicht gilt.
 +
 +<code c++ navigator.h>​
 +float get_acceptance_radius();​
 +</​code>​
 +Liefert den aktuell gültigen Abstand (Radius) zurück ab dem der nächste Wegpunkt angesteuert werden soll. Dieser verwendet die _fw_pos_ctrl_status.turn_distance,​ falls diese größer als der im Parameter gesetzte Wert ist.
 +
 +<code c++ navigator.h>​
 +float get_altitude_acceptance_radius();​
 +</​code>​
 +Liefert die akzeptable Höhendifferenz zum Wegpunkt zurück, damit dieser als erreicht gilt.
 +
 +<code c++ navigator.h>​
 +void reset_triplets();​
 +</​code>​
 +Setzt das Triplet zurück.
 +
 +<code c++ navigator.h>​
 +void publish_position_setpoint_triplet();​
 +</​code>​
 +Veröffentlicht das Triplet.
 +
 +
 +
 +<code c++ navigator.h>​
 +void load_fence_from_file(const char *filename);
 +</​code>​
 +Lädt den geofence aus einer Datei.
 +
 +<code c++ navigator.h>​
 +void publish_geofence_result();​
 +</​code>​
 +Veröffentlicht das geofence_result.
 +
 +<code c++ navigator.h>​
 +void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 +void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
 +void set_mission_result_updated() { _mission_result_updated = true; }
 +</​code>​
 +Funktionen um interne Werte des Navigators zu setzen.
 +
 +<code c++ navigator.h>​
 +struct fw_pos_ctrl_status_s *get_fw_pos_ctrl_status() { return &​_fw_pos_ctrl_status;​ }
 +struct home_position_s *get_home_position() { return &​_home_pos;​ }
 +struct mission_result_s *get_mission_result() { return &​_mission_result;​ }
 +struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &​_pos_sp_triplet;​ }
 +struct position_setpoint_triplet_s *get_reposition_triplet() { return &​_reposition_triplet;​ }
 +struct position_setpoint_triplet_s *get_takeoff_triplet() { return &​_takeoff_triplet;​ }
 +struct vehicle_global_position_s *get_global_position() { return &​_global_pos;​ }
 +struct vehicle_land_detected_s *get_land_detected() { return &​_land_detected;​ }
 +struct vehicle_local_position_s *get_local_position() { return &​_local_pos;​ }
 +struct vehicle_status_s *get_vstatus() { return &​_vstatus;​ }
 +PrecLand *get_precland() { return &​_precland;​ } /**< allow others, e.g. Mission, to use the precision land block */
 +
 +const vehicle_roi_s &​get_vroi() { return _vroi; }
 +
 +bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt);​ }
 +bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos);​ }
 +
 +int get_offboard_mission_sub() { return _offboard_mission_sub;​ }
 +
 +Geofence &​get_geofence() { return _geofence; }
 +
 +bool get_can_loiter_at_sp() { return _can_loiter_at_sp;​ }
 +float get_loiter_radius() { return _param_loiter_radius.get();​ }
 +</​code>​
 +Funktionen um interne Werte des Navigators auszulesen.
 +
 +<code c++ navigator.h>​
 +void set_mission_result_updated() { _mission_result_updated = true; }
 +</​code>​
 +Setzt das Flag, dass _mission_result aktualisiert wurde.
 +
 +<code c++ navigator.h>​
 +void set_mission_failure(const char *reason);
 +</​code>​
 +Setzt das Failure-Flag für _mission_result.
 +
 +<code c++ navigator.h>​
 +void publish_mission_result();​
 +</​code>​
 +Veröffentlicht den ORB mission_result mit dem aktuellen Wert von _mission_result um den Commadner un Mavlink zu informieren.
 +
 +<code c++ navigator.h>​
 +void publish_vehicle_cmd(vehicle_command_s *vcmd);
 +</​code>​
 +
 +Veröffentlicht den ORB vehicle_command.\\ ​
 +Über diesen können Kommandos an den Navigator selbst oder an andere Teile des Systems weitergeleitet werden (z.B. Kamera Trigger).
 +
 +<code c++ navigator.h>​
 +void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
 +</​code>​
 +
 +Veröffentlicht den ORB vehicle_command_ack.\\ ​
 +Scheinbar ist das einzige Modul welches ACK der commands verarbeitet die [[https://​mavlink.io/​en/​protocol/​arm_authorization.html|Arm Authorization]] des Commanders (arm_auth.cpp).
 +
 +<code c++ navigator.h>​
 +void check_traffic();​
 +</​code>​
 +
 +Horcht auf Transmittersignale für die Kollisionsvermeidung.
 +
 +<code c++ navigator.h>​
 +void fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,​ float altitude_diff,​ float hor_velocity,​ float ver_velocity);​
 +</​code>​
 +
 +Simuliert Transmittersignale für die Kollisionsvermeidung.
 +
 +===== Navigator-Modi =====
 +
 +
 +Alle Modis implementieren die folgenden Funktionen:
 +
 +<code c++ navigator_mode.h>​
 + /**
 + * This function is called while the mode is inactive
 + */
 + virtual void on_inactive();​
 +
 + /**
 + * This function is called one time when mode becomes active, pos_sp_triplet must be initialized here
 + */
 + virtual void on_activation();​
 +
 + /**
 + * This function is called one time when mode becomes inactive
 + */
 + virtual void on_inactivation();​
 +
 + /**
 + * This function is called while the mode is active
 + */
 + virtual void on_active();​
 +</​code>​
 +
 +Die Funktionen werden durch eine in der run-Funktion implementierten State-machine ausgelöst:
 +
 +<code c++ navigator_mode.cpp>​
 +void
 +NavigatorMode::​run(bool active)
 +{
 + if (active) {
 + if (!_active) {
 + /* first run, reset stay in failsafe flag */
 + _navigator->​get_mission_result()->​stay_in_failsafe = false;
 + _navigator->​set_mission_result_updated();​
 + on_activation();​
 +
 + } else {
 + /* periodic updates when active */
 + on_active();​
 + }
 +
 + } else {
 + /* periodic updates when inactive */
 + if (_active) {
 + on_inactivation();​
 +
 + } else {
 + on_inactive();​
 + }
 + }
 +
 + _active = active;
 +}
 +</​code>​
 +
 +Alle Modi basieren auf der Mission-Block-Basisklasse! Dieses implementiert bereits eine weitreichende Standardfunktionalität mittels der folgenden Funktionen:
 +
 +<code c++ mission_block.h>​
 + mission_item_s _mission_item{};​
 +</​code>​
 +
 +Jeder Modus nutzt kann für interne Zwecke ein Missionitem nutzen.
 +
 +<code c++ mission_block.h>​
 + void reset_mission_item_reached();​
 +
 + bool _waypoint_position_reached{false};​
 + bool _waypoint_yaw_reached{false};​
 +
 + hrt_abstime _time_first_inside_orbit{0};​
 + hrt_abstime _time_wp_reached{0};​
 +</​code>​
 +
 +Die oben gelisteten Hilfsfunktionen beziehen sich dabei alle auf das interne Missionitem.
 +
 +<code c++ mission_block.h>​
 + bool is_mission_item_reached();​
 +</​code>​
 +
 +Diese Hilfsfunktion bezieht sich ebenfalls auf das interne Missionitem,​ ist jedoch deutlich komplexer als die anderen. Sie enthält einige Sonderbehandlungen.
 +
 +Im ersten Teil wird überprüft ob ein Wegpunkt erreicht wurde:
 +
 +Für Fixedwing wird beispielsweise der Setpoint des aktuellen Wegpunkt des Triplets zwischen POSITION und LOITER umgeschalten. Dies geschieht, falls der Flieger zwar die Position erreicht hat, nicht aber die Höhe. Am Ende wird der Setpoint wieder auf POSITION zurückgeschaltet.
 +Missionitems vom Typ NAV_CMD_TAKEOFF verwenden gesonderte Akzeptanzradien.
 +Andere Missionitems verwenden den Missionitemspezifischen Akzeptanzradius der vom Navigator über get_acceptance_radius(item) zurückgeliefert wird.
 +
 +Falls ein Wegpunkt als erreicht gilt, wird nun geprüft ob auch ein ggf. im Missionitem gesetzter YAW erreicht ist.
 +
 +Es wird false zurückgeliefert,​ wenn nicht sowohl Position als Yaw als erreicht gelten.
 +
 +Sollten sowohl Position als auch Yaw erreicht worden sein wird der _time_first_inside_orbit Zeitstempel gesetzt, falls dies bisher noch nicht geschehen ist. Es wird überprüft ob die time_inside Bedingung des Missionitems erfüllt wurde. Ist dies der Fall und gibt es einen gültigen nächsten Setpoint im Triplet des Navigators, so wird auf diesen ein neuer Kurs gesetzt. Dieser wird unter Zuhilfenahme von get_bearing_to_next_waypoint() und waypoint_from_heading_and_distance() berechnet und direkt in der Datenstruktur des Triplets aktualisiert,​ ohne den ORB zu verwenden. Es wird true zurückgegeben.
 +
 +
 +<code c++ mission_block.h>​
 + void issue_command(const mission_item_s &item);
 +</​code>​
 +
 +Veröffentlicht den Befehl über den ORB vehicle_command mit Hilfe des Navigators falls er nicht den folgenden Typen entspricht.
 +Über den ORB können Kommandos an den Navigator selbst oder an andere Teile des Systems weitergeleitet werden (z.B. Kamera Trigger).
 +
 +<code c++ mission_block.cpp>​
 +bool MissionBlock::​item_contains_position(const mission_item_s &item)
 +{
 + return item.nav_cmd == NAV_CMD_WAYPOINT ||
 +        ​item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
 +        ​item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 +        ​item.nav_cmd == NAV_CMD_LAND ||
 +        ​item.nav_cmd == NAV_CMD_TAKEOFF ||
 +        ​item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
 +        ​item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
 +        ​item.nav_cmd == NAV_CMD_VTOL_LAND ||
 +        ​item.nav_cmd == NAV_CMD_DO_FOLLOW_REPOSITION;​
 +}
 +</​code>​
 +
 +Eine Hilfsfunktion zum unterscheiden einer bestimmter Missionitem-Typen.
 +
 +<code c++ mission_block.h>​
 + bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
 +</​code>​
 +
 +Konvertiert ein Missionitem in einen Setpoint (z.B. in einen der drei Punkte des Triplet).
 +
 +<code c++ mission_block.h>​
 +
 + void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
 +
 + void set_takeoff_item(struct mission_item_s *item, float abs_altitude,​ float min_pitch = 0.0f);
 +
 + void set_land_item(struct mission_item_s *item, bool at_current_location);​
 +
 + void set_idle_item(struct mission_item_s *item);
 +
 + void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode);
 +</​code>​
 +
 +Es werden eine Reihe Hilfsfunktionen implementiert,​ welche das Erstellen von bestimmten Missionitem-Typen erlauben.
 +
 +<code c++ mission_block.h>​
 + void mission_apply_limitation(mission_item_s &item);
 +</​code>​
 +
 +Allgemeine Hilfsfunktion um ein Missionitem an Fahrzeugspezifische Grenzen anszupassen. Hier wird in der Version 1.8.0 allerdings lediglich der maximal Alititude eines Missionitem limitiert.
 +
 +<code c++ mission_block.h>​
 + float get_absolute_altitude_for_item(struct mission_item_s &​mission_item) const;
 +</​code>​
 +
 +Liefert den absoluten Altitude Wert eines Missionitems indem der Altitudewert der Home-Position des Navigators zum relativen Wert addiert wird.
 +
 +<code c++ mission_block.h>​
 + float get_time_inside(const struct mission_item_s &item);
 +</​code>​
 +
 +Liefert den time_inside Wert des übergebenen Missionitems zurück und 0 für ein Missionitem vom Typ NAV_CMD_TAKEOFF.
 +===== ECL-Bibliothek =====
 +
 +Einige wichtige Teile der Navigation werden durch die [[https://​github.com/​PX4/​ecl|ECL-Bibliothek]] abgedeckt. Hier werden unter anderem die Funktionen get_bearing_to_next_waypoint und waypoint_from_heading_and_distance implementiert.
 +
 +
 +===== Modi =====
 +
 +==== Auto Loiter Mode ====
 +
 +Dieser Mode wird in der Klasse Loiter implementiert. In der für Modi zu implementierenden Statemachine verhält er sich wie folgt:
 +
 +=== Bei Aktivierung (on_activation):​ ===
 +
 +Liegt ein gültiges reposition Triplet vor wird dieses einmalig gesetzt, ansonsten wird einmalig Loiter kommandiert.
 +
 +=== Während der Modus aktiv ist (on_active):​ ===
 +
 +
 +Liegt ein neues, gültiges reposition Triplet vor wird dieses einmalig gesetzt.
 +
 +=== Kommandieren von Loiter: ===
 +
 +Loiter wird nicht kommandiert sollte das Fahrzeug nicht armed oder gelandet sein.
 +<code c++ loiter.cpp>​
 + set_loiter_item(&​_mission_item,​ _navigator->​get_loiter_min_alt());​
 +</​code>​
 +Das Missionitem des Modes wird auf Loiter gesetzt.
 +
 +<code c++ loiter.cpp>​
 + // convert mission item to current setpoint
 + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->​get_position_setpoint_triplet();​
 + pos_sp_triplet->​current.velocity_valid = false;
 + pos_sp_triplet->​previous.valid = false;
 + mission_apply_limitation(_mission_item);​
 + mission_item_to_position_setpoint(_mission_item,​ &​pos_sp_triplet->​current);​
 + pos_sp_triplet->​next.valid = false;
 +
 + _navigator->​set_can_loiter_at_sp(pos_sp_triplet->​current.type == position_setpoint_s::​SETPOINT_TYPE_LOITER);​
 +
 + _navigator->​set_position_setpoint_triplet_updated();​
 +</​code>​
 +Der letzte Punkte des Triplets wird ungültig gemacht und das Missionitem in den aktuellen Punkt des Triplets konvertiert.
 +Es wird aktualisiert ob am neuen Setpoint Loiter möglich ist (set_can_loiter_at_sp).
 +Das neue Triplet wird über set_position_setpoint_triplet_updated als durch den Navigator upzudaten markiert.
 +
 +=== Setzen des reposition Triplet: ===
 +
 +Es wird nichts getan falls das Fahrzeug nicht armed ist.
 +Als vorheriger Setpoint des neuen Triplets wird die aktuelle Position (lat, lon, alt) und Ausrichtung (yaw) verwendet.
 +Als aktueller Setpoint des neuen Triplets wird der aktuelle Punkt des reposition Triplets verwendet.
 +Der Yaw zum nächsten Wegpunkt wird gesetzt falls MIS_YAWMODE nicht MISSION_YAWMODE_NONE entspricht.
 +Es wird aktualisiert ob am neuen Setpoint Loiter möglich ist (set_can_loiter_at_sp).
 +Das neue Triplet wird über set_position_setpoint_triplet_updated als durch den Navigator upzudaten markiert.
 +Das reposition Triplet wird ungültig gemacht.
 +
 +===== Copyright =====
 +
 +
 +Für alle hier gezeigten Codeteile gilt das folgende Copyright:
 +<code c++>
 +/​****************************************************************************
 + *
 + ​* ​  ​Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
 + *
 + * Redistribution and use in source and binary forms, with or without
 + * modification,​ are permitted provided that the following conditions
 + * are met:
 + *
 + * 1. Redistributions of source code must retain the above copyright
 + ​* ​   notice, this list of conditions and the following disclaimer.
 + * 2. Redistributions in binary form must reproduce the above copyright
 + ​* ​   notice, this list of conditions and the following disclaimer in
 + ​* ​   the documentation and/or other materials provided with the
 + ​* ​   distribution.
 + * 3. Neither the name PX4 nor the names of its contributors may be
 + ​* ​   used to endorse or promote products derived from this software
 + ​* ​   without specific prior written permission.
 + *
 + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 + * POSSIBILITY OF SUCH DAMAGE.
 + *
 + ​****************************************************************************/​
 +</​code>​
  • px4-analyse-navigator.txt
  • Last modified: 2018/10/26 01:26
  • by natwati