Control Libraries¶

This section covers the control libraries for PID controllers, attitude control, and waypoint navigation.

AC_PID (PID Controller)¶

Header: libraries/AC_PID/AC_PID.h

Inheritance: None (top-level class)

Description: Generic PID (Proportional-Integral-Derivative) controller implementation. Used throughout ArduPilot for rate control, position control, and many other control loops.

Key Features:

P, I, D, FF (Feedforward) terms
Configurable filter frequencies
Integral windup protection
Slew rate limiting

Detailed Documentation:

AC_PID Detailed Reference - Detailed AC_PID API Reference

Class Definition:

class AC_PID

Copter PID control class.

Subclassed by AC_HELI_PID

Public Functions

AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float initial_srmax = 0, float initial_srtau = 1.0, float initial_dff = 0): Constructor for PID controller with EEPROM-backed gain. Parameters are initialized from defaults or EEPROM at runtime.

struct ac_pid_flags

struct Defaults

Constructor:

AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float initial_srmax, float initial_srtau, float initial_dff)

Constructor with all parameters.

Parameters:

initial_p – Initial proportional gain (P)
initial_i – Initial integral gain (I)
initial_d – Initial derivative gain (D)
initial_ff – Initial feedforward gain (FF)
initial_imax – Initial integral limit (imax)
initial_filt_T_hz – Initial derivative filter frequency (Hz)
initial_filt_E_hz – Initial error filter frequency (Hz)
initial_filt_D_hz – Initial derivative input filter frequency (Hz)
initial_srmax – Slew rate maximum
initial_srtau – Slew rate time constant
initial_dff – Initial derivative feedforward gain

Main Control Methods:

float AC_PID::update_all(float target, float measurement, float dt)¶

Run full PID update combining P, I, D terms.

Parameters:

target – Target value (setpoint)
measurement – Measured value (feedback)
dt – Time delta since last update in seconds

Returns:

Control output (sum of P + I + D + FF)

float AC_PID::update_error(float error, float dt)¶

Run PID with error input (target - measurement).

Parameters:

error – Error (target - measurement)
dt – Time delta in seconds

Returns:

Control output

void AC_PID::reset_I(): Reset integral term to zero. Call when changing setpoints or entering new flight mode.

void AC_PID::reset_I(float value)¶

Reset integral term to specific value.

Parameters:: value – Initial integral value

Gain Setting Methods:

void AC_PID::set_kP(float v)

Set proportional gain.

Parameters:: v – Proportional gain value

void AC_PID::set_kI(float v)

Set integral gain.

Parameters:: v – Integral gain value

void AC_PID::set_kD(float v)

Set derivative gain.

Parameters:: v – Derivative gain value

void AC_PID::set_kFF(float v)¶

Set feedforward gain.

Parameters:: v – Feedforward gain value

void AC_PID::set_imax(float v)

Set integral limit (maximum absolute integral term).

Parameters:: v – Maximum integral value

void AC_PID::set_filt_T(float hz)¶

Set derivative filter time constant.

Parameters:: hz – Filter frequency in Hz

Gain Getting Methods:

float AC_PID::get_p() const

Get proportional term from last update.

Returns:: P term value

float AC_PID::get_i() const

Get integral term from last update.

Returns:: I term value

float AC_PID::get_d() const

Get derivative term from last update.

Returns:: D term value

float AC_PID::get_ff() const

Get feedforward term from last update.

Returns:: FF term value

float AC_PID::get_error() const¶

Get last error value.

Returns:: Error (target - measurement)

Usage Example:

AC_PID pid;
pid.set_kP(1.0f);
pid.set_kI(0.5f);
pid.set_kD(0.1f);
pid.set_kFF(0.0f);
pid.set_imax(1000.0f);

// In control loop (e.g., 100Hz)
float target = 100.0f;
float measurement = 80.0f;
float dt = 0.01f;  // 10ms
float output = pid.update_all(target, measurement, dt);

// When changing modes or setpoints
pid.reset_I();

Related Classes:

AC_PID_2D
AC_PI
AC_P

AC_AttitudeControl (Attitude Controller)¶

Header: libraries/AC_AttitudeControl/AC_AttitudeControl.h

Inheritance: Inherits from :doxygen:`AC_AttitudeControl_Backend`

Description: Attitude control for multirotor aircraft. Provides rate control and angle control for roll, pitch, and yaw axes.

Key Features:

Rate control (100Hz+)
Angle control with quaternion support
Attitude feedback control
Throttle control

Class Definition:

class AC_AttitudeControl¶

Subclassed by AC_AttitudeControl_Heli, AC_AttitudeControl_Multi, AC_AttitudeControl_Sub

struct HeadingCommand¶

Constructor:

