25#include "generated/airframe.h"
30#define GVF_IK_LINE_KE 1
35#define GVF_IK_LINE_KN 1
39#ifndef GVF_IK_LINE_HEADING
40#define GVF_IK_LINE_HEADING 0
44#ifndef GVF_IK_SEGMENT_D1
45#define GVF_IK_SEGMENT_D1 0
49#ifndef GVF_IK_SEGMENT_D2
50#define GVF_IK_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);
184 float alpha = atanf(zx / zy);
188 gvf_setNavMode(GVF_MODE_ROUTE);
218 float px =
p->x - x1;
219 float py =
p->y - y1;
224 float beta = atan2f(zy, zx);
225 float cosb = cosf(-beta);
226 float sinb = sinf(-beta);
227 float zxr = zx * cosb - zy * sinb;
228 float pxr = px * cosb - py * sinb;
230 if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
bool nav_gvf_ik_segment_XY1_XY2(float x1, float y1, float x2, float y2)
bool nav_gvf_ik_line_XY_heading(float a, float b, float heading)
bool nav_gvf_ik_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
bool nav_gvf_ik_line_XY1_XY2(float x1, float y1, float x2, float y2)
bool nav_gvf_ik_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
#define GVF_IK_LINE_HEADING
bool nav_gvf_ik_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
#define GVF_IK_SEGMENT_D2
#define GVF_IK_SEGMENT_D1
bool nav_gvf_ik_line_wp_heading(uint8_t wp, float heading)
gvf_ik_seg_par gvf_ik_segment_par
static void gvf_ik_line(float a, float b, float heading)
gvf_ik_li_par gvf_ik_line_par
bool nav_gvf_ik_segment_wp1_wp2(uint8_t wp1, uint8_t wp2)
void gvf_ik_control_2D(float ke, float kn, float phi, struct gvf_grad *grad, struct gvf_Hess *hess)
gvf_ik_con gvf_ik_control
void gvf_ik_set_direction(int8_t s)
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.