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.
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.
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