Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-06 02:52:42 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

Support for rovers and third order Bézier splines in GVF parametric (#3145)

Browse Source
This commit is contained in:
Alfredo González Calvin
2023-11-03 21:30:53 +01:00
committed by GitHub
parent adc25c4db6
commit 26d7c9055e
18 changed files with 435 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
conf/airframes/UCM/rover_steering.xml
View File

@@ -50,6 +50,10 @@
<module name="gvf" type="module">
<define name="GVF_OCAML_GCS" value="FALSE"/>
</module>
<module name="gvf_parametric">
<define name="GVF_PARAMETRIC_2D_BEZIER_N_SEG" value="4"/>
</module>
<module name="guidance" type="rover_steering">
<define name="MAX_DELTA" value="15.0"/>

4
conf/autopilot/rover_steering.xml
View File

@@ -11,7 +11,7 @@
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/guidance/gvf/gvf.h"/>
<include name="modules/guidance/gvf_common.h"/>
<include name="navigation.h"/>
<include name="state.h"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
@@ -78,7 +78,7 @@
</control>
<control>
<call fun="rover_guidance_steering_speed_ctrl()"/>
<call fun="rover_guidance_steering_heading_ctrl(gvf_control.omega)"/>
<call fun="rover_guidance_steering_heading_ctrl(gvf_c_omega.omega)"/>
<call fun="commands[COMMAND_STEERING] = GetCmdFromDelta(guidance_control.cmd.delta);"/>
<call fun="commands[COMMAND_THROTTLE] = GetCmdFromThrottle(guidance_control.throttle);"/>

6
conf/flight_plans/UCM/fixedwing_gvfMission.xml
View File

@@ -11,7 +11,7 @@
<waypoint alt="700.0" name="CLIMB" x="-30" y="-40"/>
<waypoint alt="700.0" name="P1" x="-20" y="-40"/>
<waypoint alt="700.0" name="P2" x="20" y="-40"/>
<!-- Sectors waypoints -->
<waypoint name="_S1" x="3" y="4"/>
<waypoint name="_S2" x="3" y="-4"/>
@@ -106,9 +106,9 @@
<block name="2D_trefoil_wp" strip_icon="eight.png">
<call fun="gvf_parametric_2D_trefoil_wp(WP_P1, gvf_parametric_2d_trefoil_par.w1, gvf_parametric_2d_trefoil_par.w2, gvf_parametric_2d_trefoil_par.ratio, gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
</block>
<block name="3D_ellipse_wp" strip_icon="oval.png">
<call fun="gvf_parametric_3D_ellipse_wp(WP_ELLIPSE, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
<call fun="gvf_parametric_3D_ellipse_wp(WP_ELLIPSE, gvf_parametric_3d_ellipse_par.r, gvf_parametric_3d_ellipse_par.zl, gvf_parametric_3d_ellipse_par.zh, gvf_parametric_3d_ellipse_par.alpha)"/>
</block>
<block name="3D_lissajous_wp" strip_icon="eight.png">

31
conf/flight_plans/UCM/steering_rover_gvfMission.xml
View File

@@ -1,5 +1,5 @@
<?xml version="1.0"?>
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="660" ground_alt="650" lat0="40.450631" lon0="-3.726058" max_dist_from_home="1500" name="Rover Steering" security_height="0.3">
<header>
@@ -7,9 +7,9 @@
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="STDBY" x="0" y="-4"/>
<waypoint name="ELLIPSE" x="-3" y="-3"/>
<waypoint name="P1" x="-2" y="-4"/>
<waypoint name="P2" x="2" y="-4"/>
<waypoint name="ELLIPSE" x="-6.116" y="-8.525"/>
<waypoint name="P1" x="-4.39" y="-0.738"/>
<waypoint name="P2" x="-7.046" y="-4.57"/>
<!--waypoint name="OBJ" x="2" y="-4"/-->
<!-- Sectors waypoints -->
@@ -21,8 +21,25 @@
<waypoint name="_N2" x="100" y="-100"/>
<waypoint name="_N3" x="-100" y="-100"/>
<waypoint name="_N4" x="-100" y="100"/>
<!-- Bézier Waypoints. Define them in order -->
<waypoint name="BZ0" x="1.7" y="-6.0"/>
<waypoint name="BZ1" x="-1.4" y="-14.7"/>
<waypoint name="BZ2" x="-0.688" y="-24.723"/>
<waypoint name="BZ3" x="6.3" y="-26.2"/>
<waypoint name="BZ4" x="16.9" y="-30.7"/>
<waypoint name="BZ5" x="25.6" y="-30.0"/>
<waypoint name="BZ6" x="31.0" y="-22.6"/>
<waypoint name="BZ7" x="33.4" y="-14.7"/>
<waypoint name="BZ8" x="32.3" y="-7.0"/>
<waypoint name="BZ9" x="27.3" y="-2.0"/>
<waypoint name="BZ10" x="19.7" y="0.6"/>
<waypoint name="BZ11" x="12.6" y="1.6"/>
<waypoint name="BZ12" x="8.373" y="-2.597"/>
</waypoints>
<sectors>
<sector name="Net" color="red">
<corner name="_N1"/>
@@ -90,5 +107,11 @@
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
</block>
<block name="2D_bezier" strip_icon="eight.png">
<call fun="gvf_parametric_2D_bezier_wp(WP_BZ0)"/>
<exception cond="! InsideNet(GetPosX(), GetPosY())" deroute="Standby"/>
</block>
</blocks>
</flight_plan>

24
conf/modules/gvf_common.xml Normal file
View File

@@ -0,0 +1,24 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="gvf_common" dir="guidance/">
<doc>
<description>
Common file to allow both gvf to work together.
Still requires at least one module providing the actual gvf implementation
</description>
</doc>
<header>
<file name="gvf_common.h"/>
</header>
<makefile firmware="fixedwing">
<file name="gvf_common.c"/>
</makefile>
<makefile firmware="rover">
<file name="gvf_common.c"/>
</makefile>
</module>

6
conf/modules/gvf_module.xml
View File

@@ -59,7 +59,11 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>gvf_common</depends>
</dep>
<header>
<file name="gvf.h"/>
<file name="gvf_low_level_control.h"/>

32
conf/modules/gvf_parametric.xml
View File

@@ -29,6 +29,10 @@
<dl_setting MAX="200" MIN="10" STEP="5" VAR="gvf_parametric_2d_trefoil_par.r" shortname="2d_tre_r" param="GVF_PARAMETRIC_2D_TREFOIL_R"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_2d_trefoil_par.alpha" shortname="2d_tre_alpha" param="GVF_PARAMETRIC_2D_TREFOIL_ALPHA"/>
</dl_settings>
<dl_settings NAME="2D_bezier">
<dl_setting MAX="9.750" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_2d_bezier_par.kx" shortname="2d_bezier_kx" param="GVF_PARAMETRIC_2D_BEZIER_KX"/>
<dl_setting MAX="9.750" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_2d_bezier_par.ky" shortname="2d_bezier_ky" param="GVF_PARAMETRIC_2D_BEZIER_KY"/>
</dl_settings>
<dl_settings NAME="3D_ellipse">
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_ellipse_par.kx" shortname="3d_ell_kx" param="GVF_PARAMETRIC_3D_ELLIPSE_KX"/>
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_ellipse_par.ky" shortname="3d_ell_ky" param="GVF_PARAMETRIC_3D_ELLIPSE_KY"/>
@@ -53,16 +57,22 @@
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_lissajous_par.dz" shortname="3d_lis_dz" param="GVF_PARAMETRIC_3D_LISSAJOUS_DZ"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_lissajous_par.alpha" shortname="3d_lis_alpha" param="GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA"/>
</dl_settings>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>gvf_common</depends>
</dep>
<header>
<file name="gvf_parametric.h"/>
<file name="gvf_parametric_low_level_control.h"/>
<file name="trajectories/gvf_parametric_3d_ellipse.h"/>
<file name="trajectories/gvf_parametric_3d_lissajous.h"/>
<file name="trajectories/gvf_parametric_2d_trefoil.h"/>
<file name="trajectories/gvf_parametric_2d_bezier_splines.h"/>
</header>
<init fun = "gvf_parametric_init()"/>
@@ -78,10 +88,30 @@
<file name="trajectories/gvf_parametric_3d_ellipse.c"/>
<file name="trajectories/gvf_parametric_3d_lissajous.c"/>
<file name="trajectories/gvf_parametric_2d_trefoil.c"/>
<file name="trajectories/gvf_parametric_2d_bezier_splines.c"/>
<flag name="LDFLAGS" value="lstdc++" />
</makefile>
<makefile target="ap|nps" firmware="rover">
<configure name="CXXSTANDARD" value="-std=c++14"/>
<flag name="CXXFLAGS" value="Wno-int-in-bool-context -Wno-deprecated-copy"/>
<include name="$(PAPARAZZI_SRC)/sw/ext/eigen"/>
<define name="EIGEN_NO_MALLOC"/>
<define name="EIGEN_NO_AUTOMATIC_RESIZING"/>
<file name="gvf_parametric.cpp"/>
<file name="gvf_parametric_low_level_control.c"/>
<file name="trajectories/gvf_parametric_3d_ellipse.c"/>
<file name="trajectories/gvf_parametric_3d_lissajous.c"/>
<file name="trajectories/gvf_parametric_2d_trefoil.c"/>
<file name="trajectories/gvf_parametric_2d_bezier_splines.c"/>
<flag name="LDFLAGS" value="lstdc++" />
</makefile>
<makefile target="ap" firmware="fixedwing">
<define name="EIGEN_NO_DEBUG"/>
</makefile>
<makefile target="ap" firmware="rover">
<define name="EIGEN_NO_DEBUG"/>
</makefile>
</module>

2
conf/telemetry/GVF/gvf_default_fixedwing.xml
View File

@@ -29,7 +29,7 @@
<message name="AIR_DATA" period="1.3"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="GVF" period="3."/>
<message name="GVF_PARAMETRIC" period="3."/>
<message name="GVF_PARAMETRIC" period="0.5"/>
</mode>
<mode name="minimal">
<message name="ALIVE" period="5"/>

1
conf/telemetry/GVF/gvf_rover.xml
View File

@@ -25,6 +25,7 @@
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="GVF" period="1.0"/>
<message name="GVF_PARAMETRIC" period="0.5"/>
</mode>
<mode name="low" key_press="l">

4
conf/userconf/GVF/gvf_conf.xml
View File

@@ -7,7 +7,7 @@
telemetry="telemetry/GVF/gvf_default_fixedwing.xml"
flight_plan="flight_plans/UCM/fixedwing_gvfMission.xml"
settings="settings/fixedwing_basic.xml"
settings_modules="modules/ahrs_float_dcm.xml modules/gps.xml modules/guidance_full_pid_fw.xml modules/gvf_module.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_full_pid_fw.xml modules/gvf_module.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
gui_color="blue"
/>
<aircraft
@@ -18,7 +18,7 @@
telemetry="telemetry/GVF/gvf_rover.xml"
flight_plan="flight_plans/UCM/steering_rover_gvfMission.xml"
settings=""
settings_modules="modules/ahrs_float_dcm.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rover_steering.xml modules/gvf_module.xml~GVF~ modules/imu_common.xml modules/nav_rover_base.xml"
settings_modules="modules/ahrs_float_dcm.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_rover_steering.xml~SR Guidance~ modules/gvf_module.xml~GVF~ modules/gvf_parametric.xml~GVF_PARAMETRIC~ modules/imu_common.xml modules/nav_rover_base.xml"
gui_color="blue"
/>
</conf>

6
sw/airborne/modules/guidance/gvf/gvf.c
View File

@@ -29,6 +29,7 @@
#include "modules/guidance/gvf/trajectories/gvf_line.h"
#include "modules/guidance/gvf/trajectories/gvf_sin.h"
#include "autopilot.h"
#include "../gvf_common.h"
// Control
gvf_con gvf_control;
@@ -183,6 +184,11 @@ void gvf_control_2D(float ke, float kn, float e,
float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
gvf_control.omega = omega;
// From gvf_common.h TODO: derivative of curvature and ori_err
gvf_c_omega.omega = omega;
gvf_c_info.kappa = (nx*(H12*ny - nx*H22) + ny*(H21*nx - H11*ny))/powf(nx*nx + ny*ny,1.5);
gvf_c_info.ori_err = 1 - (md_x*cosf(course) + md_y*sinf(course));
gvf_low_level_control_2D(omega);
}

24
sw/airborne/modules/guidance/gvf_common.c Normal file
View File

@@ -0,0 +1,24 @@
/*
* 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 "./gvf_common.h"
gvf_common_omega gvf_c_omega;
gvf_common_params gvf_c_info;

53
sw/airborne/modules/guidance/gvf_common.h Normal file
View File

@@ -0,0 +1,53 @@
/*
* 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_COMMON_H
#define GVF_COMMON_H
// uint32_t
#include "std.h"
/** @typedef gvf_common_omega
* @brief Horizontal control signal for both gvf
* @param omega is the horizontal control signal
*/
typedef struct {
float omega;
} gvf_common_omega;
extern gvf_common_omega gvf_c_omega;
/** @typedef gvf_common_params
* @brief Different parameters obtained from gvfs. dot means d/dt
* @param kappa is the curve's curvature
* @param ori_err is the orientation error
*/
typedef struct {
float kappa;
float kappa_dot;
float ori_err;
float ori_err_dot;
} gvf_common_params;
extern gvf_common_params gvf_c_info;
#endif // GVF_COMMON_H

87
sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
View File

@@ -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" {
@@ -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
@@ -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)
@@ -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);
}
@@ -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;
@@ -202,6 +211,11 @@ 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;
@@ -209,6 +223,7 @@ void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d
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)
{
@@ -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
@@ -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;
}
gvf_parametric_2D_trefoil_XY(waypoints[wp].x, waypoints[wp].y, w1, w2, ratio, r, alpha);
// 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;
// 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
@@ -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;
@@ -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
sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h
View File

@@ -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
@@ -93,6 +94,7 @@ enum trajectories_parametric {
TREFOIL_2D = 0,
ELLIPSE_3D = 1,
LISSAJOUS_3D = 2,
BEZIER_2D = 3,
NONE_PARAMETRIC = 255,
};
@@ -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);
@@ -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);

4
sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c
View File

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

92
sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.c Normal file
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
sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_bezier_splines.h Normal file
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