29#include "generated/flight_plan.h"
56 float leg_length2 = Max((wp_diff.
x * wp_diff.
x + wp_diff.
y * wp_diff.
y), 0.1f);
80 if (approaching_time > 0.f) {
84 VECT2_SMUL(estimated_progress, *speed, approaching_time);
85 VECT2_SUM(estimated_pos, *pos, estimated_progress);
108 return (diff.
x * from_diff.
x + diff.
y * from_diff.
y < -0.01f);
118 if (fabsf(radius) < 0.001f) {
128 NormRadAngle(trigo_diff);
133 float sign_radius = radius > 0.f ? 1.f : -1.f;
135 float abs_radius = fabsf(radius);
136 if (abs_radius > 0.1f) {
139 carrot_angle =
Min(carrot_angle, M_PI / 4);
140 carrot_angle = Max(carrot_angle, M_PI / 16);
163#ifndef LINE_START_FUNCTION
164#define LINE_START_FUNCTION {}
166#ifndef LINE_STOP_FUNCTION
167#define LINE_STOP_FUNCTION {}
188 wp1->
x + radius *
dir.y,
189 wp1->
y - radius *
dir.x,
193 wp1->
x + 2.f * radius *
dir.y,
194 wp1->
y - 2.f * radius *
dir.x,
198 wp2->
x + 2.f * radius *
dir.y,
199 wp2->
y - 2.f * radius *
dir.x,
203 wp2->
x + radius *
dir.y,
204 wp2->
y - radius *
dir.x,
208 float qdr_out_2 = M_PI - atan2f(
dir.y,
dir.x);
209 float qdr_out_1 = qdr_out_2 + M_PI;
214 float qdr_anticipation = (radius > 0.f ? 15.f : -15.f);
218 nav.nav_circle(&p1_center, radius);
227 nav.nav_route(&p1_out, &p2_in);
228 if (
nav.nav_approaching(&p2_in, &p1_out,
CARROT)) {
237 nav.nav_circle(&p2_center, radius);
246 nav.nav_route(wp2, wp1);
247 if (
nav.nav_approaching(wp1, wp2,
CARROT)) {
259#if PERIODIC_TELEMETRY
262#if ROTORCRAFT_BASE_SEND_TRAJECTORY
263static void send_segment(
struct transport_tx *trans,
struct link_device *
dev)
265 pprz_msg_send_SEGMENT(trans,
dev, AC_ID,
272static void send_circle(
struct transport_tx *trans,
struct link_device *
dev)
274 pprz_msg_send_CIRCLE(trans,
dev, AC_ID,
283 float dist_home = sqrtf(
nav.dist2_to_home);
285 pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans,
dev, AC_ID,
287 &dist_home, &dist_wp,
289 &
nav.horizontal_mode);
290#if ROTORCRAFT_BASE_SEND_TRAJECTORY
314#if PERIODIC_TELEMETRY
static struct uart_periph * dev
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
static float float_vect2_norm(const struct FloatVect2 *v)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
#define VECT2_SUM(_c, _a, _b)
#define VECT2_ASSIGN(_a, _x, _y)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
#define LINE_STOP_FUNCTION
static void send_circle(struct transport_tx *trans, struct link_device *dev)
#define LINE_START_FUNCTION
static void send_segment(struct transport_tx *trans, struct link_device *dev)
void nav_oval(uint8_t p1, uint8_t p2, float radius)
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees).
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
struct NavBase_t nav_rotorcraft_base
Basic Nav struct.
static void nav_goto(struct EnuCoor_f *wp)
static void _nav_oval_init(void)
static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
static void nav_circle(struct EnuCoor_f *wp_center, float radius)
void nav_rotorcraft_init(void)
Init and register nav functions.
static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
static void nav_stage_init(void)
Implement basic nav function.
static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
struct FloatVect2 pos_diff
vector in East North Up coordinates Units: meters
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
struct RotorcraftNavigation nav
void nav_register_circle(navigation_circle nav_circle)
void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
void nav_register_stage_init(navigation_stage_init nav_stage_init)
Register functions.
Rotorcraft navigation functions.
#define NAV_HORIZONTAL_MODE_WAYPOINT
Nav modes these modes correspond to the flight plan instructions used to set the high level navigatio...
#define NAV_HORIZONTAL_MODE_CIRCLE
#define NAV_HORIZONTAL_MODE_ROUTE
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
#define CARROT
default approaching_time for a wp
#define NAV_CARROT_DIST
Carrot distance during navigation.
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.