44 #ifndef NAVIGATION_FREQUENCY
45 #if PERIODIC_FREQUENCY == 512
46 #define NAVIGATION_FREQUENCY 16
48 #define NAVIGATION_FREQUENCY 20
60 #define HORIZONTAL_MODE_WAYPOINT 0
61 #define HORIZONTAL_MODE_ROUTE 1
62 #define HORIZONTAL_MODE_CIRCLE 2
63 #define HORIZONTAL_MODE_ATTITUDE 3
64 #define HORIZONTAL_MODE_MANUAL 4
65 #define HORIZONTAL_MODE_GUIDED 5
81 #define VERTICAL_MODE_MANUAL 0
82 #define VERTICAL_MODE_CLIMB 1
83 #define VERTICAL_MODE_ALT 2
84 #define VERTICAL_MODE_GUIDED 3
100 #define GetPosX() (stateGetPositionEnu_f()->x)
102 #define GetPosY() (stateGetPositionEnu_f()->y)
104 #define GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
106 #define GetPosHeight() (stateGetPositionEnu_f()->z)
113 #define GetAltRef() (state.ned_origin_f.hmsl)
117 #define NormCourse(x) { \
118 while (x < 0) x += 360; \
119 while (x >= 360) x -= 360; \
153 #define NavStartDetectGround() ({ autopilot.detect_ground_once = true; false; })
154 #define NavDetectGround() nav_detect_ground()
167 #define NavSetManual nav_set_manual
168 #define NavSetFailsafe nav_set_failsafe
171 #define NavSetGroundReferenceHere nav_reset_reference
172 #define NavSetAltitudeReferenceHere nav_reset_alt
174 #define NavSetWaypointHere waypoint_set_here_2d
175 #define NavCopyWaypoint waypoint_copy
176 #define NavCopyWaypointPositionOnly waypoint_position_copy
181 #define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp].enu_i, NULL, time)
182 #define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time)
186 #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_i, time)
210 #define NavVerticalAutoThrottleMode(_pitch) { \
211 nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
216 #define NavVerticalAutoPitchMode(_throttle) {}
220 #define NavVerticalAltitudeMode(_alt, _pre_climb) { \
221 vertical_mode = VERTICAL_MODE_ALT; \
222 nav_altitude = POS_BFP_OF_REAL(_alt); \
226 #define NavVerticalClimbMode(_climb) { \
227 vertical_mode = VERTICAL_MODE_CLIMB; \
228 nav_climb = SPEED_BFP_OF_REAL(_climb); \
232 #define NavVerticalThrottleMode(_throttle) { \
233 vertical_mode = VERTICAL_MODE_MANUAL; \
234 nav_throttle = _throttle; \
238 #define NavHeading nav_set_heading_rad
240 #define NavAttitude(_roll) { \
241 horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
242 nav_roll = ANGLE_BFP_OF_REAL(_roll); \
267 #define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI)
268 #define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
270 #define CloseDegAngles(_c1, _c2) ({ int32_t _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
271 #define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
273 #define NavQdrCloseTo(x) CloseDegAngles(((x) >> INT32_ANGLE_FRAC), NavCircleQdr())
274 #define NavCourseCloseTo(x) {}
280 #define Oval(a, b, c) nav_oval((b), (a), (c))
285 #ifdef GUIDANCE_INDI_HYBRID
311 #define NavFollow nav_follow
319 #define nav_IncreaseShift(x) {}
320 #define nav_SetNavRadius(x) {}
321 #define navigation_SetFlightAltitude(x) { \
322 flight_altitude = x; \
323 nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude - state.ned_origin_f.hmsl); \
uint8_t autopilot_get_mode(void)
get autopilot mode
void autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly FIXME is i...
Core autopilot interface common to all firmwares.
Common flight_plan functions shared between fixedwing and rotorcraft.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
#define VECT2_DIFF(_c, _a, _b)
#define VECT3_COPY(_a, _b)
#define POS_FLOAT_OF_BFP(_ai)
#define POS_BFP_OF_REAL(_af)
vector in East North Up coordinates
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Paparazzi fixed point math for geodetic calculations.
void nav_home(void)
Home mode navigation (circle around HOME)
static void NavGotoWaypoint(uint8_t wp)
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
float dist2_to_wp
squared distance to next waypoint
void nav_set_heading_current(void)
Set heading to the current yaw angle.
uint8_t last_wp
Index of last waypoint.
#define HORIZONTAL_MODE_WAYPOINT
void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
Set manual roll, pitch and yaw without stabilization.
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
#define VERTICAL_MODE_ALT
struct EnuCoor_i navigation_carrot
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
uint8_t nav_oval_count
Navigation along a figure O.
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
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.
int32_t nav_circle_radians
Status on the current circle.
void nav_oval(uint8_t, uint8_t, float)
Navigation along a figure O.
#define HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_ROUTE
void nav_init(void)
Navigation Initialisation.
void nav_parse_MOVE_WP(uint8_t *buf)
int32_t nav_flight_altitude
void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height)
struct FloatVect2 line_vect to_end_vect
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
void nav_parse_BLOCK(uint8_t *buf)
static void NavSegment(uint8_t wp_start, uint8_t wp_end)
struct EnuCoor_i navigation_target
static void NavGlide(uint8_t start_wp, uint8_t wp)
Nav glide routine.
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
int32_t nav_circle_radius
float dist2_to_home
squared distance to home waypoint
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
static void NavKillThrottle(void)
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
static void NavResurrect(void)
bool nav_detect_ground(void)
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
static void NavCircleWaypoint(uint8_t wp_center, float radius)
int32_t nav_heading
with INT32_ANGLE_FRAC
int32_t nav_pitch
with INT32_ANGLE_FRAC
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.
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
static void NavGotoWaypointHeading(uint8_t wp)
void set_exception_flag(uint8_t flag_num)
void nav_set_failsafe(void)
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.