AC_AttitudeControl::AC_AttitudeControl(AP_AHRS &ahrs, AP_Motors &motors)¶

Constructor.

Parameters:

ahrs – Reference to AHRS instance
motors – Reference to Motors instance

Main Control Methods:

void AC_AttitudeControl::set_attitude_target(const Quaternion &att_target)¶

Set target attitude as quaternion.

Parameters:: att_target – Target quaternion (w, x, y, z)

void AC_AttitudeControl::set_attitude_target_euler(float roll, float pitch, float yaw)¶

Set target attitude as euler angles.

Parameters:

roll – Target roll angle in radians
pitch – Target pitch angle in radians
yaw – Target yaw angle in radians

void AC_AttitudeControl::rate_controller_run()¶: Run rate controllers (main control loop). Must be called at 100Hz+. Reads pilot input and runs rate PID controllers.

void AC_AttitudeControl::angle_controller_run()¶: Run angle controllers. Converts angle targets to rate targets and runs rate controllers.

Rate Control Methods:

void AC_AttitudeControl::set_rate_roll(float rate_rads)¶

Set roll rate target.

Parameters:: rate_rads – Target rate in rad/s

void AC_AttitudeControl::set_rate_pitch(float rate_rads)¶

Set pitch rate target.

Parameters:: rate_rads – Target rate in rad/s

void AC_AttitudeControl::set_rate_yaw(float rate_rads)¶

Set yaw rate target.

Parameters:: rate_rads – Target rate in rad/s

Angle Control Methods:

void AC_AttitudeControl::set_roll(float roll_rad)¶

Set roll angle target.

Parameters:: roll_rad – Target roll in radians

void AC_AttitudeControl::set_pitch(float pitch_rad)¶

Set pitch angle target.

Parameters:: pitch_rad – Target pitch in radians

void AC_AttitudeControl::set_yaw(float yaw_rad)¶

Set yaw angle target.

Parameters:: yaw_rad – Target yaw in radians

void AC_AttitudeControl::set_throttle(float throttle)¶

Set motor throttle (0-1 range).

Parameters:: throttle – Throttle value (0 to 1)

Specialized Classes:

AC_AttitudeControl_Multi (Multicopter)
AC_AttitudeControl_Heli (Helicopter)
AC_AttitudeControl_Sub (Submarine)

Usage Example:

AC_AttitudeControl *attitude = AC_AttitudeControl::get_singleton();

// Method 1: Rate control
attitude->set_rate_roll(input.roll);
attitude->set_rate_pitch(input.pitch);
attitude->set_rate_yaw(input.yaw);
attitude->set_throttle(input.throttle);
attitude->rate_controller_run();

// Method 2: Angle control
attitude->set_attitude_target_euler(roll, pitch, yaw);
attitude->angle_controller_run();

// Get current attitude
Vector3f euler = attitude->get_roll_pitch_yaw();

AC_WPNav (Waypoint Navigation)¶

Header: libraries/AC_WPNav/AC_WPNav.h

Inheritance: None (top-level class)

Description: Waypoint navigation for autonomous missions. Handles path planning and following for autonomous flight.

Key Features:

Waypoint tracking
Path smoothing
Loiter at waypoints
Takeoff/Landing commands

Class Definition:

class AC_WPNav¶

Public Functions

AC_WPNav(const AP_AHRS_View &ahrs, AC_PosControl &pos_control, const AC_AttitudeControl &attitude_control)¶: Constructor.

void wp_and_spline_init_m(float speed_ms = 0.0f, Vector3p stopping_point_ned_m = Vector3p{})¶

waypoint controller

waypoint navigation

bool set_spline_destination_loc(const Location &destination, const Location &next_destination, bool next_is_spline)¶: spline methods

inline float get_roll_rad() const¶: shared methods

struct wpnav_flags¶

Main Navigation Methods:

bool AC_WPNav::update_wpnav()¶

Update waypoint navigation. Main control function.

Returns:: true if navigating

void AC_WPNav::set_wp_destination(const Location &loc)¶

Set waypoint destination.

Parameters:: loc – Target Location (lat, lon, alt)

bool AC_WPNav::get_wp_destination(Location &loc)¶

Get current waypoint destination.

Parameters:: loc – Output location
Returns:: true if destination is set

bool AC_WPNav::is_wp_reached()¶

Check if waypoint reached.

Returns:: true if current waypoint reached

bool AC_WPNav::is_navigating()¶

Check if currently navigating.

Returns:: true if navigating to waypoint

Distance and Bearing:

float AC_WPNav::get_wp_distance_to_destination()¶

Get distance to waypoint.

Returns:: Distance in meters

float AC_WPNav::get_wp_bearing_to_destination()¶

Get bearing to waypoint.

