Controller Interface

The interface for the autopilot controller is defined in the autopilot package under the pegasus_autopilot/autopilot/include/autopilot/controller.hpp header file. The Controller class is an abstract class that defines the interface for the autopilot controller.

Each autopilot controller must inherit from the Controller class and implement the following one or multiple methods highlighted in yellow in the code snippet below:

  1namespace autopilot {
  2
  3class Controller {
  4
  5public: 
  6
  7    using SharedPtr = std::shared_ptr<Controller>;
  8    using UniquePtr = std::unique_ptr<Controller>;
  9    using WeakPtr = std::weak_ptr<Controller>;
 10
 11    // Configuration for the operation mode
 12    struct Config {
 13        rclcpp::Node::SharedPtr node;                                           // ROS 2 node ptr (in case the mode needs to create publishers, subscribers, etc.)
 14        std::function<State()> get_vehicle_state;                               // Function pointer to get the current state of the vehicle      
 15        std::function<VehicleStatus()> get_vehicle_status;                      // Function pointer to get the current status of the vehicle  
 16        std::function<VehicleConstants()> get_vehicle_constants;                // Function pointer to get the current dynamical constants of the vehicle    
 17    };
 18
 19    inline void initialize_controller(const Controller::Config & config) {
 20
 21        // Initialize the base class
 22        node_ = config.node;
 23        get_vehicle_state = config.get_vehicle_state;
 24        get_vehicle_status = config.get_vehicle_status;
 25        get_vehicle_constants = config.get_vehicle_constants;
 26
 27        // Initialize the derived class
 28        initialize();
 29    }
 30
 31    /**
 32     * @brief Pure virtual function that must be implemented by the derived class to initialize the controller
 33    */
 34    virtual void initialize() = 0;
 35
 36    /**
 37     * @brief Method that can be used to reset certain parameters of the controller.
 38     * This method is called by the autopilot when the vehicle is switched to the disarm mode. If not implemented, it does nothing.
 39     */
 40    virtual void reset_controller() {};
 41
 42    /** 
 43     * @brief Sets the target position of the vehicle in the inertial frame and the target yaw (in degres)
 44     * @param position The target position in the inertial frame
 45     * @param yaw The target yaw in degrees
 46    */
 47   virtual void set_position(const Eigen::Vector3d & position, double yaw, double dt) {
 48        set_position(position, yaw, 0.0, dt);
 49    }
 50
 51    virtual void set_position(const Eigen::Vector3d& position, double yaw, double yaw_rate, double dt) {
 52        set_position(position, Eigen::Vector3d::Zero(), yaw, yaw_rate, dt);
 53    }
 54
 55    virtual void set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, double yaw, double yaw_rate, double dt) {
 56        set_position(position, velocity, Eigen::Vector3d::Zero(), yaw, yaw_rate, dt);
 57    }
 58
 59    virtual void set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration, double yaw, double yaw_rate, double dt) {
 60        set_position(position, velocity, acceleration, Eigen::Vector3d::Zero(3), yaw, yaw_rate, dt);
 61    }
 62
 63    virtual void set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, double yaw, double yaw_rate, double dt) {
 64        set_position(position, velocity, acceleration, jerk, Eigen::Vector3d::Zero(3), yaw, yaw_rate, dt);
 65    }
 66
 67    virtual void set_position(const Eigen::Vector3d& position, const Eigen::Vector3d& velocity, const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, const Eigen::Vector3d& snap, double yaw, double yaw_rate, double dt) {
 68        throw std::runtime_error("set_position() not implemented in derived class");
 69    }
 70
 71    /**
 72     * @brief Sets the target velocity of the vehicle in the inertial frame and the target yaw rate (in degres/s)
 73     * @param velocity The target velocity in the inertial frame (m/s NED)
 74     * @param yaw_rate The target yaw in degrees
 75    */
 76    virtual void set_inertial_velocity(const Eigen::Vector3d& velocity, double yaw, double dt=0) {
 77        throw std::runtime_error("set_inertial_velocity() not implemented in derived class");
 78    }
 79
 80    /**
 81     * @brief Sets the target position of the vehicle in the body frame (rotated to be vertically aligned with NED) and the target yaw (in degres)
 82     * @param position The target position in the body frame (m/s f.r.d)
 83     * @param yaw The target yaw-rate in degrees/s
 84    */
 85    virtual void set_body_velocity(const Eigen::Vector3d& velocity, double yaw_rate, double dt=0) {
 86        throw std::runtime_error("set_body_velocity() not implemented in derived class");
 87    }
 88
 89    /**
 90     * @brief Set the inertial acceleration (Ax, Ay, Az) (m/s^2) of the vehicle. The adopted frame is NED 
 91     * @param acceleration The target acceleration in the inertial frame (NED)
 92     */
 93    virtual void set_inertial_acceleration(const Eigen::Vector3d& acceleration, double dt=0) {
 94        throw std::runtime_error("set_acceleration() not implemented in derived class");
 95    }
 96
 97    /**
 98     * @brief Sets the target attitude of the vehicle in the body frame and the target thrust force
 99     * @param attitude The target attitude in the body frame (in degrees)
100     * @param thrust_force The target thrust force (in Newton)
101    */
102    virtual void set_attitude(const Eigen::Vector3d& attitude, double thrust_force, double dt=0) {
103        throw std::runtime_error("set_attitude() not implemented in derived class");
104    }
105
106    /**
107     * @brief Sets the target attitude rate of the vehicle in the body frame and the target thrust force
108     * @param attitude_rate The target attitude rate in the body frame (in degrees/s)
109     * @param thrust_force The target thrust force (in Newton)
110    */
111    virtual void set_attitude_rate(const Eigen::Vector3d& attitude_rate, double thrust_force, double dt=0) {
112        throw std::runtime_error("set_attitude_rate() not implemented in derived class");
113    }
114
115    /**
116     * @brief Sets the target motor speed of the vehicle (from 0-100%)
117    */
118    virtual void set_motor_speed(const Eigen::VectorXd& motor_velocity, double dt=0) {
119        throw std::runtime_error("set_motor_velocity() not implemented in derived class");
120    }
121
122protected:
123
124    // The ROS 2 node
125    rclcpp::Node::SharedPtr node_{nullptr};

The methods that can be implemented are:

  • initialize: This method is called once when the controller is initialized. It is used to set the controller parameters.

  • reset_controller: This method is called when the controller is reset. It is used to reset the controller internal state (for example the integral term).

  • set_position: Set the desired position + yaw and yaw-rate (in deg, deg/s) to track (in the inertial frame in NED).

  • set_velocity: Set the desired velocity to track (in the inertial frame in NED).

  • set_body_velocity: Set the desired velocity to track (in the body frame in FRD).

  • set_attitude: Set the desired attitude (in deg for a FRD body frame relative to a NED inertial frame) and total thrust (in Newton) to track.

  • set_attitude_rate: Set the desired angular velocity to track (in deg/s for a FRD body frame relative to a NED inertial frame) and total thrust (in Newton) to track.

  • set_motor_speed: Set the individual desired motor speed (0-100%)

Note that you do not need to implement all of these methods, only the ones that are necessary for your controller. However, keep in mind that all the autopilot modes can call any of these methods. If an autopilot mode calls a method that is not implemented:

  1. A runtime exception will be thrown

  2. The autopilot will switch to the failsafe mode (if available)

  3. If the failsafe mode also calls a method that is not implemented, you are screwed.

In practice, most operation modes will call the set_position method to set the desired position to track. As a good rule of thumb you should always implement the most generic version of this method that receives references up to the snap (even if you do not make use these higher-order derivatives in your controller).