Autopilot

At the center of the Guidance and Control system is the Autopilot, implemented in C++. This is located inside the pegasus_autopilot/autopilot package. The Autopilot is responsible for keeping track of the current operating mode of the vehicle. It exposes to each operation mode the following APIs:

  • Control - Provides the control commands to the vehicle to track a given position, attitude, velocity, etc.

  • Geofencing - Checks if the vehicle is within the geofence and if not, provides the control commands to bring the vehicle back inside the geofence.

  • Trajectory Manager - Manages desired trajectories for the vehicle to follow.

It also provides each mode with the current state of the vehicle, which includes the current position, velocity, attitude, etc.

Autopilot Architecture

The autopilot operation modes, controllers, geofencing and trajectory manager are implemented as ROS 2 plugins. This allows for easy extensibility and customization of the Autopilot, without having to modify the base autopilot packages. The Autopilot is responsible for loading the plugins at runtime and managing the communication between them.

        classDiagram
   pegasus <|-- pegasus_interfaces
   pegasus <|-- pegasus_console
   pegasus <|-- pegasus_autopilot
   pegasus <|-- pegasus_api
   pegasus_autopilot <|-- autopilot_modes
   pegasus_autopilot <|-- autopilot_controllers
   pegasus_autopilot <|-- geofencing
   pegasus_autopilot <|-- trajectory_manager
   trajectory_manager <|-- static_trajectory_manager
   static_trajectory_manager <|-- static_trajectories
   geofencing <|-- box_geofencing
   pegasus_interfaces <|-- mocap_interface
   pegasus_interfaces <|-- mavlink_interface
   class pegasus{
      <<Meta Package>>
      VehicleLaunchFiles
      VehicleConfigurationFiles
   }
   class mocap_interface{
      <<C++ Package>>
   }
   class pegasus_interfaces{
      <<Meta Package>>
   }
   class mavlink_interface{
      <<C++ Package>>
   }
   class pegasus_console{
      <<C++ Package>>
   }
   class pegasus_autopilot{
      <<C++ Package>>
   }
   class autopilot_controllers{
      <<C++ Package>>
   }
   class autopilot_modes{
      <<C++ Package>>
   }
   class pegasus_api{
      <<Python Package>>
   }
   class trajectory_manager{
      <<C++ Package>>
   }

0. Operating Modes

The default operating modes provided by the Autopilot are:

  • Arm - The vehicle is armed and ready to take off.

  • Disarm - The vehicle is on the ground and disarmed.

  • Takeoff - The vehicle takes of to a predefined altitude above the current position.

  • Land - The vehicle lands at the current position.

  • Hold - The vehicle holds its current position.

  • Follow Trajectory - The vehicle follows the trajectory loaded in the trajectory manager.

  • Waypoint - The vehicle goes to a waypoint.

1. Autopilot Interface

