Module/guidance vector field

Modules

ArduIMU

Module/ADC generic

Module/Aeroprobe OTF

Module/Airspeed ETS

Module/AOA adc

Module/Bosch BMP085

Module/collective tracking control

Module/FlightBenchmark

Module/Freescale MPL3115A2

Module/GPS UBlox UCenter

Module/guidance vector field

Module/Honeywell HIH-4030

Module/Hygrosens TEMOD-I2C-R1

Module/infrared

Module/IST DigiPicco

Module/LM75

Module/Mag HMC58xx

Module/Meas Spec MS5611 I2C

Module/Melexis MLX90614

Module/rl obstacle avoidance

Module/Rotorcraft cam

Module/Sensirion SHT25

Module/Sensirion SHT75

Module/Servo switch

Module/System monitor

Module/TI TMP102

Module/TronSens HTM B71

Module/VTI SCP1000 I2C

Module/VTI SCP1000 SPI

Modules

Modules list

MultiUAV

Openlog

Pan Tilt Camera

SDLogger SPI Direct

Sensors/AMSYS

Introduction

We have written two algorithms for solving the problem of tracking smooth curves by an unmanned aerial vehicle:

• Guiding Vector Field Implicit
• Guiding Vector Field Parametric

Both algorithms are based on creating Guiding Vector Fields (GVF) to assist the vehicle to track a path. As their names indicate, the main difference between the two is how we describe the desired path. For example, the GVF Implicit needs the implicit equation of the path, e.g., (for a circle) $x^{2}+y^{2}-r^{2}=0$, and the GVF Parametric needs the parametric $x=r\cos(w\,\omega _{o}),y=r\sin(w\,\omega _{o})$. In Paparazzi, there is a practical difference. The GVF Implicit deals with 2D (constant altitude) paths, whereas GVF Parametric deals with 2D and 3D paths.

For fixed-wings, the output of the algorithm can be directly expressed in terms of the bank angle of the UAV in order to achieve coordinated turns. Furthermore, the algorithm can be tuned offline such that the physical constraints of the UAV, e.g., the maximum bank angle, will not be violated in a neighborhood of the desired trajectory. In addition, the GVF Parametric' in its 3D version, sets the vertical velocity of the vehicle that it is controlled by the corresponding low-level controller.

The implementation of the two algorithms for rotorcraft is still in the TO DO list.

The GVF Implicit algorithm is based on the paper Guidance algorithm for smooth trajectory tracking of a fixed wing UAV flying in wind flows, presented at ICRA 2017. A more rigorous and detailed analysis can be found in the paper A guiding vector field algorithm for path following control of nonholonomic mobile robots.

The preliminaries of the GVF Parametric algorithm has been presented in the CDC 2020 (Conference on Decision and Control) Mobile Robot Path Following Control in 2D Using a 3D Guiding Vector Field: Singularity Elimination and Global Convergence. A detailed discussion of the algorithm can be found in the IEEE Transactions on Robotics article Singularity-Free Guiding Vector Field for Robot Navigation.

The GVF Implicit has been exploited to control formations of several aircraft flying at a constant speed. Check the subsection python Apps. A detailed analysis can be found in the paper Circular formation control of fixed-wing UAVs with constant speeds (submitted to IROS 2017). A video about how the circular formation algorithm works https://www.youtube.com/watch?v=q4b8JRU1Gbw .

Here we show an animation of an airplane having the GVF Implicit as a guidance system.

GVF Implicit

How does the GVF implicit work?

We start from the implicit equation of the trajectory, for example for a circumference we have that $\varphi (x,y)=x^{2}+y^{2}-R^{2}$, where $x,y$ and $R$ are the x, y coordinates with respect to HOME ($O$ in the following figure) and R is the radius of the circumference. Note that when the vehicle is over the desired trajectory, then $\varphi (x,y)=0$, otherwise it will be different from zero. We will use this concept of "level set" to define the notion of distance to the trajectory, namely $e:=\varphi (x,y)$. It has to be noted that this is different from the Euclidean distance (see figure below).

