gvf_parametric.cpp File Reference

Guiding vector field algorithm for 2D and 3D complex trajectories. More...

#include <iostream>
#include <Eigen/Dense>
#include "gvf_parametric.h"
#include "gvf_parametric_low_level_control.h"
#include "./trajectories/gvf_parametric_3d_ellipse.h"
#include "./trajectories/gvf_parametric_3d_lissajous.h"
#include "./trajectories/gvf_parametric_2d_trefoil.h"
#include "autopilot.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for gvf_parametric.cpp:

Go to the source code of this file.

Functions
static void	send_gvf_parametric (struct transport_tx *trans, struct link_device *dev)
void	gvf_parametric_init (void)
void	gvf_parametric_set_direction (int8_t s)
void	gvf_parametric_control_2D (float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
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)
bool	gvf_parametric_2D_trefoil_XY (float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
	2D TRAJECTORIES More...
bool	gvf_parametric_2D_trefoil_wp (uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
bool	gvf_parametric_3D_ellipse_XYZ (float xo, float yo, float r, float zl, float zh, float alpha)
	3D TRAJECTORIES More...
bool	gvf_parametric_3D_ellipse_wp (uint8_t wp, float r, float zl, float zh, float alpha)
bool	gvf_parametric_3D_ellipse_wp_delta (uint8_t wp, float r, float alt_center, float delta, float alpha)
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)
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)

Variables
uint32_t	gvf_parametric_t0 = 0
gvf_parametric_con	gvf_parametric_control
gvf_parametric_tra	gvf_parametric_trajectory
int	gvf_parametric_plen = 1
int	gvf_parametric_plen_wps = 0
int	gvf_parametric_elen = 3

Detailed Description

Guiding vector field algorithm for 2D and 3D complex trajectories.

Definition in file gvf_parametric.cpp.

Function Documentation

gvf_parametric_2D_trefoil_wp()

bool gvf_parametric_2D_trefoil_wp	(	uint8_t	wp,
		float	w1,
		float	w2,
		float	ratio,
		float	r,
		float	alpha
	)

Definition at line 344 of file gvf_parametric.cpp.

References alpha, gvf_parametric_2D_trefoil_XY(), gvf_parametric_plen_wps, gvf_parametric_trajectory, gvf_parametric_tra::p_parametric, waypoints, EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

gvf_parametric_2D_trefoil_XY()

bool gvf_parametric_2D_trefoil_XY	(	float	xo,
		float	yo,
		float	w1,
		float	w2,
		float	ratio,
		float	r,
		float	alpha
	)

2D TRAJECTORIES

Definition at line 322 of file gvf_parametric.cpp.

References alpha, gvf_parametric_2d_trefoil_info(), gvf_parametric_2d_trefoil_par, gvf_parametric_control_2D(), gvf_parametric_plen, gvf_parametric_plen_wps, gvf_parametric_trajectory, gvf_par_2d_tre_par::kx, gvf_par_2d_tre_par::ky, gvf_parametric_tra::p_parametric, TREFOIL_2D, and gvf_parametric_tra::type.

Referenced by gvf_parametric_2D_trefoil_wp().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_parametric_3D_ellipse_wp()

bool gvf_parametric_3D_ellipse_wp	(	uint8_t	wp,
		float	r,
		float	zl,
		float	zh,
		float	alpha
	)

Definition at line 391 of file gvf_parametric.cpp.

References alpha, gvf_parametric_3D_ellipse_XYZ(), gvf_parametric_plen_wps, gvf_parametric_trajectory, gvf_parametric_tra::p_parametric, waypoints, EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

gvf_parametric_3D_ellipse_wp_delta()

bool gvf_parametric_3D_ellipse_wp_delta	(	uint8_t	wp,
		float	r,
		float	alt_center,
		float	delta,
		float	alpha
	)

Definition at line 400 of file gvf_parametric.cpp.

References alpha, gvf_parametric_3D_ellipse_XYZ(), waypoints, EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

gvf_parametric_3D_ellipse_XYZ()

