87 changes: 82 additions & 5 deletions sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
View file
Expand Up @@ -32,6 +32,8 @@
#include "./trajectories/gvf_parametric_3d_ellipse.h"
#include "./trajectories/gvf_parametric_3d_lissajous.h"
#include "./trajectories/gvf_parametric_2d_trefoil.h"
#include "./trajectories/gvf_parametric_2d_bezier_splines.h"
#include "../gvf_common.h"

#ifdef __cplusplus
extern "C" {
Expand All @@ -41,6 +43,7 @@ extern "C" {

// Control
uint32_t gvf_parametric_t0 = 0; // We need it for calculting the time lapse delta_T
uint32_t gvf_parametric_splines_ctr = 0; // We need it for Bézier curves splines Telemetry
gvf_parametric_con gvf_parametric_control;

// Trajectory
Expand All @@ -53,6 +56,9 @@ int gvf_parametric_plen_wps = 0;
// Error signals array lenght
int gvf_parametric_elen = 3;

// Bézier
bezier_t gvf_bezier_2D[GVF_PARAMETRIC_2D_BEZIER_N_SEG];

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
Expand All @@ -65,6 +71,7 @@ static void send_gvf_parametric(struct transport_tx *trans, struct link_device *
float wb = gvf_parametric_control.w * gvf_parametric_control.beta;

if (delta_T < 200) {
gvf_parametric_splines_ctr = (gvf_parametric_splines_ctr + 1) % 3;
pprz_msg_send_GVF_PARAMETRIC(trans, dev, AC_ID, &traj_type, &gvf_parametric_control.s, &wb, gvf_parametric_plen,
gvf_parametric_trajectory.p_parametric, gvf_parametric_elen, gvf_parametric_trajectory.phi_errors);
}
Expand Down Expand Up @@ -128,8 +135,10 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
}

// Carrot position
#ifdef FIXEDWING_FIRMWARE
desired_x = f1;
desired_y = f2;
#endif

float L = gvf_parametric_control.L;
float beta = gvf_parametric_control.beta * gvf_parametric_control.s;
Expand Down Expand Up @@ -202,13 +211,19 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
sqrtf(Xt * G * X));

// From gvf_common.h TODO: implement d/dt of kppa and ori_err
gvf_c_omega.omega = heading_rate;
gvf_c_info.kappa = (f1d * f2dd - f1dd * f2d) / powf(f1d * f1d + f2d * f2d, 1.5);
gvf_c_info.ori_err = 1 - (Xh(0) * cosf(course) + Xh(1) * sinf(course));

// Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
// the vehicle. So it is not only okei but advisable to update it.
gvf_parametric_control.w += w_dot * gvf_parametric_control.delta_T * 1e-3;

gvf_parametric_low_level_control_2D(heading_rate);
}

#ifdef FIXEDWING_FIRMWARE
void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2, float f3, float f1d, float f2d,
float f3d, float f1dd, float f2dd, float f3dd)
{
Expand Down Expand Up @@ -315,6 +330,7 @@ void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2,

gvf_parametric_low_level_control_3D(heading_rate, climbing_rate);
}
#endif // FIXED_WING FIRMWARE

/** 2D TRAJECTORIES **/
// 2D TREFOIL KNOT
Expand Down Expand Up @@ -342,17 +358,77 @@ bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float
}

bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
{
{
gvf_parametric_trajectory.p_parametric[7] = wp;
gvf_parametric_plen_wps = 1;
gvf_parametric_2D_trefoil_XY(WaypointX(wp), WaypointY(wp), w1, w2, ratio, r, alpha);
return true;
}

// 2D CUBIC BEZIER CURVE
bool gvf_parametric_2D_bezier_XY(void)
{
gvf_parametric_trajectory.type = BEZIER_2D;
float fx, fy, fxd, fyd, fxdd, fydd;
gvf_parametric_2d_bezier_splines_info(gvf_bezier_2D, &fx, &fy, &fxd, &fyd, &fxdd, &fydd);
gvf_parametric_control_2D(gvf_parametric_2d_bezier_par.kx, gvf_parametric_2d_bezier_par.ky, fx, fy, fxd, fyd, fxdd,
fydd);
return true;
}

/* @param first_wp is the first waypoint of the Bézier Spline
* there should be 3*GVF_PARAMETRIC_2D_BEZIER_N_SEG+1 points
*/
bool gvf_parametric_2D_bezier_wp(uint8_t first_wp)
{
float x[3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1];
float y[3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1];
int k;
for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
x[k] = WaypointX(first_wp + k);
y[k] = WaypointY(first_wp + k);
}
create_bezier_spline(gvf_bezier_2D, x, y);

/* Send data piecewise. Some radio modules do not allow for a big data frame.*/

// Send x points -> Indicate x with sign (+) in the first parameter
if (gvf_parametric_splines_ctr == 0) {
gvf_parametric_trajectory.p_parametric[0] = -GVF_PARAMETRIC_2D_BEZIER_N_SEG; // send x (negative value)
for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
gvf_parametric_trajectory.p_parametric[k + 1] = x[k];
}
}
// Send y points -> Indicate y with sign (-) in the first parameter
else if (gvf_parametric_splines_ctr == 1) {
gvf_parametric_trajectory.p_parametric[0] = GVF_PARAMETRIC_2D_BEZIER_N_SEG; // send y (positive value)
for (k = 0; k < 3 * GVF_PARAMETRIC_2D_BEZIER_N_SEG + 1; k++) {
gvf_parametric_trajectory.p_parametric[k + 1] = y[k];
}
}
// send kx, ky, beta and anything else needed..
else {
gvf_parametric_trajectory.p_parametric[0] = 0.0;
gvf_parametric_trajectory.p_parametric[1] = gvf_parametric_2d_bezier_par.kx;
gvf_parametric_trajectory.p_parametric[2] = gvf_parametric_2d_bezier_par.ky;
gvf_parametric_trajectory.p_parametric[3] = gvf_parametric_control.beta;
}
gvf_parametric_plen = 16;
gvf_parametric_plen_wps = 1;

