32#include "../gvf_common.h"
125#if PERIODIC_TELEMETRY
152 float H11 =
hess->H11;
153 float H12 =
hess->H12;
154 float H21 =
hess->H21;
155 float H22 =
hess->H22;
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;
165 + ((-ke * e * H12) +
s * H22) * py_dot;
167 - (
s * H12 + (ke * e * H22)) * py_dot;
182 #if defined(ROTORCRAFT_FIRMWARE)
185 #if defined(GVF_ROTORCRAFT_USE_ACCEL)
193 #warning "Using GVF for rotorcraft is still experimental, proceed with caution"
255 if (speed < 0.0) speed = 0.0;
385 float px =
p->x - x1;
386 float py =
p->y - y1;
449 if (a < 1 ||
b < 1) {
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)
void gvf_set_align(bool align)
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_speed(float speed)
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.
gvf_common_omega gvf_c_omega
gvf_common_params gvf_c_info
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
struct RotorcraftNavigation nav
struct EnuCoor_f speed
speed setpoint (in m/s)
#define NAV_SETPOINT_MODE_SPEED
struct EnuCoor_f accel
accel setpoint (in m/s)
float heading
heading setpoint (in radians)
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.