12#include "generated/flight_plan.h"
15#define BUFFER_ZONE_DIST 10
21 if (!
state.ned_initialized_i) {
33 VECT3_SMUL(tmp_enu_point_f, tmp_enu_point_i, 0.01f);
40 VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
46 if (dist_to_home > MAX_DIST_FROM_HOME) {
47 VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
50 point->
x = home.
x + vect_from_home.
x;
51 point->
y = home.
y + vect_from_home.
y;
70 if (
nav.nav_approaching(target_wp, NULL,
CARROT)) {
75 }
else {
return false; }
79 nav.nav_goto(target_wp);
90 float radius = el->
element.mission_circle.radius;
93 nav.nav_circle(center_wp, radius);
99 if (el-> duration <= 0.f){
115 if (
nav.nav_approaching(to_wp, from_wp,
CARROT)) {
120 }
else {
return false; }
124 nav.nav_route(from_wp, to_wp);
130#ifndef MISSION_PATH_SKIP_GOTO
131#define MISSION_PATH_SKIP_GOTO FALSE
138 if (el->
element.mission_path.nb == 0) {
142 if (el->
element.mission_path.path_idx == 0) {
147 else if (el->
element.mission_path.path_idx < el->
element.mission_path.nb) {
153 if (
nav.nav_approaching(to_wp, from_wp,
CARROT)) {
158 el->
element.mission_path.path_idx++;
160 }
else { el->
element.mission_path.path_idx++; }
163 nav.nav_route(from_wp, to_wp);
164 }
else {
return false; }
183#ifndef MISSION_WAIT_TIMEOUT
184#define MISSION_WAIT_TIMEOUT 30
188#if MISSION_WAIT_TIMEOUT
189static float mission_wait_time = 0.f;
194 mission_wait_time = 0.f;
216 bool el_running =
false;
Core autopilot interface common to all firmwares.
static const float dt_navigation
static float float_vect2_norm(const struct FloatVect2 *v)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT3_SMUL(_vo, _vi, _s)
int32_t alt
in millimeters above WGS84 reference ellipsoid
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int).
struct _mission_element * mission_get(void)
Get current mission element.
float params[MISSION_CUSTOM_MAX]
list of parameters
struct _mission_registered * reg
pointer to a registered custom mission element
mission_custom_cb cb
navigation/action function callback
uint8_t nb
number of parameters
union _mission_element::@136371027203076204005113273111204361211010126306 element
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
float duration
time to spend in the element (<= 0 to disable)
#define MISSION_WAIT_TIMEOUT
Implement waiting pattern Only called when MISSION_WAIT_TIMEOUT is not 0.
static bool mission_wait_started
struct EnuCoor_f last_mission_wp
#define MISSION_PATH_SKIP_GOTO
static bool mission_wait_pattern(void)
static bool mission_nav_wp(struct _mission_element *el)
Navigation function to a single waypoint (2D).
static bool mission_nav_segment(struct _mission_element *el)
Navigation function along a segment (2D).
static bool mission_nav_circle(struct _mission_element *el)
Navigation function on a circle.
static bool mission_nav_custom(struct _mission_custom *custom, bool init)
Call custom navigation function.
int mission_run()
Run mission.
bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
Utility function: converts lla (int) to local point (float).
static bool mission_nav_path(struct _mission_element *el)
Navigation function along a path (2D).
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
vector in East North Up coordinates Units: meters
struct RotorcraftNavigation nav
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.
#define CARROT
default approaching_time for a wp
Rover navigation functions.