For the sake of clarity we will consider just 2D trajectories and parallel to the ground. Let us stack the x and y coordinates in $p:={\begin{bmatrix}x&y\end{bmatrix}}^{T}$ and consider the following kinematical model for our vehicle

${\begin{cases}{\dot {p}}&=sm(\psi )+w\\{\dot {\psi }}&=u,\end{cases}}$

where $s\in \mathbb {R} ^{+}$ is a constant that can be considered as the airspeed, $m={\begin{bmatrix}\cos(\psi )&\sin(\psi )\end{bmatrix}}^{T}$ with $\psi \in (-\pi ,\pi ]$ being the attitude yaw angle, $w\in \mathbb {R} ^{2}$ is a constant with respect to $O$ representing the wind and $u$ is the control action that will make the vehicle to turn. We also notice that the course heading $\chi \in (-\pi ,\pi ]$, i.e. the direction the velocity vector ${\dot {p}}$ is pointing at, in general is different from the yaw angle $\psi$ because of the wind.

Given a desired trajectory, we compute its normal $n(p):=\nabla \varphi (p)$ and its tangent $\tau (p)=En(p),\quad E={\begin{bmatrix}0&1\\-1&0\end{bmatrix}}$. The idea is to steer the vehicle such that it follows the direction given by the unit vector calculated from

${\dot {p}}_{d}(p):=\tau (p)-k_{e}e(p)n(p),$

where $k_{e}$ is a positive gain. At each point $p$ we can build a unit vector from ${\dot {p}}_{d}(p)$. This collection of vectors is our Guidance Vector Field. Note that when the error is zero, then we are just tracking the tangent to the trajectory.

We assume that the trajectory is twice differentiable (so its gradient and Hessian exists) and that it is regular in a neighborhood of ${\mathcal {P}}$, i.e.

$\nabla \varphi (p)\neq 0,\quad p\in {\mathcal {N}}_{\mathcal {P}},\quad {\mathcal {N}}_{\mathcal {P}}:=\{p\,:\,|\varphi (p)|\leq c^{*}>0\}$.

The algorithm needs for work the measurements of ${\dot {p}}$ (typically derived from GPS), position $p$ w.r.t. HOME (typically derived from GPS) and yaw angle $\psi$ or sideslip angle $\beta =(\psi -\chi )$. The algorithm still works quite well in practice with only ${\dot {p}}$ and $p$, in other words, you will only need to have installed a GPS. The latter is the implemented version in pprz/master.

Using the GVF Implicit in the flight plan

First of all, do not forget to include the module in your flightplan file. It is just called "gvf_module" since at that time its sibling parametric did not exist.

<module name="gvf_module"/>

and in your telemetry file (already in default_fixedwing_gvf.xml)

<message name="GVF" period="1"/>

Later, you can set individually the gains for your particular aircraft in the airframe file. For example

<section name="GVF" prefix="GVF_">
  <define name="LINE_KE" value="2.5"/>
</section>

Disclaimer

The GVF Implicit is not fully integrated (yet) with the flight plan. This means that you will need to call explicitly C functions and many variables are not set by default. For example, the GVF Implicit does not control settings such as altitude or 'nav_pitch' (default pitch). You need to set manually the desired variables in the flight plan before a GVF Implicit function is called. In other words, the GVF Implicit ONLY controls (for fixed-wings) the bank-angle to be tracked such that the airplane converges to the desired trajectory.

A particular example: Your airplane is going to land and the 'altitude' is set to be 0 AGL. Then you change your mind and choose (for example by clicking on the corresponding block in the flight plan at the GCS) to track an ellipse. The altitude to be tracked will continue being zero. Be careful with these kinds of situations.

Trajectories

List of available functions (updated June 2017) for the user to be called from the flight plan. There is a demo called demo_gvf.xml in the conf/flightplan folder.

Ellipses

<call fun="gvf_ellipse_wp(WP_mywaypoint, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
<call fun="gvf_ellipse_XY(float x, float y, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>

Straight lines

