gvf.c File Reference

#include <math.h>
#include "std.h"
#include "modules/guidance/gvf/gvf.h"
#include "modules/guidance/gvf/trajectories/gvf_ellipse.h"
#include "modules/guidance/gvf/trajectories/gvf_line.h"
#include "modules/guidance/gvf/trajectories/gvf_sin.h"
#include "firmwares/fixedwing/nav.h"
#include "subsystems/navigation/common_nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "autopilot.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for gvf.c:

Go to the source code of this file.

Functions
static void	send_gvf (struct transport_tx *trans, struct link_device *dev)
static void	send_circle (struct transport_tx *trans, struct link_device *dev)
static void	send_segment (struct transport_tx *trans, struct link_device *dev)
static int	out_of_segment_area (float x1, float y1, float x2, float y2, float d1, float d2)
void	gvf_init (void)
void	gvf_control_2D (float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess)
void	gvf_set_direction (int8_t s)
static void	gvf_line (float a, float b, float heading)
bool	gvf_line_XY_heading (float a, float b, float heading)
bool	gvf_line_XY1_XY2 (float x1, float y1, float x2, float y2)
bool	gvf_line_wp1_wp2 (uint8_t wp1, uint8_t wp2)
bool	gvf_segment_loop_XY1_XY2 (float x1, float y1, float x2, float y2, float d1, float d2)
bool	gvf_segment_loop_wp1_wp2 (uint8_t wp1, uint8_t wp2, float d1, float d2)
bool	gvf_segment_XY1_XY2 (float x1, float y1, float x2, float y2)
bool	gvf_segment_wp1_wp2 (uint8_t wp1, uint8_t wp2)
bool	gvf_line_wp_heading (uint8_t wp, float heading)
bool	gvf_ellipse_XY (float x, float y, float a, float b, float alpha)
bool	gvf_ellipse_wp (uint8_t wp, float a, float b, float alpha)
bool	gvf_sin_XY_alpha (float a, float b, float alpha, float w, float off, float A)
bool	gvf_sin_wp1_wp2 (uint8_t wp1, uint8_t wp2, float w, float off, float A)
bool	gvf_sin_wp_alpha (uint8_t wp, float alpha, float w, float off, float A)

Variables
gvf_con	gvf_control
gvf_tra	gvf_trajectory
gvf_seg	gvf_segment
uint32_t	gvf_t0 = 0

Function Documentation

gvf_control_2D()

void gvf_control_2D	(	float	ke,
		float	kn,
		float	e,
		struct gvf_grad *	grad,
		struct gvf_Hess *	hess
	)

Definition at line 154 of file gvf.c.

References AP_MODE_AUTO2, autopilot_get_mode(), course, get_sys_time_msec(), gvf_control, GVF_GRAVITY, gvf_t0, gvf_Hess::H11, gvf_Hess::H12, gvf_Hess::H21, gvf_Hess::H22, h_ctl_roll_max_setpoint, h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_ROLL, gvf_grad::nx, gvf_grad::ny, s, gvf_con::s, stateGetHorizontalSpeedDir_f(), stateGetHorizontalSpeedNorm_f(), stateGetNedToBodyEulers_f(), and FloatEulers::theta.

Referenced by gvf_ellipse_XY(), gvf_line(), and gvf_sin_XY_alpha().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_ellipse_wp()

bool gvf_ellipse_wp	(	uint8_t	wp,
		float	a,
		float	b,
		float	alpha
	)

Definition at line 397 of file gvf.c.

References alpha, b, gvf_ellipse_XY(), and waypoints.

Here is the call graph for this function:

gvf_ellipse_XY()

bool gvf_ellipse_XY	(	float	x,
		float	y,
		float	a,
		float	b,
		float	alpha
	)

Definition at line 361 of file gvf.c.

References alpha, b, gvf_con::error, gvf_control, gvf_control_2D(), gvf_ellipse_info(), gvf_ellipse_par, gvf_trajectory, horizontal_mode, HORIZONTAL_MODE_CIRCLE, HORIZONTAL_MODE_WAYPOINT, gvf_ell_par::ke, gvf_con::ke, gvf_ell_par::kn, gvf_tra::p, and gvf_tra::type.