The Autopilot is defined in the under the pegasus_autopilot/autopilot/include/autopilot/autopilot.hpp header file.

  1namespace autopilot {
  2
  3class Autopilot : public rclcpp::Node {
  4
  5public:
  6
  7    Autopilot();
  8    ~Autopilot() {}
  9
 10    // Function that executes periodically the control loop of each operation mode
 11    virtual void update();
 12    
 13    // Function that establishes the state machine to transition between operating modes
 14    virtual bool change_mode(const std::string new_mode, bool force=false);
 15
 16    // Function that signals that the current mode has finished its operation and should transition to the fallback mode
 17    virtual void signal_mode_finished();
 18
 19    // Returns the current mode of operation of the autopilot and state of the vehicle
 20    inline std::string get_mode() const { return current_mode_; }
 21    inline State get_state() const { return state_; }
 22    inline VehicleStatus get_status() const { return status_; }
 23    inline VehicleConstants get_vehicle_constants() const { return vehicle_constants_; }
 24
 25    // Initializes the autopilot to run
 26    void initialize();
 27
 28private:
 29
 30    // Pre-initializations of the autopilot
 31    void initialize_controller();
 32    void initialize_geofencing();
 33    void initialize_trajectory_manager();
 34    void initialize_operating_modes();
 35
 36    // ROS2 node initializations
 37    void initialize_autopilot();
 38    void initialize_publishers();
 39    void initialize_subscribers();
 40    void initialize_services();
 41
 42    // Subscriber callbacks to get the current state of the vehicle
 43    void state_callback(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
 44    void status_callback(const pegasus_msgs::msg::Status::ConstSharedPtr msg);
 45    void vehicle_constants_callback(const pegasus_msgs::msg::VehicleConstants::ConstSharedPtr msg);
 46
 47    // Services callbacks to set the operation mode of the autopilot
 48    void change_mode_callback(const std::shared_ptr<pegasus_msgs::srv::SetMode::Request> request, std::shared_ptr<pegasus_msgs::srv::SetMode::Response> response);
 49
 50    // ROS 2 service to change the operation mode
 51    rclcpp::Service<pegasus_msgs::srv::SetMode>::SharedPtr change_mode_service_;
 52
 53    // ROS2 publishers
 54    rclcpp::Publisher<pegasus_msgs::msg::AutopilotStatus>::SharedPtr status_publisher_;
 55    
 56    // ROS2 subscribers
 57    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr state_subscriber_;
 58    rclcpp::Subscription<pegasus_msgs::msg::Status>::SharedPtr status_subscriber_;
 59    rclcpp::Subscription<pegasus_msgs::msg::VehicleConstants>::SharedPtr vehicle_constants_subscriber_;
 60
 61    // ROS2 messages
 62    pegasus_msgs::msg::AutopilotStatus status_msg_;
 63    pegasus_msgs::msg::VehicleConstants vehicle_constants_msg_;
 64
 65    // ROS 2 timer to handle the control modes, update the controllers and publish the control commands
 66    rclcpp::TimerBase::SharedPtr timer_;
 67
 68    // Modes of operation of the autopilot
 69    std::map<std::string, autopilot::Mode::UniquePtr> operating_modes_;
 70    std::map<std::string, std::vector<std::string>> valid_transitions_;
 71    std::map<std::string, std::string> fallback_modes_;
 72    std::map<std::string, std::string> on_finish_modes_;
 73    std::map<std::string, std::string> geofencing_violation_fallback_;
 74
 75    // Configuration for the operation modes for the autopilot
 76    Mode::Config mode_config_;
 77
 78    // Current state and status of the vehicle
 79    State state_;
 80    VehicleStatus status_;
 81    VehicleConstants vehicle_constants_;
 82    std::string current_mode_{"Uninitialized"};
 83
 84    // Low level controllers for reference tracking
 85    Controller::Config controller_config_;
 86    autopilot::Controller::SharedPtr controller_{nullptr};
 87
 88    // Geofencing object to check if the vehicle is inside the geofence
 89    Geofencing::Config geofencing_config_;
 90    autopilot::Geofencing::UniquePtr geofencing_{nullptr};
 91
 92    // Trajectory manager to handle complex trajectories and motion planning
 93    TrajectoryManager::Config trajectory_manager_config_;
 94    autopilot::TrajectoryManager::SharedPtr trajectory_manager_{nullptr};
 95
 96    // Auxiliar counter to keep track when forcing a mode change
 97    int force_change_counter_{0};
 98
 99    // Auxiliar variable used to keep track of time
100    rclcpp::Time last_time_;
101
102    // Auxiliar flag to check if the operating mode has finished
103    bool mode_finished_{false};
104
105    // Class loaders for the plugins
106    std::unique_ptr<pluginlib::ClassLoader<autopilot::Mode>> mode_loader_;
107    std::unique_ptr<pluginlib::ClassLoader<autopilot::Controller>> controller_loader_;
108    std::unique_ptr<pluginlib::ClassLoader<autopilot::Geofencing>> geofencing_loader_;
109    std::unique_ptr<pluginlib::ClassLoader<autopilot::TrajectoryManager>> trajectory_manager_loader_;
110};
111
112} // namespace autopilot

2. Explanation

In order to

87    // Function that executes periodically the control loop of each operation mode
88    virtual void update();

3. Autopilot Configuration File

In order to

 1/**:
 2  ros__parameters:
 3    autopilot:
 4      # Update rate
 5      rate: 50.0 # Hz
 6      # ----------------------------------------------------------------------------------------------------------
 7      # Definition of the controller that will perform the tracking of references of the different operation modes
 8      # ----------------------------------------------------------------------------------------------------------
 9      controller: "MellingerController"
10      # Configurations needed by the PX4 controller
11      OnboardController:
12        publishers:
13          control_position: "fmu/in/position"
14          control_attitude: "fmu/in/force/attitude"
15          control_attitude_rate: "fmu/in/force/attitude_rate"
16      PIDController:
17        publishers:
18          control_attitude: "fmu/in/force/attitude"
19          control_attitude_rate: "fmu/in/force/attitude_rate"
20          pid_debug_topic: "autopilot/statistics/pid"
21        # Gains for position PIDs on [x, y, z]
22        gains:
23          kp: [8.0, 8.0, 8.0]   # Proportional gain
24          kd: [3.0, 3.0, 3.0]   # Derivative gain
25          ki: [0.2, 0.2, 0.1]   # Integral gain
26          kff: [1.0, 1.0, 1.0]  # Feed-forward gain
27          min_output: [-20.0, -20.0, -20.0] # Minimum output of each PID
28          max_output: [ 20.0,  20.0,  20.0]
29      MellingerController:
30        publishers:
31          control_attitude: "fmu/in/force/attitude"
32          control_attitude_rate: "fmu/in/force/attitude_rate"
33          debug_topic: "autopilot/statistics/mellinger"
34        gains:
35          kp: [10.0, 10.0, 10.0]    # Proportional gain
36          kd: [9.0, 9.0, 9.0]    # Derivative gain
37          ki: [0.2, 0.2, 0.1]    # Integral gain
38          kr: [5.0, 5.0, 5.0]    # Attitude error gain
39          min_output: [-100.0, -100.0, -100.0]  # Minimum output of each PID
40          max_output: [ 100.0,  100.0,  100.0]  # Maximum output of each PID

41      # ----------------------------------------------------------------------------------------------------------
42      # Definition of the geofencing mechanism that will keep the vehicle in safe places
43      # ----------------------------------------------------------------------------------------------------------
44      geofencing: "BoxGeofencing"
45      BoxGeofencing:
46        limits_x: [-10.0, 10.0]
47        limits_y: [-10.0, 10.0]
48        limits_z: [-10.0,  1.0] # NED Coordinades (z-negative is up)

49      # ----------------------------------------------------------------------------------------------------------
50      # Definition of the trajectory manager that generates parameterized trajectories to be followed
51      # ----------------------------------------------------------------------------------------------------------
52      trajectory_manager: "StaticTrajectoryManager"
53      StaticTrajectoryManager:
54        trajectories: ["ArcFactory", "LineFactory", "CircleFactory", "LemniscateFactory", "CSVFactory"]
55        services:
56          reset_trajectory: "autopilot/trajectory/reset"
57        # Individual trajectory setup
58        ArcFactory:
59          service: "autopilot/trajectory/add_arc"
60        LineFactory:
61          service: "autopilot/trajectory/add_line"
62        CircleFactory:
63          service: "autopilot/trajectory/add_circle"
64        LemniscateFactory:
65          service: "autopilot/trajectory/add_lemniscate"
66        CSVFactory:
67          service: "autopilot/trajectory/add_csv"

 68      # ---------------------------------------------------------------------------------------------------------
 69      # Define the default operation mode (the one which the autopilot initializes at)
 70      # ---------------------------------------------------------------------------------------------------------
 71      default_mode: "DisarmMode"
 72      # Define all the existing operation modes
 73      modes: ["DisarmMode", "ArmMode", "TakeoffMode", "LandMode", "HoldMode", "WaypointMode", "FollowTrajectoryMode", "PassThroughMode"]
 74      # Configurations of each operating mode:
 75      # 1) Define the valid transitions from a given operation mode to other operation modes
 76      # 2) Fallback mode if something goes wrong
 77      # 3) Other specific operating mode configuration
 78      DisarmMode: 
 79        valid_transitions: ["ArmMode"]
 80        fallback: "DisarmMode"
 81        disarm_service: "fmu/kill_switch"
 82      ArmMode: 
 83        valid_transitions: ["DisarmMode", "TakeoffMode", "HoldMode", "WaypointMode", "FollowTrajectoryMode", "PassThroughMode"]
 84        fallback: "DisarmMode"
 85        geofencing_violation_fallback: "DisarmMode"
 86        arm_service: "fmu/arm"
 87        offboard_service: "fmu/offboard"
 88      TakeoffMode: 
 89        valid_transitions: ["LandMode", "HoldMode", "WaypointMode", "FollowTrajectoryMode", "PassThroughMode"]
 90        fallback: "HoldMode"
 91        on_finish: "HoldMode"
 92        geofencing_violation_fallback: "HoldMode"
 93        takeoff_altitude: -1.0 # m (NED)
 94        set_takeoff_altitude_service: "autopilot/set_takeoff"
 95      LandMode: 
 96        valid_transitions: ["DisarmMode", "ArmMode", "TakeoffMode", "HoldMode", "WaypointMode", "FollowTrajectoryMode", "PassThroughMode"]
 97        fallback: "HoldMode"
 98        on_finish: "DisarmMode"
 99        land_speed: 0.2 # m/s
100        land_detected_treshold: 0.1 # m/s