<call fun="gvf_line_wp_heading(WP_mywaypoint, gvf_line_par.heading)"/>
<call fun="gvf_line_XY_heading(float x, float y, gvf_line_par.heading)"/>
<call fun="gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)"/>
<call fun="gvf_line_wp1_wp2(WP_mywaypoint1, WP_mywaypoint2)"/>
<call fun="gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)"/>
<call fun="gvf_segment_wp1_wp2(WP_mywaypoint1, WP_mywaypoint2)"/>
<call fun="gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float gvf_segment_par.d1, gvf_segment_par.d2)"/>
<call fun="gvf_segment_loop_wp1_wp2(WP_mywaypoint1, WP_mywaypoint2, float d1, float d2)"/>

Watch out.

• `line' means that the aircraft tracks an infinite line.
• `segment' means that the aircraft tracks a segment defined by two (way)points. The first (way)point of the segment function (no for the segment_loop) defines the starting point and the second one the ending point. Once the aircraft crosses the imaginary line perpendicular to the segment at the ending point, the flight plan jumps to the next block.
• `segment_loop' means that the aircraft tracks the segment defined by two (way)points in an endless loop. The values 'd1' and 'd2' mean how far the aircraft will go beyond the end of the segment before turning back (useful when it is windy and you want to be sure that your aircraft tracks the segment by converging to it before been in the middle of the two points).

Sinusoidal

<call fun="gvf_sin_XY_alpha(float x, float y, gvf_sin_par.alpha, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
<call fun="gvf_sin_wp_alpha(WP_mywaypoint, gvf_sin_par.alpha, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
<call fun="gvf_sin_wp1_wp2(WP_maywaypoiny1, WP_mywaypoint2, gvf_sin_par.alpha, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>

Watch out

As with lines, to track a sinusoidal means that the aircraft follows an open and endless trajectory.

Direction

For setting the tracking direction (the direction of the tangent vector $\tau$ in the above figure)

<call fun="gvf_set_direction(s)"/>

where s is an integer number 1 or -1.

Gain tuning

An aircraft has its own set of two gains $k_{n},k_{e}$ for each trajectory.

• KN determines the convergence rate for aligning the vehicle to the vector field. A typical value for starting tuning $k_{n}$ should be between 0.2 and 1.
• KE determines how aggressive is the vector field. For example, in the following figure we have an ellipse with two different values of $k_{e}$, at the left we have a value of 3 and at the right 0.4. Note how in left one the vectors are "more aggressive" towards the trajectory. While a big value can make the vehicle to converge quickly to the trajectory, it can make the tracking unstable once the vehicle is close it. This is because the vector field might change so quick that physically the vehicle cannot follow it. Check the Section IV in the original paper in the introduction.

Defining your own trajectory

The algorithm is placed in $PAPARAZZI_HOME/sw/airborne/modules/guidance/gvf . The trajectories are placed in $PAPARAZZI_HOME/sw/airborne/modules/guidance/gvf/trajectories .

Here there is an example (the straight line) about how to code your own trajectory.

File:

sw/airborne/modules/guidance/gvf/gvf.h // Straigh line extern bool gvf_line_XY_heading(float x, float y, float heading); extern bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2); extern bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2); extern bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2); extern bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2); extern bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2); extern bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2); extern bool gvf_line_wp_heading(uint8_t wp, float heading);

File:

sw/airborne/modules/guidance/gvf/gvf.c bool gvf_line_wp_heading(uint8_t wp, float heading) { heading = heading * M_PI / 180; float a = waypoints[wp].x; float b = waypoints[wp].y; gvf_line_XY_heading(a, b, heading); return true; }

The idea is that while you can define a straight line in many different ways, you make all the transformations (for example, we take the X and Y coordinates from a waypoint and convert the heading from degrees to radians in this example) in the function called by the user such that the algorithm is always evaluating the trajectory in the same way. It helps for maintaining the code and for tuning the gains (you do not want to have a different set of gains depending on how you call your trajectory).

We finally call the function gvf_line_XY_heading that will invoke the algorithm.

File:

sw/airborne/modules/guidance/gvf/gvf.c bool gvf_line_XY_heading(float a, float b, float heading) { float e; struct gvf_grad grad_line; struct gvf_Hess Hess_line; gvf_trajectory.type = 0; gvf_trajectory.p[0] = a; gvf_trajectory.p[1] = b; gvf_trajectory.p[2] = heading; gvf_line_info(&e, &grad_line, &Hess_line); gvf_control.ke = gvf_line_par.ke; gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line); gvf_control.error = e; horizontal_mode = HORIZONTAL_MODE_WAYPOINT; gvf_segment.seg = 0; return true; }

The three first lines are the $\varphi (p),\nabla \varphi (p)$ and $H(\varphi (p))$, where $H$ stands for the Hessian. These three guys will be populated by gvf_line_info (in a moment we will be see how to write it).

Then gvf_traj_type stands for the kind of trajectory. Right now we have 0 for lines, 1 for ellipses and 2 for sinusoidals. Feel free to choose an index that has not been taken. The rest gvf_param[x]'s are the parameters of the trajectory. You have up to seven for describing a trajectory.

The function gvf_control_2D executes the algorithm for calculating the desired turn for the vehicle. It is a good practice that all the trajectories (not only straight lines) have the same order of magnitude (for example between 0.00 and 5.00) for $k_{e}$. So if you have to add a scaling factor, here is where you have to do it.

The variables gvf_xx are sent to the ground station as telemetry, so you can draw the vector field, the trajectory, etc.

Finally for defining $\varphi (p),\nabla \varphi (p)$ and $H(\varphi (p))$ you need to write the two files gvf_line.{c,h}

File:

sw/airborne/modules/guidance/gvf/trajectories/gvf_line.h extern void gvf_line_info(float *phi, struct gvf_grad *, struct gvf_Hess *);

and in gvf_line.c you write the implicit math of your trajectory (quite explicit)

File:

sw/airborne/modules/guidance/gvf/trajectories/gvf_line.c void gvf_line_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess){ struct EnuCoor_f *p = stateGetPositionEnu_f(); float px = p->x; float py = p->y; float a = gvf_param[0]; float b = gvf_param[1]; float alpha = gvf_param[2]; // Phi(x,y) *phi = -(px-a)*cosf(alpha) + (py-b)*sinf(alpha); // grad Phi grad->nx = -cosf(alpha); grad->ny = sinf(alpha); // Hessian Phi hess->H11 = 0; hess->H12 = 0; hess->H21 = 0; hess->H22 = 0; }

Demo

There is a flightplan called 'demo_gvf.xml' in order to test the different settings in a simulation. Do not forget to choose default_telemetry_gvf.xml in your paparazzi center.

GVF Parametric

You need Eigen to make it work!

First of all, we need the Eigen lib for compiling the code. You need to execute the following command in the Paparazzi dir: git submodule update --init --recursive sw/ext/eigen

How does the GVF Parametric works?

The preliminaries of the GVF Parametric algorithm has been presented in the CDC 2020 (Conference on Decision and Control) Mobile Robot Path Following Control in 2D Using a 3D Guiding Vector Field: Singularity Elimination and Global Convergence. A more detailed discussion can be found in the IEEE Transactions on Robotics article Singularity-Free Guiding Vector Field for Robot Navigation

Using the GVF Parametric in the flight plan

First of all, do not forget to include the module in your flightplan file. Both modules "gvf_module" (the implicit) and "gvf_parametric" are compatible, you can have both in your flight plan.

<module name="gvf_parametric"/>

and in your telemetry file (already in default_fixedwing_gvf.xml)

<message name="GVF_PARAMETRIC" period="1"/>

Later, you can set individually the gains for your particular aircraft in the airframe file. For example

<section name="GVF_PARAMATRIC" prefix="GVF_PARAMETRIC">
  <define name=""3D_ELLIPSE_KX value="0.001"/>
</section>

Disclaimer

The GVF Paramatric is not fully integrated (yet) with the flight plan. This means that you will need to call explicitly C functions and many variables are not set by default.

Only for 2D trajectories: the GVF Parametric does not control settings such as altitude or 'nav_pitch' (default pitch). You need to set manually the desired variables in the flight plan before a GVF Parametric function is called. In other words, the GVF Parametric for 2D trajectories ONLY controls (for fixed-wings) the bank-angle to be tracked such that the airplane converges to the desired trajectory.

For 3D trajectories, the GVF_Parametric sets the desired vertical velocity to be tracked by the aircraft. You must calibrate the gains of the corresponding controller first, check Auto_Throttle_and_Auto_Pitch_climb_loops.

Trajectories

List of available functions (updated August 2020) for the user to be called from the flight plan. There is a demo called demo_gvf_parametric.xml in the conf/flightplan folder.

2D Trefoil

Self-intersected trajectory, check the Wikipedia entry for more details Trefoil knot. The parameters below respond to the parametric equations (where w is the free parameter to generate the trajectoy) $x=\cos(w\,\omega _{1})(r\cos(w\,\omega _{2})+{\text{ratio}}),\quad y=\sin(w\omega _{1})(r\cos(w\,\omega _{2})+{\text{ratio}})$. Finally, the xo,yo or waypoint centers the trajectory on the map, and alpha rotates it.

<call fun="gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)"/>
<call fun="gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)"/>

2D Trefoil trajectory

3D Ellipse

The intersection of a plane with a cylinder. The parametric equations (where w is the free parameter to generate the trajectoy) are $x=r\cos(w\,\omega ),\quad y=\sin(w\,\omega ),\quad z=0.5(z_{h}+z_{l}+(z_{l}-z_{h})\sin(\alpha -w))$. Finally, the xo,yo or waypoint is the XY center of the cylinder, r is the radius of the cylinder. For the vertical component zl and zh are the lowest and highest points of the resultant ellipse, and alpha is the heading of the lowest point. For the third function, alt_center is the middle point in Z, and delta the plus/minus altitude.

<call fun="gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)"/>
<call fun="gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)"/>
<call fun="gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)"/>

3D Lissajous

You can create an endless number of Lissajous curves playing with the different frequencies and offsets in the following parametric equations $x=c_{x}\cos(w\,\omega _{x}+\delta _{x}),y=\quad c_{y}\cos(w\,\omega _{y}+\delta _{y}),\quad z=c_{z}\cos(w\,\omega _{z}+\delta _{z})$. The parameters xo,yo,zo and alpha are for locating and rotating the trajectory on the map (with respect to home).

<call fun="gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy, float wz, float dx, float dy, float dz, float alpha)"/>
<call fun="gvf_parametric_3D_lissajous_wp_center(uint8_t wp, float zo, float cx, float cy, float cz, float wx, float wy, float wz, float dx, float dy, float dz, float alpha)"/>

3D Lissajous "bended eight", the purple cube is the GVF. $\omega _{x}=2\omega _{y}=2\omega _{z},\,\delta _{x}=\delta _{z}=0,\,\delta _{y}=\pi /2$

Direction

For setting the tracking direction (the direction of the tangent vector $\tau$ in the above figure)

<call fun="gvf_set_direction(s)"/>

where s is an integer number 1 or -1.

Defining your own trajectory

The algorithm is placed in $PAPARAZZI_HOME/sw/airborne/modules/guidance/gvf_parametric . The trajectories are placed in $PAPARAZZI_HOME/sw/airborne/modules/guidance/gvf_parametric/trajectories .

Here there is an example (the 3D Lissajous) about how to code your own trajectory.

- First step, we need to write down the parametric equations. Let us choose $w$ as parameter:

x = c_x cos(w \omega_x + delta_x), y = c_y cos(w \omega_y + delta_y), z = c_z cos(w \omega_z + delta_z) (for some reason math environment does not work well with these equations here u_U).

- Second step, we need the first and second partial derivatives w.r.t. the parameter $w$:

dx/dw = -c_x \omega_x sin(w \omega_x + delta_x), dy/dw = -c_y \omega_x sin(w \omega_y + delta_y), dz/dw = -c_z \omega_z sin(w \omega_z + delta_z)

dx²/dw² = -c_x \omega_x² cos(w \omega_x + delta_x), dy²/dw² = -c_y \omega_x² cos(w \omega_y + delta_y), dz²/dw² = -c_z \omega_z² cos(w \omega_z + delta_z)

- Third step, code (below) the computations from the previous steps. The controller will need the calculations from pointers *f1,*f2,*f3,...*f3dd.

We first note that to place the trajectory in the maps, it needs to be displaced by (xo,yo,zo), and also rotated by \alpha. That is why you will in the code that we rotate and translate the previous calculations in XY, e.g., '*f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo' .

These four parameters, together with c_x, \omega_x, \delta_x, etc, are defined in 'gvf_parametric_trajectory.p_parametric[];' (we will define them in the next step in a higher-level function). Of course, do not forget to declare the function in the corresponding gvf_parametric_3d_lissajous.h .

File:

sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d, float *f1dd, float *f2dd, float *f3dd) { float xo = gvf_parametric_trajectory.p_parametric[0]; float yo = gvf_parametric_trajectory.p_parametric[1]; float zo = gvf_parametric_trajectory.p_parametric[2]; float cx = gvf_parametric_trajectory.p_parametric[3]; float cy = gvf_parametric_trajectory.p_parametric[4]; float cz = gvf_parametric_trajectory.p_parametric[5]; float wx = gvf_parametric_trajectory.p_parametric[6]; float wy = gvf_parametric_trajectory.p_parametric[7]; float wz = gvf_parametric_trajectory.p_parametric[8]; float deltax_rad = gvf_parametric_trajectory.p_parametric[9]*M_PI/180; float deltay_rad = gvf_parametric_trajectory.p_parametric[10]*M_PI/180; float deltaz_rad = gvf_parametric_trajectory.p_parametric[11]*M_PI/180; float alpha_rad = gvf_parametric_trajectory.p_parametric[12]*M_PI/180; float w = gvf_parametric_control.w; float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s; // Parametric equations of the trajectory and the partial derivatives w.r.t. 'w' float nrf1 = cx*cosf(wx*wb + deltax_rad); float nrf2 = cy*cosf(wy*wb + deltay_rad); *f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo; *f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo; *f3 = cz*cosf(wz*wb + deltaz_rad) + zo; float nrf1d = -wx*cx*sinf(wx*wb + deltax_rad); float nrf2d = -wy*cy*sinf(wy*wb + deltay_rad); *f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d; *f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d; *f3d = -wz*cz*sinf(wz*wb + deltaz_rad); float nrf1dd = -wx*wx*cx*cosf(wx*wb + deltax_rad); float nrf2dd = -wy*wy*cy*cosf(wy*wb + deltay_rad); *f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd; *f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd; *f3dd = -wz*wz*cz*cosf(wz*wb + deltaz_rad); }

- Forth step . Finally, we need to code the higher-level function that will call the algorithm to track the trajectory. This function will be called as well from your flightplan (check the flightplan gvf_demo.xml). We first define the parameters of the trajectory (check in the same file how to send them via telemetry), then we call the previous "info" function with the calculations, and finally we call the controller.

File:

sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp bool gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy, float wz, float dx, float dy, float dz, float alpha) { // Safety first! If the asked altitude is low if ((zo - cz) < 1) { zo = 10; cz = 0; } gvf_parametric_trajectory.type = LISSAJOUS_3D; gvf_parametric_trajectory.p_parametric[0] = xo; gvf_parametric_trajectory.p_parametric[1] = yo; gvf_parametric_trajectory.p_parametric[2] = zo; gvf_parametric_trajectory.p_parametric[3] = cx; gvf_parametric_trajectory.p_parametric[4] = cy; gvf_parametric_trajectory.p_parametric[5] = cz; gvf_parametric_trajectory.p_parametric[6] = wx; gvf_parametric_trajectory.p_parametric[7] = wy; gvf_parametric_trajectory.p_parametric[8] = wz; gvf_parametric_trajectory.p_parametric[9] = dx; gvf_parametric_trajectory.p_parametric[10] = dy; gvf_parametric_trajectory.p_parametric[11] = dz; gvf_parametric_trajectory.p_parametric[12] = alpha; float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd; gvf_parametric_3d_lissajous_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd); gvf_parametric_control_3D(gvf_parametric_3d_lissajous_par.kx, gvf_parametric_3d_lissajous_par.ky, gvf_parametric_3d_lissajous_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd); return true; }

Demo

There is a flightplan called 'demo_gvf.xml' in order to test the different settings in a simulation. Do not forget to choose default_telemetry_gvf.xml in your paparazzi center if you want to test the python App for visualization.

Python Apps

Visualizing the desired trajectory and the aircraft within the vector field

Until the "visual integration" of the GVF into the ground station is ready, one can track the vehicle and the trajectory by using the python script at "$PAPARAZZI_HOME/sw/ground_segment/python/gvf/gvfApp ac_id", where ac_id is the ID of the fixedwing to be tracked by the App.

• 
• 

Circular formations (centralized from the GCS)

One can synchronize or arrange a group of fixed-wing aircraft (assuming they have equal ground speed) in a desired circle by using the python script at "$PAPARAZZI_HOME/sw/ground_segment/python/gvf/gvfFormation". For running the script, all your aircraft MUST follow a circle employing the guidance vector field, i.e.,

<call fun="gvf_ellipse_wp(WP_CIRCLE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>

the values of gvf_ellipse_a, gvf_ellipse_b will be modified by the formation control script, and the value of gvf_ellipse_alpha is irrelevant. Note that the aircraft do not need necessarily to orbit around the same waypoint.

We need to configure three fields in a json configuration file (look at the examples in the subfolder ./formation):

• ids: it labels the aircrafts. For example, if the file contains '14 56 34', then the aircraft with ids 14, 56 and 34 are labeled with the tags 1, 2 and 3 respectively.
• topology: it defines the links between the aircraft. The file contains a matrix where the number of rows is the number of aircraft, and the number of columns is the number of desired links. A link is defined by setting 1 and -1 in a column. Following the example, the matrix ${\begin{bmatrix}1&0\\-1&1\\0&-1\end{bmatrix}}$ defines two links. The first one between the aircraft 1 and 2, and the second one between the aircraft 2 and 3.
• desired_intervehicle_angles (also known as 'sigmas'): they define the desired angles $\sigma _{k}$ between aircraft for each link $k$. Following the example, if the file contains '0 900', then the desired angle for the first link is 0.0 degrees (e.g., the aircraft 1 and 2 will meet each other) and the second desired angle will be +90.0 degrees. Only for the DISTRIBUTED version (next subsection): The angle is measured from the +1 aircraft to the -1 in the topology. In this example, the aircraft 3 will be 90 degrees AHEAD (positive) of aircraft 2. Note that 'ahead' means in the direction of the motion, so the order of the aircraft will be same, no matter whether they travel clockwise or counterclockwise in the circle.
• gain: this is the gain of the control algorithm. As an example, for three aircraft with the desired circumference of 80 meters, a value of $k=10$ works ok.
• desired_stationary_radius: sets the desired radius of the circular formation, i.e., once the aircraft are synchronized, they will be orbiting the waypoint with this desired radius.

Some useful tips:

• Watch out for "impossible" configurations, e.g., do not set up loops in the topology such as 1 -> 2 -> 3 -> 1 ${\begin{bmatrix}1&0&-1\\-1&1&0\\0&-1&1\end{bmatrix}}$, so the desired sigmas could be asking for an impossible configuration in the circle. Avoid loops! even if the desired configuration is ok, they can introduce undesired equilibria in the system, i.e., different configurations.
• The aircraft do not need to share the same center or waypoint. This script just control the sigmas.

The algorithm works by setting different radii to the aircraft. If two aircraft orbit a waypoint with different radius (assuming equal ground speeds), then the one with bigger radius travels more distance in a completed lap. We exploit this fact in order to control the different $\sigma _{k}$. We refer to Circular formation control of fixed-wing UAVs with constant speeds for details about the control signal and how to calculate an appropriated gain k.

In the following figure we show the rendezvous of three aircraft in the same circle by using this script.

Fully distributed circular formations

The air-to-air communication is possible since Paparazzi Link v2.0. You can compile Paparazzi with this new feature by typing "PPRZLINK_LIB_VERSION=2.0 make" in your terminal. The algorithm for synchronizing aircraft in a circle has been explained in the previous section "Circular formations (centralized from the GCS)". However, in that section it is explained how to run the algorithm in a centralized way, i.e., every aircraft transmit their positions to the GCS and a python script sends back the radii that the GVF must follow for each aircraft.

Now it is possible to let each aircraft to calculate on board the radius that it has to follow by exchanging information with its neighbors without any intervention from ground. An extra module together with gvf_module has to be called in the flight plan

<modules>
  <module name="gvf_module"/>
  <module name="distributed_circular_formation"/>
</modules>

and the following settings can be set in the airframe file

<section name="DCF" prefix="DCF_">
  <define name="GAIN_K" value="10"/>
  <define name="RADIUS" value="80"/>
  <define name="MAX_NEIGHBORS" value="4"/>
</section>

where GAIN_K is the control gain k explained in the centralized version (note that now each aircraft can have its own different gain), RADIUS is the target radius for the desired circumference once the synchronization is achieved, and MAX_NEIGHBORS defines the maximum number of neighbors that an aircraft can have. In particular, this last number defines how big is a table stored on board with the information about the neighbors of an aircraft. These three settings can be modified via the DCF tab from the GCS settings menu.

Each row of the table contains information about a specific neighbor. In particular there are four fields about the neighbor, its ID, the desired angle w.r.t. it, its actual sigma (IT IS CALLED THETA in the source code dcf.{c,h} and messages! sorry about the notation ;) ) and how old is the actual sigma, i.e., when was the last time the aircraft received information from its neighbor. There is a telemetry message called DCF where one can check the status of the table, where the angles are in deci-degrees, i.e., 900 is 90 degrees and the age of the information is in milliseconds.

The following messages must be included in your custom ./conf/messages.xml (if they are not present in your PaparazziLink version yet)

<message name="DCF" id="38">
  <field name="table" type="int16[]"/>
  <field name="errors" type="int16[]"/>
</message>
<message name="DCF_THETA" id="254">
  <field name="theta" type="float"/>
</message>

<message name="DCF_REG_TABLE" id="158" link="forwarded">
  <field name="ac_id" type="uint8"/>
  <field name="nei_id" type="uint8"/>
  <field name="desired_sigma" type="int16"/>
</message>
<message name="DCF_CLEAN_TABLE" id="159" link="forwarded">
  <field name="ac_id" type="uint8"/>
</message>

where the first two messages are msg class telemetry and the last two are msg class datalink. The ids are irrelevant, just look for one free for the messages.

The DCF message transmits the table (with low frequency) from the air to the ground. The DCF_THETA is the message that it is transmitted between aircraft. The DCF_REG_TABLE msg inits the table of the aircraft with a particular ID and DCF_CLEAN_TABLE just cleans the table. The python script dcfInitTables.py at ./sw/ground_segment/python/gvf will generate and send the messages based on the same text files for the centralized version of the algorithm (just check the subsection above to see how to define topology, IDs.. etc).

DO NOT FORGET to init the tables of the aircraft! You can check that the tables have been well set by checking the corresponding DCF telemetry msg.

The algorithm starts running once the aircraft is in the following block in the flight plan

<block name="Distributed_circular">
  <call fun="distributed_circular(WP_C)"/>
</block>

and if the aircraft is in AUTO2, then it starts to transmit its sigma to its neighbors. The aircraft will track (via the GVF) a circumference with center at the waypoint WP_C and the radius is determined by the algorithm. If there are not neighbors, it will just track a circumference with the default radius set in the DCF module (remember that the radius can be also changed in settings in the GCS).

Right now, the frequency for the air-to-air communication for the DCF_THETA msg has been hardcoded to 4Hz (something to change in the TODO list). If an aircraft does not receive any information from a neighbor during the last TIMEOUT millisecs, then the aircraft assumes that such a neighbor has left the formation. For example, the neighbor went to AUTO1 and the rest of the team will not consider it anymore. The TIMEOUT has been set to 1500 by default, but it can be changed for each aircraft in the settings in the GCS in the DCF tab.

TO DO list

• Better integration of the GVF with the ground station
• Support to rotorcrafts