Difference between revisions of "Control Loops"

From PaparazziUAV
Jump to: navigation, search
(Rate loop)
(Guidance horizontal navigation loop)
 
(39 intermediate revisions by 10 users not shown)
Line 3: Line 3:
 
= General Information =
 
= General Information =
  
All the possible combinations of control loops might not be fully detailed.
+
All the possible combinations of control loops might not be fully detailed. This is especially the case when using extra features such as '''AGR_CLIMB''', '''STRONG_WIND''', etc.
  
 
In the following diagrams, the block '''s''' is used for the '''derivative''' function and '''1/s''' is used for the '''integrator''' function.
 
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.
+
The variables' names are the ones used in the airborne code (written in '''C'''). Most of these can be used, all caps, in the [[Airframe Configuration]] file in order to define the default values of these variables. If you see all capital letters in the diagrams, the value is fixed and cannot be changed using [[Settings]] mechanism.
 +
 
 +
The most direct way to determine vehicle behavior is to look at the source code. When determining which control loops are engaged by various flight plan blocks and stages, it may be helpful to look at sw/airborne/subsystems/nav.h and the generated flightplan file in var/<AIRCRAFT_NAME>/generated/flight_plan.h.
  
 
= Fixed-wing autopilot =
 
= Fixed-wing autopilot =
Line 15: Line 17:
 
[[Image:Diagram_general.png|General overview]]
 
[[Image:Diagram_general.png|General overview]]
  
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 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 the 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.
 
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.
Line 21: Line 23:
 
== Navigation loop ==
 
== Navigation loop ==
  
The navigation loop is located in '''sw/airborne/nav.*'''. The navigation routines are called from the [[Flight Plans]].
+
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 ==
 
== Course loop ==
Line 36: Line 38:
 
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'''.
 
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 <tt>H_CTL_ROLL_ATTITUDE_GAIN</tt> is undefined, the code will fallback to a attitude-only loop, where <tt>estimator_p</tt> is disregarded and the gain for <tt>estimator_phi</tt> is <tt>H_CTL_ROLL_PGAIN</tt>.
 
If <tt>H_CTL_ROLL_ATTITUDE_GAIN</tt> is undefined, the code will fallback to a attitude-only loop, where <tt>estimator_p</tt> is disregarded and the gain for <tt>estimator_phi</tt> is <tt>H_CTL_ROLL_PGAIN</tt>.
 +
 +
Note; estimator_phi is the measured roll angle, and estimator_p is the measured rate of change in roll angle.
  
 
== Altitude loop ==
 
== Altitude loop ==
Line 43: Line 47:
 
The altitude loop is the upper stage of the vertical control.
 
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)
 
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 ==
 
== Auto Throttle and Auto Pitch climb loops ==
  
