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++) {
127 }
else {
return false; }
143 struct EnuCoor_i *center_wp = &(el->
element.mission_circle.center.center_i);
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) {
209 el->
element.mission_path.path_idx++;
211 }
else { el->
element.mission_path.path_idx++; }
218 }
else {
return false; }
237 #ifndef MISSION_WAIT_TIMEOUT
238 #define MISSION_WAIT_TIMEOUT 30
242 #if MISSION_WAIT_TIMEOUT
270 bool el_running =
false;
Core autopilot interface common to all firmwares.
static float float_vect2_norm(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)
#define VECT3_COPY(_a, _b)
#define POS_FLOAT_OF_BFP(_ai)
#define POS_BFP_OF_REAL(_af)
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
#define ENU_BFP_OF_REAL(_o, _i)
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
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
bool ned_initialized_i
true if local int coordinate frame is initialsed
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (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
float element_time
time in second spend in the current element
union _mission_element::@288 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)
uint8_t current_idx
current mission element index
static bool mission_wait_pattern(void)
static float mission_wait_time
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.
bool mission_element_convert(struct _mission_element *el)
Convert mission element's points format if needed.
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 const float dt_navigation
struct EnuCoor_i last_mission_wp
#define MISSION_WAIT_TIMEOUT
Implement waiting pattern Only called when MISSION_WAIT_TIMEOUT is not 0.
static bool mission_nav_path(struct _mission_element *el)
Navigation function along a path.
static struct _mission_element mission_wait_wp
static bool mission_wait_started
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 HORIZONTAL_MODE_WAYPOINT
#define HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_ROUTE
#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
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
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
struct EnuCoor_i navigation_target
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
Rotorcraft navigation functions.
#define CARROT
default approaching_time for a wp
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.