34#include "generated/flight_plan.h"
38#define BUFFER_ZONE_DIST 10
44 if (!
state.ned_initialized_i) {
56 VECT3_SMUL(tmp_enu_point_f, tmp_enu_point_i, 0.01);
63 VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
68 if (dist_to_home > MAX_DIST_FROM_HOME) {
69 VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
73 point->z = tmp_enu_point_f.
z;
91 if (
nav.nav_approaching(target_wp, NULL,
CARROT)
92#ifdef MISSION_ALT_PROXIMITY
100 }
else {
return false; }
104 nav.nav_goto(target_wp);
116 float radius = el->
element.mission_circle.radius;
119 nav.nav_circle(center_wp, radius);
138 if (
nav.nav_approaching(to_wp, from_wp,
CARROT)
139#ifdef MISSION_ALT_PROXIMITY
147 }
else {
return false; }
151 nav.nav_route(from_wp, to_wp);
159#ifndef MISSION_PATH_SKIP_GOTO
160#define MISSION_PATH_SKIP_GOTO FALSE
167 if (el->
element.mission_path.nb == 0) {
171 if (el->
element.mission_path.path_idx == 0) {
176 else if (el->
element.mission_path.path_idx < el->
element.mission_path.nb) {
182 if (
nav.nav_approaching(to_wp, from_wp,
CARROT)
183#ifdef MISSION_ALT_PROXIMITY
191 el->
element.mission_path.path_idx++;
193 }
else { el->
element.mission_path.path_idx++; }
196 nav.nav_route(from_wp, to_wp);
199 }
else {
return false; }
218#ifndef MISSION_WAIT_TIMEOUT
219#define MISSION_WAIT_TIMEOUT 30
223#if MISSION_WAIT_TIMEOUT
224static float mission_wait_time = 0.f;
229 mission_wait_time = 0.f;
251 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 VECT2_SUM(_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
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
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
static bool mission_wait_pattern(void)
struct EnuCoor_f last_mission_wp
static bool mission_nav_wp(struct _mission_element *el)
Navigation function to a single waypoint.
static bool mission_nav_segment(struct _mission_element *el)
Navigation function along a segment.
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.
#define MISSION_PATH_SKIP_GOTO
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.
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
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.
Rotorcraft navigation functions.
#define CARROT
default approaching_time for a wp