gvf_parametric_2D_trefoil_XY(waypoints[wp].x, waypoints[wp].y, w1, w2, ratio, r, alpha);
// restart the spline
if (gvf_parametric_control.w >= (float)GVF_PARAMETRIC_2D_BEZIER_N_SEG) {
gvf_parametric_control.w = 0;
} else if (gvf_parametric_control.w < 0) {
gvf_parametric_control.w = 0;
}
gvf_parametric_2D_bezier_XY();
return true;
}

/** 3D TRAJECTORIES **/
// 3D ELLIPSE

#ifdef FIXEDWING_FIRMWARE
bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
{
horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
Expand Down Expand Up @@ -389,7 +465,7 @@ bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float
}

bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
{
{
gvf_parametric_trajectory.p_parametric[6] = wp;
gvf_parametric_plen_wps = 1;

Expand Down Expand Up @@ -445,10 +521,11 @@ bool gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, flo

bool 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)
{
{
gvf_parametric_trajectory.p_parametric[13] = wp;
gvf_parametric_plen_wps = 1;

gvf_parametric_3D_lissajous_XYZ(waypoints[wp].x, waypoints[wp].y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha);
return true;
}
#endif // FIXEDWING_FIRMWARE
9 changes: 9 additions & 0 deletions sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h
View file
Expand Up @@ -66,6 +66,7 @@ extern "C" {
#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h"
#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h"
#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h"
#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h"

/** @typedef gvf_parametric_con
* @brief Control parameters for the GVF_PARAMETRIC
Expand Down Expand Up @@ -93,6 +94,7 @@ enum trajectories_parametric {
TREFOIL_2D = 0,
ELLIPSE_3D = 1,
LISSAJOUS_3D = 2,
BEZIER_2D = 3,
NONE_PARAMETRIC = 255,
};

Expand All @@ -104,6 +106,9 @@ typedef struct {

extern gvf_parametric_tra gvf_parametric_trajectory;

// Bezier struct
extern bezier_t gvf_bezier_2D[GVF_PARAMETRIC_2D_BEZIER_N_SEG];

// Init function
extern void gvf_parametric_init(void);

Expand All @@ -117,6 +122,10 @@ extern void gvf_parametric_control_3D(float, float, float, float, float, float,
extern bool gvf_parametric_2D_trefoil_XY(float, float, float, float, float, float, float);
extern bool gvf_parametric_2D_trefoil_wp(uint8_t, float, float, float, float, float);

// 2D BEZIER
extern bool gvf_parametric_2D_bezier_wp(uint8_t);
extern bool gvf_parametric_2D_bezier_XY(void);

// 3D Ellipse
extern bool gvf_parametric_3D_ellipse_XYZ(float, float, float, float, float, float);
extern bool gvf_parametric_3D_ellipse_wp(uint8_t, float, float, float, float);
Expand Down
4 changes: 4 additions & 0 deletions sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c
View file
Expand Up @@ -47,6 +47,8 @@ void gvf_parametric_low_level_control_2D(float heading_rate)
-gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta));
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
}
// Allow for rover operation
#elif defined(ROVER_FIRMWARE)
#else
#error gvf_parametric does not support your firmware yet
#endif
Expand All @@ -72,6 +74,8 @@ void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate
-gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta));
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
}
// Allow for rover operation
#elif defined(ROVER_FIRMWARE)
#else
#error gvf_parametric does not support your firmware yet
#endif
Expand Down
92 changes: 92 additions & 0 deletions sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.c
View file
@@ -0,0 +1,92 @@
/*
* Copyright (C) 2023 Alfredo Gonzalez Calvin <alfredgo@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

#include "modules/nav/common_nav.h"
#include "modules/guidance/gvf_parametric/gvf_parametric.h"
#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h"

#ifndef GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX
#define GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX 2.0
#endif

#ifndef GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY
#define GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY 2.0
#endif

gvf_par_2d_bezier_par gvf_parametric_2d_bezier_par = {GVF_PARAMETRIC_2D_BEZIER_SPLINES_KX,
GVF_PARAMETRIC_2D_BEZIER_SPLINES_KY
};

// Bezier is just an array
void create_bezier_spline(bezier_t *bezier, float *px, float *py)
{

int k, j;
j = 0;
for (k = 0; k < GVF_PARAMETRIC_2D_BEZIER_N_SEG; k++) {
bezier[k].p0[0] = px[j];
bezier[k].p0[1] = py[j];
bezier[k].p1[0] = px[j + 1];
bezier[k].p1[1] = py[j + 1];
bezier[k].p2[0] = px[j + 2];
bezier[k].p2[1] = py[j + 2];
bezier[k].p3[0] = px[j + 3];
bezier[k].p3[1] = py[j + 3];

// This allows for C^0 continuity (last point is init point)
j += 3;
}
}

void gvf_parametric_2d_bezier_splines_info(bezier_t *bezier, float *f1, float *f2, float *f1d, float *f2d, float *f1dd,
float *f2dd)
{
// How can we select in which bezier curve are we? Check w. spline zero: 0 <= t <= 1, spline ones: 1 <= t <= 2;
float t = gvf_parametric_control.w;
int n_seg = floorl(t);
float tt = t - n_seg;
if (n_seg < 0) {
n_seg = 0; // w could be < 0 in that case go to first point of first segment
tt = 0;
}
// Evalute the corresponding bezier curve
float p0x = bezier[n_seg].p0[0]; float p0y = bezier[n_seg].p0[1];
float p1x = bezier[n_seg].p1[0]; float p1y = bezier[n_seg].p1[1];
float p2x = bezier[n_seg].p2[0]; float p2y = bezier[n_seg].p2[1];
float p3x = bezier[n_seg].p3[0]; float p3y = bezier[n_seg].p3[1];

// Bézier curves

// Curve (x,y)
*f1 = (1 - tt) * (1 - tt) * (1 - tt) * p0x + 3 * (1 - tt) * (1 - tt) * tt * p1x + 3 *
(1 - tt) * tt * tt * p2x + tt * tt * tt * p3x;
*f2 = (1 - tt) * (1 - tt) * (1 - tt) * p0y + 3 * (1 - tt) * (1 - tt) * tt * p1y + 3 *
(1 - tt) * tt * tt * p2y + tt * tt * tt * p3y;

// First derivative
*f1d = 3 * (1 - tt) * (1 - tt) * (p1x - p0x) + 6 * (1 - tt) * tt * (p2x - p1x) + 3 * tt * tt * (p3x - p2x);
*f2d = 3 * (1 - tt) * (1 - tt) * (p1y - p0y) + 6 * (1 - tt) * tt * (p2y - p1y) + 3 * tt * tt * (p3y - p2y);

// Second derivative
*f1dd = 6 * (1 - tt) * (p2x - 2 * p1x + p0x) + 6 * tt * (p3x - 2 * p2x + p1x);
*f2dd = 6 * (1 - tt) * (p2y - 2 * p1y + p0y) + 6 * tt * (p3y - 2 * p2y + p1y);


}
65 changes: 65 additions & 0 deletions sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h
View file
@@ -0,0 +1,65 @@
/*
* Copyright (C) 2023 Alfredo Gonzalez Calvin <alfredgo@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/

#ifndef GVF_PARAMETRIC_2D_BEZIER_SPLINES_H
#define GVF_PARAMETRIC_2D_BEZIER_SPLINES_H


// Define only one segment by default
#ifndef GVF_PARAMETRIC_2D_BEZIER_N_SEG
#define GVF_PARAMETRIC_2D_BEZIER_N_SEG 1
#endif


#ifdef __cplusplus
extern "C" {
#endif


typedef struct {
float kx;
float ky;
} gvf_par_2d_bezier_par;


// Cubic bezier
typedef struct {
float p0[2];
float p1[2];
float p2[2];
float p3[2];
} bezier_t;


extern gvf_par_2d_bezier_par gvf_parametric_2d_bezier_par;

extern void create_bezier_spline(bezier_t *bezier, float *px, float *py);
extern void gvf_parametric_2d_bezier_splines_info(bezier_t *bezier, float *f1, float *f2, float *f1d, float *f2d,
float *f1dd, float *f2dd);

#ifdef __cplusplus
}
#endif





#endif // bezier splines