54 float leg_length2 = Max((wp_diff.
x * wp_diff.
x + wp_diff.
y * wp_diff.
y), 0.1f);
77 if (approaching_time > 0.f) {
81 VECT2_SMUL(estimated_progress, *speed, approaching_time);
82 VECT2_SUM(estimated_pos, *pos, estimated_progress);
104 return (diff.
x * from_diff.
x + diff.
y * from_diff.
y < 0.f);
120 NormRadAngle(trigo_diff);
124 float sign_radius = radius > 0.f ? 1.f : -1.f;
126 float abs_radius = fabsf(radius);
127 if (abs_radius > 0.1f) {
130 carrot_angle =
Min(carrot_angle, M_PI / 4);
131 carrot_angle = Max(carrot_angle, M_PI / 16);
132 carrot_angle =
nav_rover_base.circle.qdr - sign_radius * carrot_angle;
153#ifndef LINE_START_FUNCTION
154#define LINE_START_FUNCTION {}
156#ifndef LINE_STOP_FUNCTION
157#define LINE_STOP_FUNCTION {}
177 wp1->
x + radius *
dir.y,
178 wp1->
y - radius *
dir.x,
182 wp1->
x + 2.f * radius *
dir.y,
183 wp1->
y - 2.f * radius *
dir.x,
187 wp2->
x + 2.f * radius *
dir.y,
188 wp2->
y - 2.f * radius *
dir.x,
192 wp2->
x + radius *
dir.y,
193 wp2->
y - radius *
dir.x,
197 float qdr_out_2 = M_PI - atan2f(
dir.y,
dir.x);
198 float qdr_out_1 = qdr_out_2 + M_PI;
203 float qdr_anticipation = (radius > 0.f ? 15.f : -15.f);
207 nav.nav_circle(&p1_center, radius);
216 nav.nav_route(&p1_out, &p2_in);
217 if (
nav.nav_approaching(&p2_in, &p1_out,
CARROT)) {
226 nav.nav_circle(&p2_center, radius);
235 nav.nav_route(wp2, wp1);
236 if (
nav.nav_approaching(wp1, wp2,
CARROT)) {
248#if PERIODIC_TELEMETRY
251#if ROVER_BASE_SEND_TRAJECTORY
252static void send_segment(
struct transport_tx *trans,
struct link_device *
dev)
254 pprz_msg_send_SEGMENT(trans,
dev, AC_ID,
261static void send_circle(
struct transport_tx *trans,
struct link_device *
dev)
263 pprz_msg_send_CIRCLE(trans,
dev, AC_ID,
272 float dist_home = sqrtf(
nav.dist2_to_home);
274 pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans,
dev, AC_ID,
276 &dist_home, &dist_wp,
279#if ROVER_BASE_SEND_TRAJECTORY
303#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 VECT3_COPY(_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)
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)
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 send_nav_status(struct transport_tx *trans, struct link_device *dev)
static void nav_goto(struct EnuCoor_f *wp)
void nav_rover_init(void)
Init and register nav functions.
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)
struct RoverNavBase nav_rover_base
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.
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
#define CARROT
default approaching_time for a wp
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
#define NAV_MODE_WAYPOINT
Nav modes.
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.