36#include "generated/modules.h"
37#include "generated/flight_plan.h"
46#include "pprzlink/messages.h"
92 nav.fp_altitude = SECURITY_HEIGHT;
93 nav.nav_altitude = SECURITY_HEIGHT;
94 nav.fp_max_speed = -1.f;
97 nav.too_far_from_home =
false;
99 nav.dist2_to_home = 0.f;
117 if (DL_BLOCK_ac_id(buf) != AC_ID) {
return; }
123 uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
124 if (ac_id != AC_ID) {
return; }
126 uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
128 lla.
lat = DL_MOVE_WP_lat(buf);
129 lla.
lon = DL_MOVE_WP_lon(buf);
139#ifndef CLOSE_TO_WAYPOINT
140#define CLOSE_TO_WAYPOINT 15.f
160 VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
168#if GUIDANCE_H_USE_REF
185 static bool wp_reached =
false;
186 static struct EnuCoor_i wp_last = { 0, 0, 0 };
191 if ((wp_last.
x != wp_i.
x) || (wp_last.
y != wp_i.
y)) {
204 time_at_wp =
autopilot.flight_time - wp_entry_time;
210 if (time_at_wp > stay_time) {
219 static float last_alt = 0.f;
225 if (fabsf(
nav.fp_altitude - last_alt) > 0.2f) {
226 nav.nav_altitude =
nav.fp_altitude;
228 last_alt =
nav.fp_altitude;
250 if (
nav.nav_stage_init) {
251 nav.nav_stage_init();
285 VECT2_DIFF(wp_diff, *end_point, *start_point);
289 float alt = start_point->
z + (end_point->
z - start_point->
z) * progress;
330#ifdef InGeofenceSector
332 nav.too_far_from_home =
nav.too_far_from_home || !(InGeofenceSector(pos->
x, pos->
y));
358 nav.heading = heading_f;
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
struct pprz_autopilot autopilot
Global autopilot structure.
bool autopilot_in_flight(void)
get in_flight flag
Core autopilot interface common to all firmwares.
void common_flight_plan_init(void)
void nav_goto_block(uint8_t b)
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Common code for AP and FBW telemetry.
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define FLOAT_VECT3_ZERO(_v)
static float float_vect2_norm(const struct FloatVect2 *v)
static float float_vect2_norm2(const struct FloatVect2 *v)
#define FLOAT_RATES_ZERO(_r)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_STRIM(_v, _min, _max)
#define VECT2_COPY(_a, _b)
#define VECT2_SUM(_c, _a, _b)
#define VECT2_SDIV(_vo, _vi, _s)
#define VECT3_COPY(_a, _b)
#define INT_VECT3_ZERO(_v)
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t lon
in degrees*1e7
#define ENU_BFP_OF_REAL(_o, _i)
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Integrated Navigation System interface.
#define INS_RESET_VERTICAL_REF
#define INS_RESET_REF
flags for INS reset
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
float waypoint_get_alt(uint8_t wp_id)
Get altitude of waypoint in meters (above reference).
void waypoints_init(void)
initialize global and local waypoints
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
void nav_oval(uint8_t p1, uint8_t p2, float radius)
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
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.
struct FloatVect2 pos_diff
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
Paparazzi fixed point algebra.
vector in East North Up coordinates Units: meters
Horizontal guidance for rotorcrafts.
void nav_home(void)
Home mode navigation.
static void empty_oval_init(void)
static bool empty_approaching(struct EnuCoor_f *wp_to UNUSED, struct EnuCoor_f *wp_from UNUSED, float approaching_time UNUSED)
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
void nav_set_heading_current(void)
Set heading to the current yaw angle.
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
const float max_dist_from_home
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
bool nav_is_in_flight(void)
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
struct RotorcraftNavigation nav
void nav_init(void)
Navigation Initialisation.
void nav_parse_MOVE_WP(uint8_t *buf)
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)
static void empty_stage_init(void)
Empty navigation functions set at init.
static void empty_oval(struct EnuCoor_f *wp1 UNUSED, struct EnuCoor_f *wp2 UNUSED, float radius UNUSED)
void nav_parse_BLOCK(uint8_t *buf)
#define CLOSE_TO_WAYPOINT
static void empty_goto(struct EnuCoor_f *wp UNUSED)
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
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.
static void empty_route(struct EnuCoor_f *wp_start UNUSED, struct EnuCoor_f *wp_end UNUSED)
const float max_dist2_from_home
void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point)
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
bool nav_detect_ground(void)
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
static void empty_circle(struct EnuCoor_f *wp_center UNUSED, float radius UNUSED)
bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp.
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
static void UNUSED nav_advance_carrot(void)
static void nav_set_altitude(void)
void nav_set_failsafe(void)
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...
void(* navigation_goto)(struct EnuCoor_f *wp)
#define NAV_DESCEND_VSPEED
bool(* navigation_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
void(* navigation_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
#define NAV_CARROT_DIST
Carrot distance during navigation.
void(* navigation_oval_init)(void)
#define NAV_VERTICAL_MODE_ALT
void(* navigation_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
void(* navigation_circle)(struct EnuCoor_f *wp_center, float radius)
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
void(* navigation_stage_init)(void)
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
General Navigation structure.
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
API to get/set the generic vehicle states.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.