|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Rotorcraft navigation functions. More...
#include "std.h"#include "math/pprz_geodetic_int.h"#include "math/pprz_geodetic_float.h"#include "state.h"#include "modules/nav/waypoints.h"#include "modules/nav/common_flight_plan.h"#include "autopilot.h"#include "generated/airframe.h"Go to the source code of this file.
Data Structures | |
| struct | RotorcraftNavigation |
| General Navigation structure. More... | |
Macros | |
| #define | DEFAULT_CIRCLE_RADIUS 5.f |
| default nav_circle_radius in meters | |
| #define | NAV_CLIMB_VSPEED 0.5f |
| #define | NAV_DESCEND_VSPEED -0.8f |
| #define | ARRIVED_AT_WAYPOINT 3.0f |
| minimum horizontal distance to waypoint to mark as arrived | |
| #define | FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME) |
| Maximum distance from HOME waypoint before going into failsafe mode. | |
| #define | NAV_CARROT_DIST 12 |
| Carrot distance during navigation. | |
| #define | CARROT 0 |
| default approaching_time for a wp | |
| #define | NAVIGATION_FREQUENCY 20 |
| default navigation frequency | |
| #define | NAV_HORIZONTAL_MODE_WAYPOINT 0 |
| Nav modes these modes correspond to the flight plan instructions used to set the high level navigation. | |
| #define | NAV_HORIZONTAL_MODE_ROUTE 1 |
| #define | NAV_HORIZONTAL_MODE_CIRCLE 2 |
| #define | NAV_HORIZONTAL_MODE_ATTITUDE 3 |
| #define | NAV_HORIZONTAL_MODE_NONE 4 |
| #define | NAV_HORIZONTAL_MODE_GUIDED 5 |
| #define | NAV_VERTICAL_MODE_MANUAL 0 |
| #define | NAV_VERTICAL_MODE_CLIMB 1 |
| #define | NAV_VERTICAL_MODE_ALT 2 |
| #define | NAV_VERTICAL_MODE_GUIDED 3 |
| #define | NAV_SETPOINT_MODE_POS 0 |
| Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which setpoint should be considered. | |
| #define | NAV_SETPOINT_MODE_SPEED 1 |
| #define | NAV_SETPOINT_MODE_ACCEL 2 |
| #define | NAV_SETPOINT_MODE_ATTITUDE 3 |
| #define | NAV_SETPOINT_MODE_QUAT 4 |
| #define | NAV_SETPOINT_MODE_RATE 5 |
| #define | NAV_SETPOINT_MODE_MANUAL 6 |
| #define | GetPosX() |
| Get current x (east) position in local coordinates. | |
| #define | GetPosY() |
| Get current y (north) position in local coordinates. | |
| #define | GetPosAlt() |
| Get current altitude above MSL. | |
| #define | GetPosHeight() |
| Get current height above reference. | |
| #define | GetAltRef() |
| Get current altitude reference for local coordinates. | |
| #define | NavStartDetectGround() |
| #define | NavDetectGround() |
| #define | NavSetFailsafe nav_set_failsafe |
| #define | NavSetGroundReferenceHere nav_reset_reference |
| #define | NavSetAltitudeReferenceHere nav_reset_alt |
| #define | NavSetWaypointHere waypoint_set_here_2d |
| #define | NavCopyWaypoint waypoint_copy |
| #define | NavCopyWaypointPositionOnly waypoint_position_copy |
| #define | NavCheckWaypointTime(wp, time) |
| #define | NavAttitude(_roll) |
| #define | NavVerticalAutoThrottleMode(_pitch) |
| Set the climb control to auto-throttle with the specified pitch pre-command. | |
| #define | NavVerticalAltitudeMode(_alt, _pre_climb) |
| Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command. | |
| #define | NavVerticalClimbMode(_climb) |
| Set the vertical mode to climb control with the specified climb setpoint. | |
| #define | NavVerticalThrottleMode(_throttle) |
| Set the vertical mode to fixed throttle with the specified setpoint. | |
| #define | NavHeading nav_set_heading_rad |
| Set the heading of the rotorcraft, nothing else. | |
| #define | NavSetMaxSpeed(_speed) |
| Set maximum speed. | |
| #define | nav_IncreaseShift(x) |
| #define | nav_SetNavRadius(x) |
| #define | navigation_SetFlightAltitude(x) |
| #define | NavVerticalAutoPitchMode(_throttle) |
| Unused compat macros. | |
| #define | NavFollow(_i, _d, _h) |
Typedefs | |
| typedef void(* | navigation_stage_init) (void) |
| typedef void(* | navigation_goto) (struct EnuCoor_f *wp) |
| typedef void(* | navigation_route) (struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end) |
| typedef bool(* | navigation_approaching) (struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time) |
| typedef void(* | navigation_circle) (struct EnuCoor_f *wp_center, float radius) |
| typedef void(* | navigation_oval_init) (void) |
| typedef void(* | navigation_oval) (struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius) |
Functions | |
| void | nav_register_stage_init (navigation_stage_init nav_stage_init) |
| Registering functions. | |
| void | nav_register_goto_wp (navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching) |
| void | nav_register_circle (navigation_circle nav_circle) |
| void | nav_register_oval (navigation_oval_init _nav_oval_init, navigation_oval nav_oval) |
| void | nav_init (void) |
| Navigation Initialisation. | |
| void | nav_run (void) |
| void | nav_parse_BLOCK (uint8_t *buf) |
| void | nav_parse_MOVE_WP (uint8_t *buf) |
| float | get_dist2_to_waypoint (uint8_t wp_id) |
| Returns squared horizontal distance to given waypoint. | |
| float | get_dist2_to_point (struct EnuCoor_f *p) |
| Returns squared horizontal distance to given point. | |
| void | compute_dist2_to_home (void) |
| Computes squared distance to the HOME waypoint potentially sets too_far_from_home. | |
| void | nav_home (void) |
| Home mode navigation (circle around HOME). | |
| void | nav_reset_reference (void) |
| Reset the geographic reference to the current GPS fix. | |
| void | nav_reset_alt (void) |
| Reset the altitude reference to the current GPS alt. | |
| void | nav_periodic_task (void) |
| Navigation main: call to the code generated from the XML flight plan. | |
| bool | nav_is_in_flight (void) |
| void | nav_glide_points (struct EnuCoor_f *start_point, struct EnuCoor_f *end_point) |
| void | nav_set_heading_rad (float rad) |
| heading utility functions | |
| void | nav_set_heading_deg (float deg) |
| Set nav_heading in degrees. | |
| void | nav_set_heading_towards (float x, float y) |
| Set heading to point towards x,y position in local coordinates. | |
| void | nav_set_heading_towards_waypoint (uint8_t wp) |
| Set heading in the direction of a waypoint. | |
| void | nav_set_heading_towards_target (void) |
| Set heading in the direction of the target. | |
| void | nav_set_heading_current (void) |
| Set heading to the current yaw angle. | |
| void | nav_set_failsafe (void) |
| bool | nav_detect_ground (void) |
| static void | NavKillThrottle (void) |
| static void | NavResurrect (void) |
| 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. | |
| static void | NavGotoWaypoint (uint8_t wp) |
| static void | NavSegment (uint8_t wp_start, uint8_t wp_end) |
| static bool | NavApproaching (uint8_t wp, float approaching_time) |
| static bool | NavApproachingFrom (uint8_t to, uint8_t from, float approaching_time) |
| static void | NavCircleWaypoint (uint8_t wp_center, float radius) |
| static void | nav_oval_init (void) |
| static void | Oval (uint8_t wp1, uint8_t wp2, float radius) |
| static void | NavGlide (uint8_t wp_start, uint8_t wp_end) |
| Nav glide routine. | |
Variables | |
| struct RotorcraftNavigation | nav |
| float | flight_altitude |
| Dynamically adjustable, reset to nav_altitude when it is changing. | |
Rotorcraft navigation functions.
Definition in file navigation.h.
| struct RotorcraftNavigation |
General Navigation structure.
Definition at line 119 of file navigation.h.
| Data Fields | ||
|---|---|---|
| struct EnuCoor_f | accel | accel setpoint (in m/s) |
| struct EnuCoor_f | carrot | carrot position (also used for GCS display) |
| float | climb | climb setpoint (in m/s) |
| float | climb_vspeed | climb speed setting, mostly used in flight plans |
| float | descend_vspeed | descend speed setting, mostly used in flight plans |
| float | dist2_to_home | squared distance to home waypoint |
| float | failsafe_mode_dist2 | maximum squared distance to home wp before going to failsafe mode |
| float | fp_altitude | altitude setpoint from flight plan (in meters) |
| float | fp_max_speed | maximum speed setpoint from flight plan (in m/s), negative value means unset or invalid, do not use |
| float | heading | heading setpoint (in radians) |
| uint8_t | horizontal_mode | |
| float | nav_altitude | current altitude setpoint (in meters): might differ from fp_altitude depending on altitude shift from operator |
| navigation_approaching | nav_approaching | |
| navigation_circle | nav_circle | |
| navigation_goto | nav_goto | |
| navigation_oval | nav_oval | |
| navigation_oval_init | nav_oval_init | |
| navigation_route | nav_route | |
| navigation_stage_init | nav_stage_init | |
| float | pitch | pitch angle (in radians) |
| struct FloatQuat | quat | quaternion setpoint |
| float | radius | radius setpoint (in meters) |
| struct FloatRates | rates | rates setpoint (in rad/s) |
| float | roll | roll angle (in radians) |
| uint8_t | setpoint_mode | |
| struct EnuCoor_f | speed | speed setpoint (in m/s) |
| struct EnuCoor_f | target | final target position (in meters) |
| uint32_t | throttle | throttle command (in pprz_t) |
| bool | too_far_from_home | too_far flag |
| uint8_t | vertical_mode | |
| #define ARRIVED_AT_WAYPOINT 3.0f |
minimum horizontal distance to waypoint to mark as arrived
Definition at line 55 of file navigation.h.
Referenced by airspeed_consistency_periodic(), nav_approaching(), nav_approaching(), nav_check_wp_time(), nav_check_wp_time(), and nav_hybrid_approaching().
| #define CARROT 0 |
default approaching_time for a wp
Definition at line 70 of file navigation.h.
Referenced by fly_to_xy(), mission_nav_path(), mission_nav_path(), mission_nav_path(), mission_nav_segment(), mission_nav_segment(), mission_nav_segment(), mission_nav_wp(), mission_nav_wp(), mission_nav_wp(), nav_circle_XY(), nav_eight(), nav_gvf_survey_polygon_run(), nav_land_run(), nav_line_border_run(), nav_line_run(), nav_oval(), nav_oval(), nav_oval(), nav_route_xy(), nav_survey_disc_run(), nav_survey_polygon_run(), nav_survey_zamboni_run(), nav_takeoff_run(), nav_vertical_raster_run(), potential_task(), and snav_route().
| #define DEFAULT_CIRCLE_RADIUS 5.f |
default nav_circle_radius in meters
Definition at line 42 of file navigation.h.
Referenced by nav_init(), nav_lace_init(), nav_rosette_init(), nav_rotorcraft_hybrid_init(), nav_rotorcraft_init(), nav_rover_init(), and nav_trinity_init().
| #define FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME) |
Maximum distance from HOME waypoint before going into failsafe mode.
Definition at line 60 of file navigation.h.
Referenced by nav_init().
| #define GetAltRef | ( | ) |
Get current altitude reference for local coordinates.
This is the ground_alt from the flight plan at first, but might be updated later through a call to NavSetGroundReferenceHere() or NavSetAltitudeReferenceHere(), e.g. in the GeoInit flight plan block.
Definition at line 192 of file navigation.h.
| #define GetPosAlt | ( | ) |
Get current altitude above MSL.
Definition at line 183 of file navigation.h.
| #define GetPosHeight | ( | ) |
Get current height above reference.
Definition at line 185 of file navigation.h.
| #define GetPosX | ( | ) |
Get current x (east) position in local coordinates.
Definition at line 179 of file navigation.h.
| #define GetPosY | ( | ) |
Get current y (north) position in local coordinates.
Definition at line 181 of file navigation.h.
| #define NAV_CARROT_DIST 12 |
Carrot distance during navigation.
Definition at line 65 of file navigation.h.
Referenced by nav_advance_carrot(), nav_circle(), and nav_route().
| #define NAV_CLIMB_VSPEED 0.5f |
Definition at line 46 of file navigation.h.
Referenced by nav_init().
| #define NAV_DESCEND_VSPEED -0.8f |
Definition at line 50 of file navigation.h.
Referenced by nav_init().
| #define NAV_HORIZONTAL_MODE_ATTITUDE 3 |
Definition at line 88 of file navigation.h.
Referenced by guidance_h_from_nav(), and guidance_plane_attitude_from_nav().
| #define NAV_HORIZONTAL_MODE_CIRCLE 2 |
Definition at line 87 of file navigation.h.
Referenced by airspeed_consistency_periodic(), control_mixing_heewing_nav_run(), nav_circle(), nav_hybrid_circle(), and send_nav_status().
| #define NAV_HORIZONTAL_MODE_GUIDED 5 |
Definition at line 90 of file navigation.h.
Referenced by guidance_plane_attitude_from_nav().
| #define NAV_HORIZONTAL_MODE_NONE 4 |
Definition at line 89 of file navigation.h.
Referenced by guidance_h_from_nav(), guidance_plane_attitude_from_nav(), nav_heli_spinup_run(), and nav_heli_spinup_setup().
| #define NAV_HORIZONTAL_MODE_ROUTE 1 |
Definition at line 86 of file navigation.h.
Referenced by control_mixing_heewing_nav_run(), nav_hybrid_route(), nav_route(), nav_survey_rectangle_rotorcraft_setup(), and send_nav_status().
| #define NAV_HORIZONTAL_MODE_WAYPOINT 0 |
Nav modes these modes correspond to the flight plan instructions used to set the high level navigation.
Definition at line 85 of file navigation.h.
Referenced by nav_goto(), nav_home(), nav_hybrid_goto(), and nav_init().
| #define nav_IncreaseShift | ( | x | ) |
Definition at line 380 of file navigation.h.
| #define nav_SetNavRadius | ( | x | ) |
Definition at line 381 of file navigation.h.
| #define NAV_SETPOINT_MODE_ACCEL 2 |
Definition at line 102 of file navigation.h.
Referenced by guidance_h_update_reference().
| #define NAV_SETPOINT_MODE_ATTITUDE 3 |
Definition at line 103 of file navigation.h.
| #define NAV_SETPOINT_MODE_MANUAL 6 |
Definition at line 106 of file navigation.h.
| #define NAV_SETPOINT_MODE_POS 0 |
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which setpoint should be considered.
Definition at line 100 of file navigation.h.
Referenced by guidance_h_update_reference(), nav_circle(), nav_goto(), nav_home(), nav_hybrid_goto(), nav_init(), nav_route(), and oneloop_from_nav().
| #define NAV_SETPOINT_MODE_QUAT 4 |
Definition at line 104 of file navigation.h.
| #define NAV_SETPOINT_MODE_RATE 5 |
Definition at line 105 of file navigation.h.
| #define NAV_SETPOINT_MODE_SPEED 1 |
Definition at line 101 of file navigation.h.
Referenced by guidance_h_update_reference(), gvf_control_2D(), nav_hybrid_circle(), nav_hybrid_goto(), nav_hybrid_route(), and oneloop_from_nav().
| #define NAV_VERTICAL_MODE_ALT 2 |
Definition at line 94 of file navigation.h.
Referenced by guidance_v_from_nav(), nav_home(), nav_init(), and oneloop_from_nav().
| #define NAV_VERTICAL_MODE_CLIMB 1 |
Definition at line 93 of file navigation.h.
Referenced by guidance_plane_thrust_from_nav(), guidance_v_from_nav(), and oneloop_from_nav().
| #define NAV_VERTICAL_MODE_GUIDED 3 |
Definition at line 95 of file navigation.h.
Referenced by guidance_v_from_nav().
| #define NAV_VERTICAL_MODE_MANUAL 0 |
Definition at line 92 of file navigation.h.
Referenced by guidance_plane_thrust_from_nav(), guidance_v_from_nav(), nav_heli_spinup_run(), and nav_heli_spinup_setup().
| #define NavAttitude | ( | _roll | ) |
Definition at line 257 of file navigation.h.
| #define NavCheckWaypointTime | ( | wp, | |
| time ) |
Definition at line 250 of file navigation.h.
| #define NavCopyWaypoint waypoint_copy |
Definition at line 245 of file navigation.h.
| #define NavCopyWaypointPositionOnly waypoint_position_copy |
Definition at line 246 of file navigation.h.
| #define NavDetectGround | ( | ) |
Definition at line 226 of file navigation.h.
| #define NavFollow | ( | _i, | |
| _d, | |||
| _h ) |
Definition at line 395 of file navigation.h.
| #define NavHeading nav_set_heading_rad |
Set the heading of the rotorcraft, nothing else.
Definition at line 290 of file navigation.h.
| #define NAVIGATION_FREQUENCY 20 |
default navigation frequency
Definition at line 78 of file navigation.h.
| #define navigation_SetFlightAltitude | ( | x | ) |
Definition at line 382 of file navigation.h.
| #define NavSetAltitudeReferenceHere nav_reset_alt |
Definition at line 242 of file navigation.h.
| #define NavSetFailsafe nav_set_failsafe |
Definition at line 239 of file navigation.h.
| #define NavSetGroundReferenceHere nav_reset_reference |
Definition at line 241 of file navigation.h.
| #define NavSetMaxSpeed | ( | _speed | ) |
Set maximum speed.
Definition at line 293 of file navigation.h.
| #define NavSetWaypointHere waypoint_set_here_2d |
Definition at line 244 of file navigation.h.
Referenced by stereocam_droplet_periodic().
| #define NavStartDetectGround | ( | ) |
Definition at line 225 of file navigation.h.
| #define NavVerticalAltitudeMode | ( | _alt, | |
| _pre_climb ) |
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition at line 271 of file navigation.h.
| #define NavVerticalAutoPitchMode | ( | _throttle | ) |
| #define NavVerticalAutoThrottleMode | ( | _pitch | ) |
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition at line 265 of file navigation.h.
| #define NavVerticalClimbMode | ( | _climb | ) |
Set the vertical mode to climb control with the specified climb setpoint.
Definition at line 277 of file navigation.h.
| #define NavVerticalThrottleMode | ( | _throttle | ) |
Set the vertical mode to fixed throttle with the specified setpoint.
Definition at line 284 of file navigation.h.
| typedef bool(* navigation_approaching) (struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time) |
Definition at line 111 of file navigation.h.
| typedef void(* navigation_circle) (struct EnuCoor_f *wp_center, float radius) |
Definition at line 112 of file navigation.h.
| typedef void(* navigation_goto) (struct EnuCoor_f *wp) |
Definition at line 109 of file navigation.h.
Definition at line 114 of file navigation.h.
| typedef void(* navigation_oval_init) (void) |
Definition at line 113 of file navigation.h.
Definition at line 110 of file navigation.h.
| typedef void(* navigation_stage_init) (void) |
Definition at line 108 of file navigation.h.
|
extern |
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Updates dist2_to_home and potentially sets too_far_from_home
Definition at line 326 of file navigation.c.
|
extern |
Returns squared horizontal distance to given point.
Definition at line 308 of file navigation.c.
|
extern |
Returns squared horizontal distance to given waypoint.
Definition at line 318 of file navigation.c.
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp.
Definition at line 180 of file navigation.c.
References ARRIVED_AT_WAYPOINT, autopilot, ENU_BFP_OF_REAL, float_vect2_norm(), INT_VECT3_ZERO, stateGetPositionEnu_f(), VECT2_DIFF, EnuCoor_i::x, and EnuCoor_i::y.
Referenced by mission_nav_path(), mission_nav_path(), mission_nav_segment(), mission_nav_segment(), mission_nav_wp(), and mission_nav_wp().
|
extern |
Definition at line 267 of file navigation.c.
References autopilot.
Definition at line 282 of file navigation.c.
References float_vect2_norm2(), NavVerticalAltitudeMode, pos_diff, stateGetPositionEnu_f(), VECT2_DIFF, FloatVect2::x, FloatVect2::y, and EnuCoor_f::z.
Referenced by nav_land_run(), and NavGlide().
|
extern |
|
extern |
Definition at line 277 of file navigation.c.
|
inlinestatic |
Definition at line 350 of file navigation.h.
References nav.
|
extern |
Definition at line 115 of file navigation.c.
|
extern |
Definition at line 121 of file navigation.c.
|
extern |
|
extern |
Definition at line 399 of file navigation.c.
|
extern |
Definition at line 392 of file navigation.c.
|
extern |
Definition at line 404 of file navigation.c.
|
extern |
|
extern |
Reset the altitude reference to the current GPS alt.
Definition at line 241 of file navigation.c.
|
extern |
Reset the geographic reference to the current GPS fix.
Definition at line 234 of file navigation.c.
|
extern |
Definition at line 165 of file navigation.c.
|
extern |
Definition at line 380 of file navigation.c.
|
extern |
Set heading to the current yaw angle.
Definition at line 375 of file navigation.c.
|
extern |
Set nav_heading in degrees.
Definition at line 344 of file navigation.c.
|
extern |
|
extern |
Set heading to point towards x,y position in local coordinates.
Definition at line 350 of file navigation.c.
|
extern |
Set heading in the direction of the target.
Definition at line 369 of file navigation.c.
|
extern |
Set heading in the direction of a waypoint.
Definition at line 363 of file navigation.c.
|
inlinestatic |
Definition at line 320 of file navigation.h.
References nav, and waypoint_get_enu_f().
Definition at line 329 of file navigation.h.
References nav, and waypoint_get_enu_f().
|
inlinestatic |
Definition at line 340 of file navigation.h.
References nav, and waypoint_get_enu_f().
Nav glide routine.
Definition at line 367 of file navigation.h.
References nav_glide_points(), and waypoint_get_enu_f().
|
inlinestatic |
Definition at line 302 of file navigation.h.
References nav, and waypoint_get_enu_f().
|
inlinestatic |
Definition at line 229 of file navigation.h.
References AP_MODE_NAV, autopilot_get_mode(), and autopilot_set_motors_on().
|
inlinestatic |
Definition at line 233 of file navigation.h.
References AP_MODE_NAV, autopilot_get_mode(), and autopilot_set_motors_on().
Definition at line 311 of file navigation.h.
References nav, and waypoint_get_enu_f().
Definition at line 355 of file navigation.h.
References nav, and waypoint_get_enu_f().
|
extern |
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition at line 74 of file nav.c.
Referenced by formation_flight(), nav_init(), nav_set_altitude(), and nav_set_altitude().
|
extern |
Definition at line 51 of file navigation.c.