Referenced by distributed_circular(), gvf_ellipse_wp(), and gvf_nav_survey_polygon_run().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_init()

void gvf_init

(

void

)

Definition at line 139 of file gvf.c.

References DefaultPeriodic, gvf_control, gvf_trajectory, gvf_con::ke, gvf_con::kn, NONE, register_periodic_telemetry(), gvf_con::s, send_circle(), send_gvf(), send_segment(), and gvf_tra::type.

Here is the call graph for this function:

gvf_line()

static void gvf_line ( float  a, float  b, float  heading  )

static

Definition at line 229 of file gvf.c.

References b, gvf_con::error, gvf_control, gvf_control_2D(), gvf_line_info(), gvf_line_par, gvf_segment, gvf_trajectory, heading, horizontal_mode, HORIZONTAL_MODE_WAYPOINT, gvf_li_par::ke, gvf_con::ke, gvf_li_par::kn, gvf_tra::p, gvf_seg::seg, and gvf_tra::type.

Referenced by gvf_line_XY_heading(), and gvf_segment_loop_XY1_XY2().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_line_wp1_wp2()

bool gvf_line_wp1_wp2	(	uint8_t	wp1,
		uint8_t	wp2
	)

Definition at line 274 of file gvf.c.

References gvf_line_XY1_XY2(), waypoints, point::x, and point::y.

Here is the call graph for this function:

gvf_line_wp_heading()

bool gvf_line_wp_heading	(	uint8_t	wp,
		float	heading
	)

Definition at line 349 of file gvf.c.

References b, gvf_line_XY_heading(), heading, waypoints, point::x, and point::y.

Here is the call graph for this function:

gvf_line_XY1_XY2()

bool gvf_line_XY1_XY2	(	float	x1,
		float	y1,
		float	x2,
		float	y2
	)

Definition at line 257 of file gvf.c.

References gvf_line_XY_heading(), gvf_segment, horizontal_mode, HORIZONTAL_MODE_ROUTE, gvf_seg::seg, gvf_seg::x1, gvf_seg::x2, gvf_seg::y1, and gvf_seg::y2.

Referenced by gvf_line_wp1_wp2(), and gvf_segment_XY1_XY2().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_line_XY_heading()

bool gvf_line_XY_heading	(	float	a,
		float	b,
		float	heading
	)

Definition at line 250 of file gvf.c.

References b, gvf_line(), gvf_set_direction(), and heading.

Referenced by gvf_line_wp_heading(), and gvf_line_XY1_XY2().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_segment_loop_wp1_wp2()

bool gvf_segment_loop_wp1_wp2	(	uint8_t	wp1,
		uint8_t	wp2,
		float	d1,
		float	d2
	)

Definition at line 307 of file gvf.c.

References d1, d2, gvf_segment_loop_XY1_XY2(), waypoints, point::x, and point::y.

Here is the call graph for this function:

gvf_segment_loop_XY1_XY2()

bool gvf_segment_loop_XY1_XY2	(	float	x1,
		float	y1,
		float	x2,
		float	y2,
		float	d1,
		float	d2
	)

Definition at line 284 of file gvf.c.

References alpha, d1, d2, gvf_control, gvf_line(), gvf_segment, horizontal_mode, HORIZONTAL_MODE_ROUTE, out_of_segment_area(), s, gvf_con::s, gvf_seg::seg, gvf_seg::x1, gvf_seg::x2, gvf_seg::y1, and gvf_seg::y2.

Referenced by gvf_segment_loop_wp1_wp2().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_segment_wp1_wp2()

bool gvf_segment_wp1_wp2	(	uint8_t	wp1,
		uint8_t	wp2
	)

Definition at line 339 of file gvf.c.

References gvf_segment_XY1_XY2(), waypoints, point::x, and point::y.

Here is the call graph for this function:

gvf_segment_XY1_XY2()

bool gvf_segment_XY1_XY2	(	float	x1,
		float	y1,
		float	x2,
		float	y2
	)

Definition at line 317 of file gvf.c.

References gvf_line_XY1_XY2(), p, and stateGetPositionEnu_f().

Referenced by gvf_nav_points(), and gvf_segment_wp1_wp2().