Two climb loops are available. The are called from the [[Flight Plans#Navigation_modes|flight plan]] by changing the vertical navigation mode. The default mode is '''Auto Throttle'''.
+
Two climb loops are available. The are called from the [[Flight Plans#Navigation_modes|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. Only one loop is active at a time. 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 ===
 
=== Auto throttle loop ===
 
[[Image:Diagram_auto_throttle_loop.png|Auto Throttle climb loop]]
 
[[Image:Diagram_auto_throttle_loop.png|Auto Throttle climb 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 ===
 
=== Auto pitch loop ===
[[Image:Diagram_auto_pitch_loop.png|Auto Pitch climb loop]]
+
[[Image:Diagram_auto_pitch_loop.png|924px|Auto Pitch climb loop]]
  
 
The climb loop is the intermediate stage of the vertical control. It is located in '''guidance_v.c'''.
 
The climb loop is the intermediate stage of the vertical control. It is located in '''guidance_v.c'''.
Line 61: Line 68:
  
 
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'''.
 
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'''.
 +
The first sum block of the diagram is not completely accurate. The input to the pitch stabilization loop is h_ctl_pitch_setpoint, though v_ctl_pitch_of_vz is not a direct input. Rather h_ctl_pitch_setpoint is assigned in the main autopilot loop from nav_pitch, in general (in auto1, it is assigned directly from the pitch r/c input). The nav_pitch value is obtained in a few ways, depending on which control loop(s) are active. If auto throttle is active, nav_pitch is the sum of v_ctl_pitch_of_vz and a fixed setpoint defined using the pitch attribute in the flightplan. If AGR_CLIMB is set, nav_pitch is either the airframe file defined ascent and descent pitch setpoints, the standard sum, or a blend of each. If auto pitch is active, nav_pitch is defined directly from the output of that loop. If airspeed is active, nav_pitch is defined in the auto airspeed pitch loop.
  
 
== Control loops using Airspeed Sensor ==
 
== 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 control loops using an airspeed sensor are described [[Airspeed_sensor|here]].
+
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 [[Airspeed_sensor|here]].
 +
 
 +
[[Image:Diagram_auto_airspeed_loop.png|Auto Airspeed climb loop]]
  
= Multi-rotor (Booz) autopilot =
+
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'''.
 +
 
 +
== Energy Control loops using Airspeed Sensor ==
 +
 
 +
Total Energy (speed + height) control for fixed wing vehicles developed by the [http://www.mavlab.info MAVLab] of the Technical [http://www.tudelft.nl University] of Delft. An airspeed sensor is mandatory.
 +
The energy control loops control both pitch and throttle. This control loop set replaces the Auto Throttle climb loop and is located in '''energy_ctrl.c'''.
 +
 
 +
[[Image:Energycontrol.png|Energy Control loop]]
 +
 
 +
= Rotorcraft 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.
 
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 ==
+
== Overview ==
  
 
[[Image:Booz_autopilot_globalview.png]]
 
[[Image:Booz_autopilot_globalview.png]]
  
 
The code for this autopilot is located in '''sw/airborne/firmwares/rotorcraft'''.
 
The code for this autopilot is located in '''sw/airborne/firmwares/rotorcraft'''.
 +
 +
Since v5.0, the Supervision was replaced by [[Rotorcraft_Configuration|Motor mixing]]. Alternatively, the stabilization commands can also be directly used in the [[Airframe_Configuration#Servos|servo command laws]].
  
 
== Vertical Control ==
 
== Vertical Control ==
Line 80: Line 101:
 
Depending on the general mode selected, one of the following vertical modes is used.
 
Depending on the general mode selected, one of the following vertical modes is used.
  
=== mode: GUIDANCE_V_MODE_RC_CLIMB ===
+
=== mode: GUIDANCE_V_MODE_RC_CLIMB (speed control) ===
 +
[[File:Rc_climb.png]]
  
=== mode: GUIDANCE_V_MODE_CLIMB ===
+
=== mode: GUIDANCE_V_MODE_CLIMB (speed control) ===
 +
[[File:Climb.png]]
  
=== mode: GUIDANCE_V_MODE_HOVER ===
+
=== mode: GUIDANCE_V_MODE_HOVER (altitude control) ===
 +
[[File:Z_hold.png]]
  
 
=== mode: GUIDANCE_V_MODE_NAV ===
 
=== mode: GUIDANCE_V_MODE_NAV ===
Line 90: Line 114:
 
== Reference generators ==
 
== Reference generators ==
  
=== from z set point ===
+
z relates to altitude, zd to speed and zdd to acceleration
 +
 
 +
=== from z set point (desired altitude) ===
  
 
[[Image:Refgen_from_z_sp.png]]
 
[[Image:Refgen_from_z_sp.png]]
  
=== from z dot set point ===
+
=== from z dot set point (desired speed) ===
  
 
[[Image:Booz_refgen_zdot.png]]]
 
[[Image:Booz_refgen_zdot.png]]]
Line 100: Line 126:
 
The '''adjust accel''' is a simple algorithm to set acceleration to zero when velocity reaches boundaries.
 
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'''
+
The code for these reference generators is located in '''guidance/guidance_v_ref.c'''
  
== Hover loop ==
+
== Vertical loop ==
 +
[[File:vertical_loop_V40.png]]
  
(coming soon...)
+
the source code is located in '''guidance\guidance_v.c'''
 +
 
 +
NOMINAL_HOVER_THROTTLE is the nominal thrust when hovering. It's calculated by default by an adaptative controller, in  '''guidance\guidance_v_adpt.h'''.
 +
 
 +
Instead of the adaptative controller, NOMINAL_HOVER_TROTTLE can be set manually in the Airframe configuration file with:
 +
 
 +
<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
 +
 
 +
The value is found after flying manually in hover: NOMINAL_HOVER_THROTTLE= THRUST/MAX_PPRZ (THRUST:mean value of ROTORCRAFT_FP THRUST message in hover, MAX_PPRZ:9600)  
 +
 
 +
In most cases it makes more sense to not specify this explicitly and use the adaptive controller
  
 
== Horizontal Control ==
 
== Horizontal Control ==
Line 159: Line 196:
  
 
[[Image:Booz_refgen_xy_sp.png]]
 
[[Image:Booz_refgen_xy_sp.png]]
 +
 +
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'''.
 
The code for this reference generator is located in '''guidance/guidance_h_ref.h'''.
Line 178: Line 217:
 
[[Image:guidance_horizontal_hover.png]]
 
[[Image:guidance_horizontal_hover.png]]
  
The code for this loop is located in '''guidance/guidance_h.c'''.
+
The code for this loop is located in '''guidance/guidance_h.c'''.
 +
 
 +
In v 5.14 (and probably even earlier), the integral action is implemented differently: the output of the PD controller is integrated, and the integral value thus represents the attitude trim.
  
 
== Guidance horizontal navigation loop ==
 
== Guidance horizontal navigation loop ==
Line 186: Line 227:
 
The code for this loop is located in '''guidance/guidance_h.c'''
 
The code for this loop is located in '''guidance/guidance_h.c'''
  
(to be developed later..)
+
In v 5.14 (and probably even earlier), the integral action is implemented differently: the output of the PD controller is integrated, and the integral value thus represents the attitude trim.
 
 
Integral action or tracking algorithm:
 
 
 
if (|guidance_h_pos_err| < HOLD_DISTANCE) -> intregal error * guidance_h_igain
 
 
 
else -> path tracking error * guidance_h_ngain
 
  
 
[[Category:Software]] [[Category:Developer_Documentation]]
 
[[Category:Software]] [[Category:Developer_Documentation]]

Latest revision as of 09:37, 7 February 2019

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. This is especially the case when using extra features such as AGR_CLIMB, STRONG_WIND, etc.

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 ones used in the airborne code (written in C). Most of these can be used, all caps, in the Airframe Configuration file in order to define the default values of these variables. If you see all capital letters in the diagrams, the value is fixed and cannot be changed using Settings mechanism.

The most direct way to determine vehicle behavior is to look at the source code. When determining which control loops are engaged by various flight plan blocks and stages, it may be helpful to look at sw/airborne/subsystems/nav.h and the generated flightplan file in var/<AIRCRAFT_NAME>/generated/flight_plan.h.

Fixed-wing autopilot

Global view

General overview

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 the 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

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

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.

Note; estimator_phi is the measured roll angle, and estimator_p is the measured rate of change in roll angle.

Altitude loop

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. Only one loop is active at a time. Note also that if USE_AIRSPEED is defined, then the Auto Throttle loop is replaced by the Auto Airspeed loop, see below.

Auto throttle loop

Auto Throttle climb 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

Auto Pitch climb loop

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

Pitch loop

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. The first sum block of the diagram is not completely accurate. The input to the pitch stabilization loop is h_ctl_pitch_setpoint, though v_ctl_pitch_of_vz is not a direct input. Rather h_ctl_pitch_setpoint is assigned in the main autopilot loop from nav_pitch, in general (in auto1, it is assigned directly from the pitch r/c input). The nav_pitch value is obtained in a few ways, depending on which control loop(s) are active. If auto throttle is active, nav_pitch is the sum of v_ctl_pitch_of_vz and a fixed setpoint defined using the pitch attribute in the flightplan. If AGR_CLIMB is set, nav_pitch is either the airframe file defined ascent and descent pitch setpoints, the standard sum, or a blend of each. If auto pitch is active, nav_pitch is defined directly from the output of that loop. If airspeed is active, nav_pitch is defined in the auto airspeed pitch loop.

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.

Auto Airspeed climb loop

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.

Energy Control loops using Airspeed Sensor

Total Energy (speed + height) control for fixed wing vehicles developed by the MAVLab of the Technical University of Delft. An airspeed sensor is mandatory. The energy control loops control both pitch and throttle. This control loop set replaces the Auto Throttle climb loop and is located in energy_ctrl.c.

Energy Control loop

Rotorcraft 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.

Overview

Booz autopilot globalview.png

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

Since v5.0, the Supervision was replaced by Motor mixing. Alternatively, the stabilization commands can also be directly used in the servo command laws.

Vertical Control

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

mode: GUIDANCE_V_MODE_RC_CLIMB (speed control)

Rc climb.png

mode: GUIDANCE_V_MODE_CLIMB (speed control)

Climb.png

mode: GUIDANCE_V_MODE_HOVER (altitude control)

Z hold.png

mode: GUIDANCE_V_MODE_NAV

Reference generators

z relates to altitude, zd to speed and zdd to acceleration

from z set point (desired altitude)

Refgen from z sp.png

from z dot set point (desired speed)

Booz refgen zdot.png]

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.c

Vertical loop

Vertical loop V40.png

the source code is located in guidance\guidance_v.c

NOMINAL_HOVER_THROTTLE is the nominal thrust when hovering. It's calculated by default by an adaptative controller, in guidance\guidance_v_adpt.h.

Instead of the adaptative controller, NOMINAL_HOVER_TROTTLE can be set manually in the Airframe configuration file with:

<define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>

The value is found after flying manually in hover: NOMINAL_HOVER_THROTTLE= THRUST/MAX_PPRZ (THRUST:mean value of ROTORCRAFT_FP THRUST message in hover, MAX_PPRZ:9600)

In most cases it makes more sense to not specify this explicitly and use the adaptive controller

Horizontal Control

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

mode: GUIDANCE_H_MODE_RATE

GUIDANCE H MODE RATE.png

mode: GUIDANCE_H_MODE_ATTITUDE

GUIDANCE H MODE ATTITUDE.png

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.

GUIDANCE H MODE HOVER.png

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

GUIDANCE H MODE NAV.png

Rotation to body is the same as in GUIDANCE_H_MODE_HOVER.

Reference generators

from euler attitude angle set point

Booz refgen att.png

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

Booz refgen rate.png

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

from pos set point

Booz refgen xy sp.png

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

Attitude loop.png

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

Rate loop

Rate loop.png

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

Guidance horizontal hover loop

Guidance horizontal hover.png

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

In v 5.14 (and probably even earlier), the integral action is implemented differently: the output of the PD controller is integrated, and the integral value thus represents the attitude trim.

Guidance horizontal navigation loop

Booz horizontal nav.png

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

In v 5.14 (and probably even earlier), the integral action is implemented differently: the output of the PD controller is integrated, and the integral value thus represents the attitude trim.