Control Loops

This page presents the default control loops used by the Paparazzi airborne code for navigation, guidance and control.

General Information

All the possible combinations of control loops might not be fully detailed.

In the following diagrams, the block s is used for the derivative function and 1/s is used for the integrator function.

The variables' names are the one used in the airborne code (written in C). Most of this name can the used capitalized in the Airframe Configuration file in order to define the default value of these variables. If in capital letters in the diagrams, the value is fixed and cannot be changed using Settings mechanism.

Fixed-wing autopilot

Global view

The elements servos, rc_commands, commands and command_laws correspond to specific section of the Airframe Configuration file. Most of the code located in these blocks is generated from the xml of th configuration file.

The value +/-9600 correspond to +/-MAX_PPRZ. This pprz unit is used as a normalized internal unit for input and output values of the control_laws block.

Navigation loop

The navigation loop is located in sw/airborne/subsystems/nav.*. The navigation routines are called from the Flight Plans. Advanced navigation routines are in sw/airborne/subsystems/navigation/.

Course loop

The course loop is the upper stage of the horizontal control. It is located in sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c (formerly fw_h_ctl.c).

Roll loop

The roll loop is the lower stage of the horizontal control and is used for lateral attitude stabilization. It is located in stabilization_attitude.c. If H_CTL_ROLL_ATTITUDE_GAIN is undefined, the code will fallback to a attitude-only loop, where estimator_p is disregarded and the gain for estimator_phi is H_CTL_ROLL_PGAIN.

Altitude loop

The altitude loop is the upper stage of the vertical control. It is located in sw/airborne/firmwares/fixedwing/guidance/guidance_v.c (formerly fw_v_ctl.c) If AGR_CLIMB is defined in the airframe file, the altitude loop also sets the v_ctl_auto_throttle_submode for use in the climb loop.

Auto Throttle and Auto Pitch climb loops

Two climb loops are available. The are called from the flight plan by changing the vertical navigation mode. The default mode is Auto Throttle. The Auto Pitch loop is only available if V_CTL_AUTO_PITCH_PGAIN is defined. Note also that if USE_AIRSPEED is defined, then the Auto Throttle loop is replaced by the Auto Airspeed loop, see [Control_Loops#Control_loops_using_Airspeed_Sensor|below].

Auto throttle loop

If AGR_CLIMB is defined, then the auto throttle submode is used to determine setpoint outputs. This may be the standard control loop outputs, the aggressive outputs (static setpoints defined in airframe file) or a blend of both.

Auto pitch loop

The climb loop is the intermediate stage of the vertical control. It is located in guidance_v.c.

Pitch loop

The pitch loop is the lower stage of the vertical control and is used for longitudinal attitude stabilization. It is located in stabilization_attitude.c.

Control loops using Airspeed Sensor

Adding an airspeed sensor measures actual airspeed resulting in better throttle control and aircraft performance especially in windy conditions. The use of an airspeed sensor is described here.

The auto airspeed cascaded control loops control both pitch and throttle. This control loop set replaces the Auto Throttle climb loop; it is engaged in the same manner in the flight plan. It is located in guidance_v.c.

Multi-rotor (Booz) autopilot

The schemes of the multi-rotor autopilot were made with the drawing tool from Google docs. To edit the drawings, send an e-mail to microuav@gmail.com.

Global View

The code for this autopilot is located in sw/airborne/firmwares/rotorcraft.

Vertical Control

Depending on the general mode selected, one of the following vertical modes is used.

Reference generators

from z set point

from z dot set point

]

The adjust accel is a simple algorithm to set acceleration to zero when velocity reaches boundaries.

The code for these reference generators is located in guidance/guidance_v_ref.h

Hover loop

(coming soon...)

Horizontal Control

Depending on the general mode selected, one of the following horizontal modes is used.

mode: GUIDANCE_H_MODE_RATE

mode: GUIDANCE_H_MODE_ATTITUDE

mode: GUIDANCE_H_MODE_HOVER

When this mode is entered, the measured position at the instance of initiation is set as guidance_h_pos_sp. This means that the rotorcraft will hover on that position.

rotation to body:

$\phi _{cmd_{body}}=-\sin \psi \cdot x_{cmd_{earth}}+\cos \psi \cdot y_{cmd_{earth}}$

$\theta _{cmd_{body}}=-\left(\cos \psi \cdot x_{cmd_{earth}}+\sin \psi \cdot y_{cmd_{earth}}\right)$

mode: GUIDANCE_H_MODE_NAV

The set points guidance_h_pos_sp (x and y coordinates) are obtained from the navigation carrot after a 'NED of ENU' transformation:

guidance_h_pos_sp.x = navigation_carrot.y

guidance_h_pos_sp.y = navigation_carrot.x

Rotation to body is the same as in GUIDANCE_H_MODE_HOVER.

Reference generators

from euler attitude angle set point

Acceleration is set to zero when rate reaches min or max value.

The code for this reference generator is located in stabilization/stabilization_attitude_ref_euler_int.c.

from angular rate set point

The code for this reference generator is located in stabilization/stabilization_rate.c.

from pos set point

Acceleration is set to zero when rate reaches min or max value.

The code for this reference generator is located in guidance/guidance_h_ref.h.

Attitude loop

The code for this loop is located in stabilization/stabilization_attitude_euler_int.c.

Rate loop

The code for this loop is located in stabilization/stabilization_rate.c.

Guidance horizontal hover loop

The code for this loop is located in guidance/guidance_h.c.

Guidance horizontal navigation loop

The code for this loop is located in guidance/guidance_h.c

(to be developed later..)

Integral action or tracking algorithm:

if (|guidance_h_pos_err| < HOLD_DISTANCE) -> intregal error * guidance_h_igain

else -> path tracking error * guidance_h_ngain