Controller

The controller module is used for all processes that filter signals for the purpose of controlling state variables

class DigitalFilter

Class for digital filtering. Any discrete filter can be implemented, including FIR and IIR. Typical use cases are for PID controllers, and frequency selective filters.

Example Usage:

// Create a simple integrator filter
std::vector<double> num = {0.0, 1.0};
std::vector<double> den = {1.0, -1.0};
flyMS::DigitalFilter filt(num, den);
 for (auto i = 0; i < 50; i++) {
   if (i == filt.update_filter(1.)) {
     std::cout << "Integrator works!" << std::endl;
   } else {
     std::cout << "Integrator failed!" << std::endl;
 }

Public Functions

DigitalFilter(const std::vector<double> &numerator, const std::vector<double> &denominator)

Construct a new Digital Filter object.

Parameters:

numerator – Numerator coefficients. Length must match denominator
denominator – Denominator coefficients. Leading term should be 1, the filter will normalize itself otherwise

DigitalFilter() = default: Construct a new Digital Filter object.

void init(const std::vector<double> &numerator, const std::vector<double> &denominator)

Initialize the object if created with default ctor.

Parameters:

numerator – Numerator coefficients. Length must match denominator
denominator – Denominator coefficients. Leading term should be 1, the filter will normalize itself otherwise

double update_filter(double val)

Increment the filter and return the output.

Parameters:: val – Input value
Returns:: double Filtered value

void zero_values(): Zeros the values in the filter.

void set_to_value(double val)

Sets all the values of the filtered and unfiltered output states to a value. Helpful to avoid step inputs.

Parameters:: val –

class Setpoint

Setpoint class. Communicates with the remote control (using robotics_cape rc_dsm interface) to receive commands. Commands are: throttle, roll, pitch, yaw, kill_switch, and Aux_switch. Switches have binary information.

Public Functions

Setpoint(FlightMode flight_mode, std::array<float, 3> max_setpts_stabilized, std::array<float, 3> max_setpts_acro, std::array<float, 2> throttle_limits)

Construct a new Setpoint object.

Parameters:

flight_mode – flight mode: stabilized or acro
max_setpts_stabilized – max setpoints for stabilized mode (in RPW rad, rad, rad/s)
max_setpts_acro – max setpoints for acro mode (in RPW rad/s, rad/s, rad/s)
throttle_limits – The min (index 0) and max (index 1) throttle values between 0-1

Setpoint(FlightMode flight_mode, const YAML::Node &config_params)

Construct a new Setpoint object using a yaml node. The node must have all the parameters needed in the main constructor.

Parameters:: config_params – The yaml node with config params

~Setpoint(): Destroy the Setpoint object.

Setpoint(Setpoint&&) = default: Construct a new Setpoint object using the move constructor.

Setpoint &operator=(Setpoint&&) = default

Construct a new Setpoint object using the move assignment operator.

Returns:: Setpoint&

void set_yaw_ref(float ref)

Set the Yaw Setpoint value to a user defined offset.

Parameters:: ref – the reference value to set

inline float get_min_throttle() const

Get the Min Throttle.

Returns:: float

class AttitudeController

Execute the PID controllers for the system, and convert the system’s measured state and desired state to control commands to be executed by the motors.

Public Functions

explicit AttitudeController(const AttitudeControllerConfig &config)

Construct a new Attitude Controller object.

Parameters:: config – The configuration parameters for the AttitudeController

explicit AttitudeController(const YAML::Node &controller_params)

Construct a new Attitude Controller object with a YAML::Node.

Parameters:: controller_params – The configuration parameters for the AttitudeController in YAML::Node format

TRPY<float> calculate_control_loop(const TRPY<float> &setpoints, const StateData &imu_data_body, FlightMode flight_mode)

Calculate the control loop for one time step. The input is the current measured state of the system, and the desired state of the system, the output is the control commands. Depending on the flight mode of the system, the ‘setpointscan mean different things. In stabilized flight mode, thesetpointsfor roll and pitch are the desired angle in radians. In acro mode, thesetpoints` for roll and pitch are the desired angular velocity in radians/s. Yaw is always controlled as angular velocity (radians/s)

Parameters:

setpoints – The desired system state
imu_data_body – The measured system state
flight_mode – The current flight mode of the system

Returns:

TRPW<float> The calculated control input

void zero_pids(): Zeros the signals for all internal PID filters.

class PositionController

The position controller object is responsible for generating roll/pitch/yaw commands given the estimated XYZ position, and the desitred XYZ position. It uses an outer/inner loop structure to control the drone’s position in 3D space. All member functions of this object are thread-safe.

Public Functions

PositionController(std::array<std::array<double, 3>, 2> pid_coeffs_x, std::array<std::array<double, 3>, 2> pid_coeffs_y, std::array<std::array<double, 3>, 2> pid_coeffs_z, std::array<float, 3> RPY_saturation_limits)

Construct a new Position Controller object. The PID constants used in this constructor are 2 arrays of 3 values each. The first array is the output-loop PID constants and the second array is the inner loop PID constants.

Parameters:

pid_coeffs_x – PID coefficients for controlling the X direction
pid_coeffs_y – PID coefficients for controlling the Y direction
pid_coeffs_z – PID coefficients for controlling the Z direction
RPY_saturation_limits – The maximum commanded roll/pitch/yaw values

PositionController(const YAML::Node &config_params)

Construct a new Position Controller object using a YAML::Node.

Parameters:: config_params – A yaml node containing all of the parameters found in the main constructors

~PositionController(): Destroy the Position Controller object.

void ReceiveVio(const VioData &vio)

Receive Visual-Inertial-Odometry information (the XYZ position)

Parameters:: vio – The estimated visual inertial odometry object

std::tuple<Eigen::Vector3f, float> GetSetpoint() const

Gets the current setpoint state and current yaw position.

Returns:: std::tuple<Eigen::Vector3f, float>. The roll/pitch/yaw setpoint, and the currnet yaw position

void SetReferencePosition(const Eigen::Vector3f &position)

Set the Reference Position. This is the ‘commanded’ or ‘desired’ position we want to be in.

Parameters:: position – The reference position in XYZ coordinates

void ResetController(): Resets the values and filters used in the object.s.