bool gvf_parametric_3D_ellipse_XYZ	(	float	xo,
		float	yo,
		float	r,
		float	zl,
		float	zh,
		float	alpha
	)

3D TRAJECTORIES

Definition at line 356 of file gvf_parametric.cpp.

References alpha, ELLIPSE_3D, gvf_parametric_3d_ellipse_info(), gvf_parametric_3d_ellipse_par, gvf_parametric_control_3D(), gvf_parametric_plen, gvf_parametric_plen_wps, gvf_parametric_trajectory, horizontal_mode, HORIZONTAL_MODE_CIRCLE, gvf_par_3d_ell_par::kx, gvf_par_3d_ell_par::ky, gvf_par_3d_ell_par::kz, gvf_parametric_tra::p_parametric, and gvf_parametric_tra::type.

Referenced by gvf_parametric_3D_ellipse_wp(), and gvf_parametric_3D_ellipse_wp_delta().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_parametric_3D_lissajous_wp_center()

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
	)

Definition at line 446 of file gvf_parametric.cpp.

References alpha, gvf_parametric_3D_lissajous_XYZ(), gvf_parametric_plen_wps, gvf_parametric_trajectory, gvf_parametric_tra::p_parametric, waypoints, EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

gvf_parametric_3D_lissajous_XYZ()

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
	)

Definition at line 411 of file gvf_parametric.cpp.

References alpha, gvf_parametric_3d_lissajous_info(), gvf_parametric_3d_lissajous_par, gvf_parametric_control_3D(), gvf_parametric_plen, gvf_parametric_plen_wps, gvf_parametric_trajectory, gvf_par_3d_lis_par::kx, gvf_par_3d_lis_par::ky, gvf_par_3d_lis_par::kz, LISSAJOUS_3D, gvf_parametric_tra::p_parametric, and gvf_parametric_tra::type.

Referenced by gvf_parametric_3D_lissajous_wp_center().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_parametric_control_2D()

void gvf_parametric_control_2D	(	float	kx,
		float	ky,
		float	f1,
		float	f2,
		float	f1d,
		float	f2d,
		float	f1dd,
		float	f2dd
	)

Definition at line 118 of file gvf_parametric.cpp.

References gvf_parametric_con::beta, course, gvf_parametric_con::delta_T, desired_x, desired_y, get_sys_time_msec(), ground_speed, gvf_parametric_control, gvf_parametric_elen, gvf_parametric_low_level_control_2D(), gvf_parametric_t0, gvf_parametric_trajectory, h(), I, J, gvf_parametric_con::k_psi, gvf_parametric_con::L, gvf_parametric_tra::phi_errors, gvf_parametric_con::s, stateGetHorizontalSpeedDir_f(), stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), stateGetSpeedEnu_f(), gvf_parametric_con::w, EnuCoor_f::x, mesonh.mesonh_atmosphere::X, and EnuCoor_f::y.

Referenced by gvf_parametric_2D_trefoil_XY().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_parametric_control_3D()

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
	)

Definition at line 212 of file gvf_parametric.cpp.

References gvf_parametric_con::beta, course, gvf_parametric_con::delta_T, desired_x, desired_y, E, get_sys_time_msec(), ground_speed, gvf_parametric_control, gvf_parametric_elen, gvf_parametric_low_level_control_3D(), gvf_parametric_t0, gvf_parametric_trajectory, h(), I, J, gvf_parametric_con::k_psi, gvf_parametric_con::L, gvf_parametric_tra::phi_errors, gvf_parametric_con::s, stateGetHorizontalSpeedDir_f(), stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), stateGetSpeedEnu_f(), gvf_parametric_con::w, EnuCoor_f::x, mesonh.mesonh_atmosphere::X, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by gvf_parametric_3D_ellipse_XYZ(), and gvf_parametric_3D_lissajous_XYZ().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_parametric_init()

void gvf_parametric_init

(

void

)

Definition at line 93 of file gvf_parametric.cpp.

