![]() |
Paparazzi UAS
v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include <math.h>
#include "std.h"
#include "modules/guidance/gvf/gvf.h"
#include "modules/guidance/gvf/gvf_low_level_control.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 "autopilot.h"
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Functions | |
static void | send_gvf (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_st | gvf_state |
gvf_tra | gvf_trajectory |
gvf_seg | gvf_segment |
uint32_t | gvf_t0 = 0 |
int | gvf_plen = 1 |
int | gvf_plen_wps = 0 |
Definition at line 127 of file gvf.c.
References gvf_st::course, course, get_sys_time_msec(), gvf_control, gvf_low_level_control_2D(), gvf_low_level_getState(), gvf_state, gvf_t0, gvf_Hess::H11, gvf_Hess::H12, gvf_Hess::H21, gvf_Hess::H22, gvf_grad::nx, gvf_grad::ny, gvf_con::omega, gvf_st::px_dot, gvf_st::py_dot, gvf_con::s, and s.
Referenced by gvf_ellipse_XY(), gvf_line(), and gvf_sin_XY_alpha().
bool gvf_ellipse_wp | ( | uint8_t | wp, |
float | a, | ||
float | b, | ||
float | alpha | ||
) |
Definition at line 395 of file gvf.c.
References alpha, b, gvf_ellipse_XY(), gvf_plen_wps, gvf_trajectory, gvf_tra::p, WaypointX, and WaypointY.
bool gvf_ellipse_XY | ( | float | x, |
float | y, | ||
float | a, | ||
float | b, | ||
float | alpha | ||
) |
Definition at line 356 of file gvf.c.
References alpha, b, gvf_con::error, gvf_control, gvf_control_2D(), gvf_ellipse_info(), gvf_ellipse_par, gvf_plen, gvf_plen_wps, gvf_trajectory, gvf_con::ke, gvf_ell_par::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().
void gvf_init | ( | void | ) |
Definition at line 114 of file gvf.c.
References DefaultPeriodic, gvf_control, gvf_trajectory, gvf_con::ke, gvf_con::kn, NONE, register_periodic_telemetry(), gvf_con::s, send_gvf(), and gvf_tra::type.
|
static |
Definition at line 196 of file gvf.c.
References b, gvf_con::error, gvf_control, gvf_control_2D(), gvf_line_info(), gvf_line_par, gvf_plen, gvf_plen_wps, gvf_segment, gvf_trajectory, heading, gvf_con::ke, gvf_li_par::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().
Definition at line 251 of file gvf.c.
References gvf_line_XY1_XY2(), gvf_plen_wps, gvf_trajectory, gvf_tra::p, WaypointX, and WaypointY.
bool gvf_line_wp_heading | ( | uint8_t | wp, |
float | heading | ||
) |
Definition at line 341 of file gvf.c.
References b, gvf_line_XY_heading(), gvf_plen_wps, gvf_trajectory, heading, gvf_tra::p, WaypointX, and WaypointY.
bool gvf_line_XY1_XY2 | ( | float | x1, |
float | y1, | ||
float | x2, | ||
float | y2 | ||
) |
Definition at line 227 of file gvf.c.
References gvf_line_XY_heading(), gvf_plen_wps, gvf_segment, gvf_trajectory, gvf_tra::p, 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().
bool gvf_line_XY_heading | ( | float | a, |
float | b, | ||
float | heading | ||
) |
Definition at line 220 of file gvf.c.
References b, gvf_line(), gvf_set_direction(), and heading.
Referenced by gvf_line_wp_heading(), and gvf_line_XY1_XY2().
Definition at line 289 of file gvf.c.
References d1, d2, gvf_plen_wps, gvf_segment_loop_XY1_XY2(), gvf_trajectory, gvf_tra::p, WaypointX, and WaypointY.
bool gvf_segment_loop_XY1_XY2 | ( | float | x1, |
float | y1, | ||
float | x2, | ||
float | y2, | ||
float | d1, | ||
float | d2 | ||
) |
Definition at line 265 of file gvf.c.
References alpha, d1, d2, gvf_line(), gvf_segment, gvf_set_direction(), out_of_segment_area(), s, gvf_seg::seg, gvf_seg::x1, gvf_seg::x2, gvf_seg::y1, and gvf_seg::y2.
Referenced by gvf_segment_loop_wp1_wp2().
Definition at line 327 of file gvf.c.
References gvf_plen_wps, gvf_segment_XY1_XY2(), gvf_trajectory, gvf_tra::p, WaypointX, and WaypointY.
bool gvf_segment_XY1_XY2 | ( | float | x1, |
float | y1, | ||
float | x2, | ||
float | y2 | ||
) |
Definition at line 305 of file gvf.c.
References gvf_line_XY1_XY2(), p, and stateGetPositionEnu_f().
Referenced by gvf_nav_points(), and gvf_segment_wp1_wp2().
void gvf_set_direction | ( | int8_t | s | ) |
Definition at line 189 of file gvf.c.
References gvf_control, gvf_con::s, and s.
Referenced by gvf_line_XY_heading(), gvf_nav_direction_circle(), and gvf_segment_loop_XY1_XY2().
Definition at line 431 of file gvf.c.
References A, alpha, gvf_plen_wps, gvf_sin_XY_alpha(), gvf_trajectory, gvf_tra::p, WaypointX, and WaypointY.
bool gvf_sin_wp_alpha | ( | uint8_t | wp, |
float | alpha, | ||
float | w, | ||
float | off, | ||
float | A | ||
) |
Definition at line 454 of file gvf.c.
References A, alpha, gvf_plen_wps, gvf_sin_XY_alpha(), gvf_trajectory, gvf_tra::p, WaypointX, and WaypointY.
bool gvf_sin_XY_alpha | ( | float | a, |
float | b, | ||
float | alpha, | ||
float | w, | ||
float | off, | ||
float | A | ||
) |
Definition at line 406 of file gvf.c.
References A, alpha, b, gvf_con::error, gvf_control, gvf_control_2D(), gvf_plen, gvf_plen_wps, 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().
|
static |
Definition at line 83 of file gvf.c.
References alpha, d1, d2, p, s, and stateGetPositionEnu_f().
Referenced by gvf_segment_loop_XY1_XY2().
|
static |
Definition at line 52 of file gvf.c.
References dev, ELLIPSE, gvf_con::error, get_sys_time_msec(), gvf_control, gvf_plen, gvf_segment, gvf_t0, gvf_trajectory, gvf_con::ke, LINE, gvf_tra::p, gvf_con::s, gvf_seg::seg, gvf_tra::type, gvf_seg::x1, gvf_seg::x2, gvf_seg::y1, and gvf_seg::y2.
Referenced by gvf_init().
gvf_con gvf_control |
Definition at line 34 of file gvf.c.
Referenced by distributed_circular(), gvf_control_2D(), gvf_ellipse_XY(), gvf_init(), gvf_line(), gvf_set_direction(), gvf_sin_XY_alpha(), and send_gvf().
int gvf_plen = 1 |
Definition at line 47 of file gvf.c.
Referenced by gvf_ellipse_XY(), gvf_line(), gvf_sin_XY_alpha(), and send_gvf().
int gvf_plen_wps = 0 |
Definition at line 48 of file gvf.c.
Referenced by gvf_ellipse_wp(), gvf_ellipse_XY(), gvf_line(), gvf_line_wp1_wp2(), gvf_line_wp_heading(), gvf_line_XY1_XY2(), gvf_segment_loop_wp1_wp2(), gvf_segment_wp1_wp2(), gvf_sin_wp1_wp2(), gvf_sin_wp_alpha(), and gvf_sin_XY_alpha().
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_gvf().
gvf_st gvf_state |
Definition at line 37 of file gvf.c.
Referenced by gvf_control_2D(), and gvf_low_level_getState().
uint32_t gvf_t0 = 0 |
Definition at line 44 of file gvf.c.
Referenced by gvf_control_2D(), and send_gvf().
gvf_tra gvf_trajectory |
Definition at line 40 of file gvf.c.
Referenced by gvf_ellipse_info(), gvf_ellipse_wp(), gvf_ellipse_XY(), gvf_init(), gvf_line(), gvf_line_info(), gvf_line_wp1_wp2(), gvf_line_wp_heading(), gvf_line_XY1_XY2(), gvf_segment_loop_wp1_wp2(), gvf_segment_wp1_wp2(), gvf_sin_info(), gvf_sin_wp1_wp2(), gvf_sin_wp_alpha(), gvf_sin_XY_alpha(), and send_gvf().