Skip to content

Commit

Permalink
Support for rovers and third order Bézier splines in GVF parametric (#…
Browse files Browse the repository at this point in the history
  • Loading branch information
alfredoFBW committed Nov 3, 2023
1 parent adc25c4 commit 26d7c90
Show file tree
Hide file tree
Showing 18 changed files with 435 additions and 19 deletions.
4 changes: 4 additions & 0 deletions conf/airframes/UCM/rover_steering.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down
4 changes: 2 additions & 2 deletions conf/autopilot/rover_steering.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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)"/>
Expand Down Expand Up @@ -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);"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/flight_plans/UCM/fixedwing_gvfMission.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down Expand Up @@ -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">
Expand Down
31 changes: 27 additions & 4 deletions conf/flight_plans/UCM/steering_rover_gvfMission.xml
Original file line number Diff line number Diff line change
@@ -1,15 +1,15 @@
<?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>

</header>
<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 -->
Expand All @@ -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"/>
Expand Down Expand Up @@ -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 changes: 24 additions & 0 deletions conf/modules/gvf_common.xml
Original file line number Diff line number Diff line change
@@ -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 changes: 5 additions & 1 deletion conf/modules/gvf_module.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down
32 changes: 31 additions & 1 deletion conf/modules/gvf_parametric.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand All @@ -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()"/>
Expand All @@ -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 changes: 1 addition & 1 deletion conf/telemetry/GVF/gvf_default_fixedwing.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down
1 change: 1 addition & 0 deletions conf/telemetry/GVF/gvf_rover.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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">
Expand Down
4 changes: 2 additions & 2 deletions conf/userconf/GVF/gvf_conf.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 changes: 6 additions & 0 deletions sw/airborne/modules/guidance/gvf/gvf.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

Expand Down
24 changes: 24 additions & 0 deletions sw/airborne/modules/guidance/gvf_common.c
Original file line number Diff line number Diff line change
@@ -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 changes: 53 additions & 0 deletions sw/airborne/modules/guidance/gvf_common.h
Original file line number Diff line number Diff line change
@@ -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

0 comments on commit 26d7c90

Please sign in to comment.