References gvf_parametric_con::beta, DefaultPeriodic, gvf_parametric_con::delta_T, gvf_parametric_control, GVF_PARAMETRIC_CONTROL_BETA, GVF_PARAMETRIC_CONTROL_KCLIMB, GVF_PARAMETRIC_CONTROL_KPSI, GVF_PARAMETRIC_CONTROL_KROLL, GVF_PARAMETRIC_CONTROL_L, gvf_parametric_con::k_climb, gvf_parametric_con::k_psi, gvf_parametric_con::k_roll, gvf_parametric_con::L, register_periodic_telemetry(), gvf_parametric_con::s, send_gvf_parametric(), and gvf_parametric_con::w.

Here is the call graph for this function:

gvf_parametric_set_direction()

void gvf_parametric_set_direction

(

int8_t

s

)

Definition at line 113 of file gvf_parametric.cpp.

References gvf_parametric_control, gvf_parametric_con::s, and s.

send_gvf_parametric()

static void send_gvf_parametric ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 58 of file gvf_parametric.cpp.

References gvf_parametric_con::beta, dev, get_sys_time_msec(), gvf_parametric_control, gvf_parametric_elen, gvf_parametric_plen, gvf_parametric_t0, gvf_parametric_trajectory, gvf_parametric_tra::p_parametric, gvf_parametric_tra::phi_errors, gvf_parametric_con::s, gvf_parametric_tra::type, and gvf_parametric_con::w.

Referenced by gvf_parametric_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

gvf_parametric_control

gvf_parametric_con gvf_parametric_control

Definition at line 44 of file gvf_parametric.cpp.

Referenced by gvf_parametric_2d_trefoil_info(), gvf_parametric_3d_ellipse_info(), gvf_parametric_3d_lissajous_info(), gvf_parametric_control_2D(), gvf_parametric_control_3D(), gvf_parametric_init(), gvf_parametric_low_level_control_2D(), gvf_parametric_low_level_control_3D(), gvf_parametric_set_direction(), and send_gvf_parametric().

gvf_parametric_elen

int gvf_parametric_elen = 3

Definition at line 54 of file gvf_parametric.cpp.

Referenced by gvf_parametric_control_2D(), gvf_parametric_control_3D(), and send_gvf_parametric().

gvf_parametric_plen

int gvf_parametric_plen = 1

Definition at line 50 of file gvf_parametric.cpp.

Referenced by gvf_parametric_2D_trefoil_XY(), gvf_parametric_3D_ellipse_XYZ(), gvf_parametric_3D_lissajous_XYZ(), and send_gvf_parametric().

gvf_parametric_plen_wps

int gvf_parametric_plen_wps = 0

Definition at line 51 of file gvf_parametric.cpp.

Referenced by gvf_parametric_2D_trefoil_wp(), gvf_parametric_2D_trefoil_XY(), gvf_parametric_3D_ellipse_wp(), gvf_parametric_3D_ellipse_XYZ(), gvf_parametric_3D_lissajous_wp_center(), and gvf_parametric_3D_lissajous_XYZ().

gvf_parametric_t0

uint32_t gvf_parametric_t0 = 0

Definition at line 43 of file gvf_parametric.cpp.

Referenced by gvf_parametric_control_2D(), gvf_parametric_control_3D(), and send_gvf_parametric().

gvf_parametric_trajectory

gvf_parametric_tra gvf_parametric_trajectory

Definition at line 47 of file gvf_parametric.cpp.

Referenced by gvf_parametric_2d_trefoil_info(), gvf_parametric_2D_trefoil_wp(), gvf_parametric_2D_trefoil_XY(), gvf_parametric_3d_ellipse_info(), gvf_parametric_3D_ellipse_wp(), gvf_parametric_3D_ellipse_XYZ(), gvf_parametric_3d_lissajous_info(), gvf_parametric_3D_lissajous_wp_center(), gvf_parametric_3D_lissajous_XYZ(), gvf_parametric_control_2D(), gvf_parametric_control_3D(), and send_gvf_parametric().