25#include "generated/airframe.h"
39#ifndef GVF_LINE_HEADING
40#define GVF_LINE_HEADING 0
45#define GVF_SEGMENT_D1 0
50#define GVF_SEGMENT_D2 0
80 gvf_setNavMode(GVF_MODE_WAYPOINT);
93 float alpha = atan2f(zy, zx);
95 float cosa = cosf(-
alpha);
96 float sina = sinf(-
alpha);
98 float pxr = px * cosa - py * sina;
99 float zxr = zx * cosa - zy * sina;
105 }
else if (pxr > (zxr +
d2)) {
151 gvf_setNavMode(GVF_MODE_ROUTE);
186 float alpha = atanf(zx / zy);
190 gvf_setNavMode(GVF_MODE_ROUTE);
220 float px =
p->x - x1;
221 float py =
p->y - y1;
226 float beta = atan2f(zy, zx);
227 float cosb = cosf(-beta);
228 float sinb = sinf(-beta);
229 float zxr = zx * cosb - zy * sinb;
230 float pxr = px * cosb - py * sinb;
232 if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
bool nav_gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
bool nav_gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
bool nav_gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
bool nav_gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
bool nav_gvf_line_XY_heading(float a, float b, float heading)
static void gvf_line(float a, float b, float heading)
gvf_seg_par gvf_segment_par
bool nav_gvf_segment_points(struct FloatVect2 start, struct FloatVect2 end)
bool nav_gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
bool nav_gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
bool nav_gvf_line_wp_heading(uint8_t wp, float heading)
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)
Guidance algorithm based on vector fields.
void gvf_line_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
vector in East North Up coordinates Units: meters
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.