50 #if PERIODIC_TELEMETRY
52 static void send_gvf(
struct transport_tx *trans,
struct link_device *
dev)
66 pprz_msg_send_CIRCLE(trans,
dev, AC_ID,
72 pprz_msg_send_SEGMENT(trans,
dev, AC_ID,
91 float alpha = atan2f(zy, zx);
93 float cosa = cosf(-
alpha);
94 float sina = sinf(-
alpha);
96 float pxr = px * cosa - py * sina;
97 float zxr = zx * cosa - zy * sina;
103 }
else if (pxr > (zxr +
d2)) {
121 #if PERIODIC_TELEMETRY
144 float tx =
s * grad->
ny;
145 float ty = -
s * grad->
nx;
148 float H11 = hess->
H11;
149 float H12 = hess->
H12;
150 float H21 = hess->
H21;
151 float H22 = hess->
H22;
154 float pdx_dot = tx - ke * e * nx;
155 float pdy_dot = ty - ke * e * ny;
157 float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
158 float md_x = pdx_dot / norm_pd_dot;
159 float md_y = pdy_dot / norm_pd_dot;
161 float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
162 float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
164 float Bpd_dot_dot_x = ((-ke * e * H11) +
s * H21) * px_dot
165 + ((-ke * e * H12) +
s * H22) * py_dot;
166 float Bpd_dot_dot_y = -(
s * H11 + (ke * e * H21)) * px_dot
167 - (
s * H12 + (ke * e * H22)) * py_dot;
169 float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
170 float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
172 float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
175 float md_dot_x = md_y * md_dot_const;
176 float md_dot_y = -md_x * md_dot_const;
178 float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
180 float mr_x = sinf(
course);
181 float mr_y = cosf(
course);
183 float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
215 gvf_setNavMode(GVF_MODE_WAYPOINT);
241 gvf_setNavMode(GVF_MODE_ROUTE);
274 float alpha = atanf(zx / zy);
278 gvf_setNavMode(GVF_MODE_ROUTE);
308 float px =
p->x - x1;
309 float py =
p->y - y1;
314 float beta = atan2f(zy, zx);
315 float cosb = cosf(-beta);
316 float sinb = sinf(-beta);
317 float zxr = zx * cosb - zy * sinb;
318 float pxr = px * cosb - py * sinb;
320 if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) {
372 if (a < 1 ||
b < 1) {
378 gvf_setNavMode(GVF_MODE_CIRCLE);
381 gvf_setNavMode(GVF_MODE_WAYPOINT);
387 e, &grad_ellipse, &Hess_ellipse);
447 float alpha = atanf(zy / zx);
Core autopilot interface common to all firmwares.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2)
bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2)
bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha)
bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
bool gvf_segment_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 gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
void gvf_control_2D(float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess)
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
void gvf_set_direction(int8_t s)
bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
static void gvf_line(float a, float b, float heading)
bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
bool gvf_line_XY_heading(float a, float b, float heading)
bool gvf_line_wp_heading(uint8_t wp, float heading)
bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2)
Guidance algorithm based on vector fields.
void gvf_ellipse_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
gvf_ell_par gvf_ellipse_par
Guidance algorithm based on vector fields 2D Ellipse trajectory.
void gvf_line_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Guidance algorithm based on vector fields 2D straight line trajectory.
void gvf_low_level_control_2D(float omega)
void gvf_low_level_getState(void)
Firmware dependent file for the guiding vector field algorithm for 2D trajectories.
void gvf_sin_info(float *phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Guidance algorithm based on vector fields 2D sinusoidal trajectory.
vector in East North Up coordinates Units: meters
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.