Create a Controller Plugin

A Controller plugin converts the desired_state_ variable and the current state_ variable into actuator inputs that drive the motion model’s dynamics. Currently, there isn’t an automated script to create a Controller plugin, so if you need to create a controller plugin, you will need to manually create files and name them appropriately. This will be fixed in the near future. Currently, Controller plugins are directly associated with Motion Model plugins, so by convention, Controller plugins are placed in the corresponding Motion model’s directory. (e.g., SimpleAircraftControllerPID is placed under the motion/SimpleAircraft directory).

Examine Controller Plugin

Let’s take a look at the SimpleAircraftControllerPID Controller plugin. The SimpleAircraftControllerPID controls the thrust, roll, pitch, and yaw of the SimpleAircraft in order to reach the desired speed, altitude, and heading set by the Autonomy plugin. The SimpleAircraftControllerPID plugin makes use of SCRIMMAGE’s built-in PID controller. The init function sets the PID gains with the help of a helper function, set_pid:

 1void set_pid(sc::PID &pid, std::string str, bool is_angle) {
 2    std::vector<std::string> str_vals;
 3    boost::split(str_vals, str, boost::is_any_of(","));
 4
 5    if (str_vals.size() != 4) {
 6        std::cout << "error parsing in SimpleAircraftControllerPID" << std::endl;
 7    } else {
 8        double p = std::stod(str_vals[0]);
 9        double i = std::stod(str_vals[1]);
10        double d = std::stod(str_vals[2]);
11        pid.set_parameters(p, i, d);
12
13        if (is_angle) {
14            double i_lim = sc::Angles::deg2rad(std::stod(str_vals[3]));
15            pid.set_integral_band(i_lim);
16            pid.set_is_angle(true);
17        } else {
18            double i_lim = std::stod(str_vals[3]);
19            pid.set_integral_band(i_lim);
20        }
21    }
22}
23
24void SimpleAircraftControllerPID::init(std::map<std::string, std::string> &params) {
25    set_pid(heading_pid_, params["heading_pid"], true);
26    set_pid(alt_pid_, params["alt_pid"], false);
27    set_pid(vel_pid_, params["vel_pid"], false);
28    u_ = std::make_shared<Eigen::Vector3d>();
29}

The plugin’s XML file contains the actual PID gain values:

<params>

...

<heading_pid>0.2, 0.01, 0.001, 9</heading_pid>
<alt_pid>0.0025, 0.0001, 0.0002, 1</alt_pid>
<vel_pid>1, 0.1, 0, 1</vel_pid>

...

</params>

The Controller’s step method merely assigns the new setpoints coming from the Autonomy plugin to each PID controller, tells each PID the current value of the state variable being tracked, and tells each PID to update itself. The PID output values are assigned to the vars_ variable available to all SCRIMMAGE plugins, which is retrieved later by the Motion Model plugin when it runs.

 1bool SimpleAircraftControllerPID::step(double t, double dt) {
 2    heading_pid_.set_setpoint(vars_.input(input_roll_or_heading_idx_));
 3    double u_roll_rate = use_roll_ ?
 4        -heading_pid_.step(dt, state_->quat().roll()) :
 5        heading_pid_.step(dt, state_->quat().yaw());
 6
 7    alt_pid_.set_setpoint(vars_.input(input_altitude_idx_));
 8    double u_pitch_rate = -alt_pid_.step(dt, state_->pos()(2));
 9
10    vel_pid_.set_setpoint(vars_.input(input_velocity_idx_));
11    double u_throttle = vel_pid_.step(dt, state_->vel().norm());
12
13    vars_.output(output_roll_rate_idx_, u_roll_rate);
14    vars_.output(output_pitch_rate_idx_, u_pitch_rate);
15    vars_.output(output_throttle_idx_, u_throttle);
16    return true;
17}