Returns:: Bearing in degrees (0-360)

Usage Example:

AC_WPNav *wpnav = AC_WPNav::get_singleton();

// Set destination
Location target;
target.lat = 123456789;  // microdegrees
target.lng = 987654321;
target.alt = 10000;     // cm above home
wpnav->set_wp_destination(target);

// In main loop
if (wpnav->update_wpnav()) {
    // Still navigating
}

// Check progress
float distance = wpnav->get_wp_distance_to_destination();
float bearing = wpnav->get_wp_bearing_to_destination();

if (wpnav->is_wp_reached()) {
    // Waypoint reached, advance to next
}

AC_Loiter (Loiter Controller)¶

Header: libraries/AC_WPNav/AC_Loiter.h

Inheritance: Part of AC_AttitudeControl

Description: Position hold controller using GPS. Maintains hover position using position and velocity control.

Class Definition:

class AC_Loiter¶

Public Functions

AC_Loiter(const AP_AHRS_View &ahrs, AC_PosControl &pos_control, const AC_AttitudeControl &attitude_control)¶: Constructor.

Main Control Methods:

void AC_Loiter::init()¶: Initialize loiter controller.

void AC_Loiter::pos_control_start()¶: Start position control.

void AC_Loiter::update()¶: Update loiter controller. Main control function.

Target Setting:

void AC_Loiter::set_desired_position(float x, float y, float z)¶

Set desired position.

Parameters:

x – North position in meters
y – East position in meters
z – Altitude in meters

void AC_Loiter::set_desired_velocity(Vector2f velocity_ef)¶

Set desired velocity.

Parameters:: velocity_ef – Target velocity

State Checking:

bool AC_Loiter::is_active()¶

Check if loiter is active.

Returns:: true if active

Speed Control:

void AC_Loiter::set_speed_xy(float speed_cms)¶

Set horizontal speed limit.

Parameters:: speed_cms – Speed in cm/s

void AC_Loiter::set_speed_z(float speed_cms)¶

Set vertical speed limit.

Parameters:: speed_cms – Speed in cm/s

Usage Example:

AC_Loiter *loiter = AC_Loiter::get_singleton();

// Start loiter at current position
loiter->init();
loiter->pos_control_start();

// In main loop
loiter->update();

// Check if active
if (loiter->is_active()) {
    // Currently holding position
}

// Set new target position
loiter->set_desired_position(x, y, z);

AC_PosControl (Position Control)¶

Header: libraries/AC_AttitudeControl/AC_PosControl.h

Inheritance: Part of AC_AttitudeControl

Description: Position control for vehicles.

Class Definition:

class AC_PosControl¶

Public Functions

AC_PosControl(AP_AHRS_View &ahrs, const class AP_Motors &motors, AC_AttitudeControl &attitude_control)¶: Constructor.

void update_estimates(bool high_vibes = false)¶: System methods

void input_pos_NED_m(const Vector3p &pos_ned_m, float pos_terrain_target_d_m, float terrain_margin_m)¶: 3D position shaper

void NE_set_max_speed_accel_cm(float speed_cms, float accel_cmss)¶: Lateral position controller

void D_set_max_speed_accel_cm(float decent_speed_max_cms, float climb_speed_max_cms, float accel_max_u_cmss)¶: Vertical position controller

inline float get_max_speed_down_ms() const¶: Returns maximum descent rate in m/s (zero or positive).

void set_pos_vel_accel_NED_m(const Vector3p &pos_ned_m, const Vector3f &vel_ned_ms, const Vector3f &accel_ned_mss)¶: Accessors

inline const Vector3p &get_pos_estimate_NED_m() const¶: Position.

void get_stopping_point_NE_m(Vector2p &stopping_point_ne_m) const¶: Stopping Point.

inline const Vector3f get_pos_error_NED_m() const¶: Position Error.

inline const Vector3f &get_vel_estimate_NED_ms() const¶: Velocity.

inline void set_accel_desired_NE_mss(const Vector2f &accel_desired_ne_mss)¶: Acceleration.

inline void set_pos_terrain_target_U_cm(float pos_terrain_target_u_cm)¶: Terrain.

void set_posvelaccel_offset_target_NE_m(const Vector2p &pos_offset_target_ne_m, const Vector2f &vel_offset_target_ne_ms, const Vector2f &accel_offset_target_ne_mss)¶: Offset.

inline float get_roll_rad() const¶: Outputs.

float get_lean_angle_max_rad() const¶: Accessors

inline AC_P_2D &NE_get_pos_p()¶: Other.

Protected Functions

void init_terrain()¶

Terrain Following.

Terrain.

void update_terrain()¶

Terrain.

private methods

void NE_init_offsets()¶: Offsets.