34 #include "generated/flight_plan.h"
38 #define BUFFER_ZONE_DIST 10
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;
84 switch (tmp_element.
type) {
96 for (i = 0; i < 5; i++) {
123 last_mission_wp = *target_wp;
127 }
else {
return false; }
143 struct EnuCoor_i *center_wp = &(el->
element.mission_circle.center.center_i);
168 last_mission_wp = *to_wp;
172 }
else {
return false; }
189 if (el->
element.mission_path.nb == 0) {
193 if (el->
element.mission_path.path_idx == 0) {
194 el->
element.mission_wp.wp.wp_i = el->
element.mission_path.path.path_i[0];
198 else if (el->
element.mission_path.path_idx < el->
element.mission_path.nb) {
205 last_mission_wp = *to_wp;
209 el->
element.mission_path.path_idx++;
211 }
else { el->
element.mission_path.path_idx++; }
218 }
else {
return false; }
240 bool el_running =
false;
struct EnuCoor_i navigation_target
int mission_run()
Run mission.
struct EnuCoor_i last_mission_wp
#define VECT2_SUM(_c, _a, _b)
struct _mission_element * mission_get(void)
Get current mission element.
vector in East North Up coordinates Units: meters
#define POS_BFP_OF_REAL(_af)
#define VECT3_COPY(_a, _b)
static bool mission_nav_custom(struct _mission_custom *custom, bool init)
Call custom navigation function.
static bool mission_nav_circle(struct _mission_element *el)
Navigation function on a circle.
#define VECT2_DIFF(_c, _a, _b)
#define HORIZONTAL_MODE_CIRCLE
float duration
time to spend in the element (<= 0 to disable)
vector in Latitude, Longitude and Altitude
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct _mission_registered * reg
pointer to a registered custom mission element
float element_time
time in second spend in the current element
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
static float float_vect2_norm(struct FloatVect2 *v)
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command...
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
mission_custom_cb cb
navigation/action function callback
static bool mission_nav_path(struct _mission_element *el)
Navigation function along a path.
struct LlaCoor_i lla
Reference point in lla.
#define HORIZONTAL_MODE_ROUTE
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.
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
#define VECT2_SMUL(_vo, _vi, _s)
bool ned_initialized_i
true if local int coordinate frame is initialsed
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Core autopilot interface common to all firmwares.
Rotorcraft navigation functions.
#define VECT3_SMUL(_vo, _vi, _s)
uint8_t nb
number of parameters
vector in East North Up coordinates
uint8_t current_idx
current mission element index
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
bool mission_element_convert(struct _mission_element *el)
Convert mission element's points format if needed.
#define ENU_BFP_OF_REAL(_o, _i)
#define POS_FLOAT_OF_BFP(_ai)
static const float dt_navigation
float params[MISSION_CUSTOM_MAX]
list of parameters
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
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.
bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp.
bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
Utility function: converts lla (int) to local point (float)
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
#define HORIZONTAL_MODE_WAYPOINT
#define CARROT
default approaching_time for a wp
union _mission_element::@299 element