Difference between revisions of "Theory of Operation"

From PaparazziUAV
Jump to: navigation, search
m
(Added link to Youtube video explaining PID.)
 
(29 intermediate revisions by 14 users not shown)
Line 1: Line 1:
== Formal Methods ==
+
== Control Loops Writeup ==
The critical core of the Paparazzi code has been proven by [http://en.wikipedia.org/wiki/Formal_methods formal methods] to ensure absolute reliability of the servo, R/C, and manual control portions of the code using the same tools and methods used for full-scale autopilots, nuclear power plants, and other critical control systems.  See our page on [[Lustre|formally proving critical Paparazzi code using Lustre]].
+
Excellent explanation of the [[Control Loops]] and some basics on [[ControlTheory|Control Theory]] behind it.
  
 
== PID ==
 
== PID ==
 
Paparazzi uses common Proportional Integral Derivative (PID) control for stability and navigation.  PID is probably the most commonly used feedback control design as it is simple to implement and intuitive to operate.  PID controllers use three terms operating on the measured error to produce a control output. If ''u(t)'' is the control signal sent to the system, ''y(t)'' is the measured output and ''r(t)'' is the desired output, and tracking error <math>e(t)=r(t)- y(t)</math>, a PID controller has the general form
 
Paparazzi uses common Proportional Integral Derivative (PID) control for stability and navigation.  PID is probably the most commonly used feedback control design as it is simple to implement and intuitive to operate.  PID controllers use three terms operating on the measured error to produce a control output. If ''u(t)'' is the control signal sent to the system, ''y(t)'' is the measured output and ''r(t)'' is the desired output, and tracking error <math>e(t)=r(t)- y(t)</math>, a PID controller has the general form
  
:<math>u(t) =  K_P e(t)   K_I \int e(t)dt   K_D \dot{e}(t)</math>
+
:<math>u(t) =  K_P e(t) + K_I \int e(t)dt + K_D \dot{e}(t)</math>
  
 
The desired closed loop dynamics are obtained by adjusting the three parameters <math> K_P</math>, <math> K_I</math> and <math> K_D</math>, often iteratively by "tuning" and without specific knowledge of a plant model. Stability can often be ensured using only the proportional term in well damped systems. The integral term compensates for steady long-term errors, and the derivative term is used to provide damping to reduce oscillation.
 
The desired closed loop dynamics are obtained by adjusting the three parameters <math> K_P</math>, <math> K_I</math> and <math> K_D</math>, often iteratively by "tuning" and without specific knowledge of a plant model. Stability can often be ensured using only the proportional term in well damped systems. The integral term compensates for steady long-term errors, and the derivative term is used to provide damping to reduce oscillation.
Line 11: Line 11:
 
See more on Wikipedia: [http://en.wikipedia.org/wiki/PID_controller PID Controller]
 
See more on Wikipedia: [http://en.wikipedia.org/wiki/PID_controller PID Controller]
  
Paparazzi uses PID controllers on all loops but many of the I and D terms are not fully implemented as they are often unnecessary or difficult to tune.  Below are some examples of the PID implementations in Paparazzi.
+
PID explained by RC Model Reviews: [https://youtu.be/0vqWyramGy8 What PIDs do and how they do it]
 +
 
 +
Paparazzi uses PID controllers on all loops but many of the I and D terms are not fully implemented as they are often unnecessary or difficult to tune.  Below are some examples of the PID implementations in Paparazzi. There is a [[Control Loops|graphical description of the control loops]] as well.
  
 
=== Roll Rate ===
 
=== Roll Rate ===
{{Box Code|In <b>sw/airborne/fw_h_ctl.c</b> we define the roll rate loop:|
+
{{Box Code|In <b>sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c</b> we define the roll rate loop:|
  float cmd <nowiki>=</nowiki> throttle_dep_pgain * ( err   h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES   h_ctl_roll_rate_dgain * d_err);
+
  float cmd <nowiki>=</nowiki> throttle_dep_pgain * ( err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + h_ctl_roll_rate_dgain * d_err);
 
}}
 
}}
 
Note that the roll Pgain is variable with throttle and multiplies through the entire equation affecting the I and D terms as well for ease of tuning.
 
Note that the roll Pgain is variable with throttle and multiplies through the entire equation affecting the I and D terms as well for ease of tuning.
 +
 +
=== Roll Attitude ===
 +
{{Box Code|In <b>sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c</b> we define the roll attitude gain loop:|
 +
float err <nowiki>=</nowiki> estimator_phi - h_ctl_roll_setpoint;
 +
float cmd <nowiki>=</nowiki> - h_ctl_roll_attitude_gain * err- h_ctl_roll_rate_gain * estimator_p+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
 +
}}
  
 
=== Pitch Angle ===
 
=== Pitch Angle ===
{{Box Code|In <b>sw/airborne/fw_h_ctl.c</b> we define the pitch angle loop:|
+
{{Box Code|In <b>sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c</b> we define the pitch angle loop:|
 
  float err <nowiki>=</nowiki> estimator_theta - h_ctl_pitch_setpoint;
 
  float err <nowiki>=</nowiki> estimator_theta - h_ctl_pitch_setpoint;
 
  float d_err <nowiki>=</nowiki> err - last_err;
 
  float d_err <nowiki>=</nowiki> err - last_err;
 
  last_err <nowiki>=</nowiki> err;
 
  last_err <nowiki>=</nowiki> err;
  float cmd <nowiki>=</nowiki> h_ctl_pitch_pgain * (err   h_ctl_pitch_dgain * d_err)   h_ctl_elevator_of_roll * fabs(estimator_phi);
+
  float cmd <nowiki>=</nowiki> h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err) + h_ctl_elevator_of_roll * fabs(estimator_phi);
 
}}
 
}}
  
Line 30: Line 38:
  
 
=== Navigation ===
 
=== Navigation ===
{{Box Code|In <b>sw/airborne/fw_h_ctl.c</b> we define the navigation loop:|
+
{{Box Code|In <b>sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c</b> we define the navigation loop:|
 
   float err <nowiki>=</nowiki> estimator_hspeed_dir - h_ctl_course_setpoint;
 
   float err <nowiki>=</nowiki> estimator_hspeed_dir - h_ctl_course_setpoint;
 
   NormRadAngle(err);
 
   NormRadAngle(err);
Line 37: Line 45:
 
   float cmd <nowiki>=</nowiki> h_ctl_course_pgain * err * speed_depend_nav;
 
   float cmd <nowiki>=</nowiki> h_ctl_course_pgain * err * speed_depend_nav;
 
}}
 
}}
Here we only use the Proportional term though a Derivative would be useful as navigation is not well damped.  Note that an Integral term cannot be used for navigtation without accurate and reliable wind information and the necessary implementation of wind data.  Note however that we increase/reduce the commanded bank angle for navigation based on the ground speed.  This reduces "hunting" on upwind legs, keeps the navigation tight on fast downwind legs and helps keep circles round in a crosswind.
+
Here we only use the Proportional term though a Derivative would be useful as navigation is not well damped.  Note that an Integral term cannot be used for navigation without accurate and reliable wind information and the necessary implementation of wind data.  Note however that we increase/reduce the commanded bank angle for navigation based on the ground speed.  This reduces "hunting" on upwind legs, keeps the navigation tight on fast downwind legs and helps keep circles round in a crosswind.
  
 
=== Climb Rate ===
 
=== Climb Rate ===
{{Box Code|In <b>sw/airborne/fw_v_ctl.c</b> we compute the desired climb rate:|
+
{{Box Code|In <b>sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c</b> we compute the desired climb rate:|
 
  v_ctl_altitude_error <nowiki>=</nowiki> estimator_z - v_ctl_altitude_setpoint;
 
  v_ctl_altitude_error <nowiki>=</nowiki> estimator_z - v_ctl_altitude_setpoint;
  v_ctl_climb_setpoint <nowiki>=</nowiki> v_ctl_altitude_pgain * v_ctl_altitude_error   v_ctl_altitude_pre_climb;
+
  v_ctl_climb_setpoint <nowiki>=</nowiki> v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb;
 
}}
 
}}
 
Here we only use the Proportional term though a Derivative would be useful as climb rate is not well damped.  An integral term may prove useful if well-implemented.  The last term <b><tt>v_ctl_altitude_pre_climb</tt></b> represents the desired constant climb rate needed to follow a 3-dimensional navigation path.  This term is zero for level flight, altitude maintenance, and commanded altitude changes.
 
Here we only use the Proportional term though a Derivative would be useful as climb rate is not well damped.  An integral term may prove useful if well-implemented.  The last term <b><tt>v_ctl_altitude_pre_climb</tt></b> represents the desired constant climb rate needed to follow a 3-dimensional navigation path.  This term is zero for level flight, altitude maintenance, and commanded altitude changes.
{{Box Code|Then we compute the throttle response:|
+
{{Box Code| Then we compute the throttle response:|
 
  float err <nowiki>=</nowiki> estimator_z_dot - v_ctl_climb_setpoint;
 
  float err <nowiki>=</nowiki> estimator_z_dot - v_ctl_climb_setpoint;
  float controlled_throttle <nowiki>=</nowiki> v_ctl_auto_throttle_cruise_throttle   v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint  
+
  float controlled_throttle <nowiki>=</nowiki> v_ctl_auto_throttle_cruise_throttle + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint  
  v_ctl_auto_throttle_pgain * (err   v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err);
+
+ v_ctl_auto_throttle_pgain * (err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err);
 
and pitch response
 
and pitch response
 
  float v_ctl_pitch_of_vz <nowiki>=</nowiki> v_ctl_climb_setpoint * v_ctl_auto_throttle_pitch_of_vz_pgain;
 
  float v_ctl_pitch_of_vz <nowiki>=</nowiki> v_ctl_climb_setpoint * v_ctl_auto_throttle_pitch_of_vz_pgain;
 
}}
 
}}
  
== Infrared Sensors ==
+
[[Category:Software]] [[Category:User_Documentation]]
[[Image:Electromagnetic-Spectrum.png|thumb|Electromagnetic Spectrum]]
 
The infrared spectrum spans the gap between red light and microwave radiation and consists of a very large range of wavelengths from 0.75µm to 1000µm (compare to 0.38µm to 0.75µm for the entire range of visible light).  With such a broad range, it is not surprising that the properties of IR radiation vary widely with wavelength and therefore are usually categorized as follows:
 
* near infrared (NIR, IR-A): 0.75–1.4
 

Latest revision as of 11:51, 15 April 2016

Control Loops Writeup

Excellent explanation of the Control Loops and some basics on Control Theory behind it.

PID

Paparazzi uses common Proportional Integral Derivative (PID) control for stability and navigation. PID is probably the most commonly used feedback control design as it is simple to implement and intuitive to operate. PID controllers use three terms operating on the measured error to produce a control output. If u(t) is the control signal sent to the system, y(t) is the measured output and r(t) is the desired output, and tracking error e(t)=r(t)- y(t), a PID controller has the general form

u(t) =  K_P e(t) + K_I \int e(t)dt + K_D \dot{e}(t)

The desired closed loop dynamics are obtained by adjusting the three parameters  K_P,  K_I and  K_D, often iteratively by "tuning" and without specific knowledge of a plant model. Stability can often be ensured using only the proportional term in well damped systems. The integral term compensates for steady long-term errors, and the derivative term is used to provide damping to reduce oscillation.

See more on Wikipedia: PID Controller

PID explained by RC Model Reviews: What PIDs do and how they do it

Paparazzi uses PID controllers on all loops but many of the I and D terms are not fully implemented as they are often unnecessary or difficult to tune. Below are some examples of the PID implementations in Paparazzi. There is a graphical description of the control loops as well.

Roll Rate

File: In sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c we define the roll rate loop:
float cmd = throttle_dep_pgain * ( err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + h_ctl_roll_rate_dgain * d_err);

Note that the roll Pgain is variable with throttle and multiplies through the entire equation affecting the I and D terms as well for ease of tuning.

Roll Attitude

File: In sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c we define the roll attitude gain loop:
float err = estimator_phi - h_ctl_roll_setpoint;
float cmd = - h_ctl_roll_attitude_gain * err- h_ctl_roll_rate_gain * estimator_p+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;

Pitch Angle

File: In sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c we define the pitch angle loop:
float err = estimator_theta - h_ctl_pitch_setpoint;
float d_err = err - last_err;
last_err = err;
float cmd = h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err) + h_ctl_elevator_of_roll * fabs(estimator_phi);

Here we use only Proportional as the Derivative is always set to zero. An integral term could prove useful here. Aircraft pitch response is normally very well damped. Those with "plank" style aircraft or other pitch-sensitive designs may benefit from implementing a gyro-based D term.

Navigation

File: In sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c we define the navigation loop:
 float err = estimator_hspeed_dir - h_ctl_course_setpoint;
 NormRadAngle(err);
 float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; 
 Bound(speed_depend_nav, 0.66, 1.5);
 float cmd = h_ctl_course_pgain * err * speed_depend_nav;

Here we only use the Proportional term though a Derivative would be useful as navigation is not well damped. Note that an Integral term cannot be used for navigation without accurate and reliable wind information and the necessary implementation of wind data. Note however that we increase/reduce the commanded bank angle for navigation based on the ground speed. This reduces "hunting" on upwind legs, keeps the navigation tight on fast downwind legs and helps keep circles round in a crosswind.

Climb Rate

File: In sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c we compute the desired climb rate:
v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint;
v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb;

Here we only use the Proportional term though a Derivative would be useful as climb rate is not well damped. An integral term may prove useful if well-implemented. The last term v_ctl_altitude_pre_climb represents the desired constant climb rate needed to follow a 3-dimensional navigation path. This term is zero for level flight, altitude maintenance, and commanded altitude changes.

File: Then we compute the throttle response:
float err = estimator_z_dot - v_ctl_climb_setpoint;
float controlled_throttle = v_ctl_auto_throttle_cruise_throttle + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint 
+ v_ctl_auto_throttle_pgain * (err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err);

and pitch response

float v_ctl_pitch_of_vz = v_ctl_climb_setpoint * v_ctl_auto_throttle_pitch_of_vz_pgain;