Here is the call graph for this function:

Here is the caller graph for this function:

gvf_set_direction()

void gvf_set_direction

(

int8_t

s

)

Definition at line 222 of file gvf.c.

References gvf_control, s, and gvf_con::s.

Referenced by gvf_line_XY_heading(), and gvf_nav_direction_circle().

Here is the caller graph for this function:

gvf_sin_wp1_wp2()

bool gvf_sin_wp1_wp2	(	uint8_t	wp1,
		uint8_t	wp2,
		float	w,
		float	off,
		float	A
	)

Definition at line 428 of file gvf.c.

References A, alpha, gvf_sin_XY_alpha(), waypoints, point::x, and point::y.

Here is the call graph for this function:

gvf_sin_wp_alpha()

bool gvf_sin_wp_alpha	(	uint8_t	wp,
		float	alpha,
		float	w,
		float	off,
		float	A
	)

Definition at line 447 of file gvf.c.

References A, alpha, gvf_sin_XY_alpha(), waypoints, point::x, and point::y.

Here is the call graph for this function:

gvf_sin_XY_alpha()

bool gvf_sin_XY_alpha	(	float	a,
		float	b,
		float	alpha,
		float	w,
		float	off,
		float	A
	)

Definition at line 405 of file gvf.c.

References A, alpha, b, gvf_con::error, gvf_control, gvf_control_2D(), gvf_sin_info(), gvf_sin_par, gvf_trajectory, gvf_con::ke, gvf_s_par::ke, gvf_s_par::kn, gvf_tra::p, and gvf_tra::type.

Referenced by gvf_sin_wp1_wp2(), and gvf_sin_wp_alpha().

Here is the call graph for this function:

Here is the caller graph for this function:

out_of_segment_area()

static int out_of_segment_area ( float  x1, float  y1, float  x2, float  y2, float  d1, float  d2  )

static

Definition at line 108 of file gvf.c.

References alpha, d1, d2, p, s, and stateGetPositionEnu_f().

Referenced by gvf_segment_loop_XY1_XY2().

Here is the call graph for this function:

Here is the caller graph for this function:

send_circle()

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

static

Definition at line 79 of file gvf.c.

References dev, ELLIPSE, get_sys_time_msec(), gvf_t0, gvf_trajectory, gvf_tra::p, and gvf_tra::type.

Referenced by gvf_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_gvf()

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

static

Definition at line 48 of file gvf.c.

References dev, ELLIPSE, gvf_con::error, get_sys_time_msec(), gvf_control, gvf_t0, gvf_trajectory, gvf_con::ke, LINE, gvf_tra::p, gvf_con::s, SIN, and gvf_tra::type.

Referenced by gvf_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_segment()

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

static

Definition at line 93 of file gvf.c.

References dev, get_sys_time_msec(), gvf_segment, gvf_t0, gvf_trajectory, LINE, gvf_seg::seg, gvf_tra::type, gvf_seg::x1, gvf_seg::x2, gvf_seg::y1, and gvf_seg::y2.

Referenced by gvf_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

gvf_control

gvf_con gvf_control

Definition at line 37 of file gvf.c.

Referenced by distributed_circular(), gvf_control_2D(), gvf_ellipse_XY(), gvf_init(), gvf_line(), gvf_segment_loop_XY1_XY2(), gvf_set_direction(), gvf_sin_XY_alpha(), and send_gvf().

gvf_segment

gvf_seg gvf_segment

Definition at line 41 of file gvf.c.

Referenced by gvf_line(), gvf_line_XY1_XY2(), gvf_segment_loop_XY1_XY2(), and send_segment().

gvf_t0

uint32_t gvf_t0 = 0

Definition at line 44 of file gvf.c.

Referenced by gvf_control_2D(), send_circle(), send_gvf(), and send_segment().

gvf_trajectory

gvf_tra gvf_trajectory

Definition at line 40 of file gvf.c.

Referenced by gvf_ellipse_info(), gvf_ellipse_XY(), gvf_init(), gvf_line(), gvf_line_info(), gvf_sin_info(), gvf_sin_XY_alpha(), send_circle(), send_gvf(), and send_segment().