|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Rover navigation functions. More...
#include "std.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"Go to the source code of this file.
Data Structures | |
| struct | RoverNavigation |
| General Navigation structure. More... | |
Macros | |
| #define | CARROT 0.f |
| default approaching_time for a wp | |
| #define | CARROT_DIST 5.f |
| #define | NAVIGATION_FREQUENCY 20 |
| default navigation frequency | |
| #define | DEFAULT_CIRCLE_RADIUS 6.0f |
| default nav_circle_radius in meters | |
| #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_MODE_WAYPOINT 0 |
| Nav modes. | |
| #define | NAV_MODE_ROUTE 1 |
| #define | NAV_MODE_CIRCLE 2 |
| #define | NAV_MODE_HEADING 3 |
| #define | NAV_MODE_MANUAL 4 |
| #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 | NormCourse(x) |
| Normalize a degree angle between 0 and 359. | |
| #define | NormCourseRad(x) |
| Normalize a rad angle between 0 and 2*PI. | |
| #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 | NavVerticalThrottleMode(_speed) |
| #define | NavHeading nav_set_heading_rad |
| Set the heading of the rover, nothing else. | |
| #define | NavSetMaxSpeed(_speed) |
| #define | navigation_IncreaseShift(x) |
| Settings handlers. | |
| #define | navigation_SetNavRadius(x) |
| #define | NavFollow nav_follow(_id, _dist, _height) {} |
| #define | NavGlide(_start_wp, _wp) |
| Unused compat macros. | |
| #define | NavVerticalAutoThrottleMode(_pitch) |
| #define | NavVerticalAutoPitchMode(_throttle) |
| #define | NavVerticalAltitudeMode(_alt, _pre_climb) |
| #define | NavVerticalClimbMode(_climb) |
Typedefs | |
| typedef void(* | nav_rover_stage_init) (void) |
| typedef void(* | nav_rover_goto) (struct EnuCoor_f *wp) |
| typedef void(* | nav_rover_route) (struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end) |
| typedef bool(* | nav_rover_approaching) (struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time) |
| typedef void(* | nav_rover_circle) (struct EnuCoor_f *wp_center, float radius) |
| typedef void(* | nav_rover_oval_init) (void) |
| typedef void(* | nav_rover_oval) (struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius) |
Functions | |
| void | nav_register_stage_init (nav_rover_stage_init nav_stage_init) |
| Registering functions. | |
| void | nav_register_goto_wp (nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching) |
| void | nav_register_circle (nav_rover_circle nav_circle) |
| void | nav_register_oval (nav_rover_oval_init nav_oval_init, nav_rover_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) |
| void | set_exception_flag (uint8_t flag_num) |
| 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_set_manual (float speed, float turn) |
| 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_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) |
| static void | NavKillThrottle (void) |
| static void | NavResurrect (void) |
| bool | nav_check_wp_time (struct EnuCoor_f *wp, float 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) |
Variables | |
| struct RoverNavigation | nav |
Rover navigation functions.
Definition in file navigation.h.
| struct RoverNavigation |
General Navigation structure.
Definition at line 89 of file navigation.h.
| Data Fields | ||
|---|---|---|
| struct EnuCoor_f | carrot | carrot position |
| float | dist2_to_home | squared distance to home waypoint |
| bool | exception_flag[10] | array of flags that might be used in flight plans |
| float | failsafe_mode_dist2 | maximum squared distance to home wp before going to failsafe mode |
| float | heading | heading setpoint (in radians) |
| struct EnuCoor_f | last_pos | last stage position |
| uint8_t | mode | |
| nav_rover_approaching | nav_approaching | |
| nav_rover_circle | nav_circle | |
| nav_rover_goto | nav_goto | |
| nav_rover_oval | nav_oval | |
| nav_rover_oval_init | nav_oval_init | |
| nav_rover_route | nav_route | |
| nav_rover_stage_init | nav_stage_init | |
| float | radius | radius setpoint |
| float | shift | lateral shift (in meters) |
| float | speed | speed setpoint |
| struct EnuCoor_f | target | final target |
| bool | too_far_from_home | too_far flag |
| float | turn | turn rate setpoint |
| #define ARRIVED_AT_WAYPOINT 3.0f |
minimum horizontal distance to waypoint to mark as arrived
Definition at line 63 of file navigation.h.
| #define CARROT 0.f |
default approaching_time for a wp
Definition at line 39 of file navigation.h.
| #define CARROT_DIST 5.f |
Definition at line 43 of file navigation.h.
Referenced by nav_circle(), and nav_route().
| #define DEFAULT_CIRCLE_RADIUS 6.0f |
default nav_circle_radius in meters
Definition at line 58 of file navigation.h.
| #define FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME) |
Maximum distance from HOME waypoint before going into failsafe mode.
Definition at line 68 of file navigation.h.
| #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 150 of file navigation.h.
| #define GetPosAlt | ( | ) |
Get current altitude above MSL.
Definition at line 141 of file navigation.h.
| #define GetPosHeight | ( | ) |
Get current height above reference.
Definition at line 143 of file navigation.h.
| #define GetPosX | ( | ) |
Get current x (east) position in local coordinates.
Definition at line 137 of file navigation.h.
| #define GetPosY | ( | ) |
Get current y (north) position in local coordinates.
Definition at line 139 of file navigation.h.
| #define NAV_MODE_CIRCLE 2 |
Definition at line 74 of file navigation.h.
Referenced by nav_circle(), NavCircleWaypoint(), and send_nav_status().
| #define NAV_MODE_HEADING 3 |
Definition at line 75 of file navigation.h.
| #define NAV_MODE_MANUAL 4 |
Definition at line 76 of file navigation.h.
| #define NAV_MODE_ROUTE 1 |
Definition at line 73 of file navigation.h.
Referenced by nav_route(), NavSegment(), and send_nav_status().
| #define NAV_MODE_WAYPOINT 0 |
Nav modes.
Definition at line 72 of file navigation.h.
Referenced by nav_goto(), nav_home(), nav_init(), and NavGotoWaypoint().
| #define NavAttitude | ( | _roll | ) |
Definition at line 223 of file navigation.h.
| #define NavCheckWaypointTime | ( | wp, | |
| time ) |
Definition at line 215 of file navigation.h.
| #define NavCopyWaypoint waypoint_copy |
Definition at line 209 of file navigation.h.
| #define NavCopyWaypointPositionOnly waypoint_position_copy |
Definition at line 210 of file navigation.h.
| #define NavFollow nav_follow(_id, _dist, _height) {} |
Definition at line 316 of file navigation.h.
| #define NavGlide | ( | _start_wp, | |
| _wp ) |
| #define NavHeading nav_set_heading_rad |
Set the heading of the rover, nothing else.
Definition at line 236 of file navigation.h.
| #define NAVIGATION_FREQUENCY 20 |
default navigation frequency
Definition at line 51 of file navigation.h.
| #define navigation_IncreaseShift | ( | x | ) |
Settings handlers.
Definition at line 310 of file navigation.h.
| #define navigation_SetNavRadius | ( | x | ) |
Definition at line 312 of file navigation.h.
| #define NavSetAltitudeReferenceHere nav_reset_alt |
Definition at line 206 of file navigation.h.
| #define NavSetFailsafe nav_set_failsafe |
Definition at line 203 of file navigation.h.
| #define NavSetGroundReferenceHere nav_reset_reference |
Definition at line 205 of file navigation.h.
| #define NavSetMaxSpeed | ( | _speed | ) |
Definition at line 238 of file navigation.h.
| #define NavSetWaypointHere waypoint_set_here_2d |
Definition at line 208 of file navigation.h.
| #define NavVerticalAltitudeMode | ( | _alt, | |
| _pre_climb ) |
Definition at line 325 of file navigation.h.
| #define NavVerticalAutoPitchMode | ( | _throttle | ) |
Definition at line 324 of file navigation.h.
| #define NavVerticalAutoThrottleMode | ( | _pitch | ) |
Definition at line 323 of file navigation.h.
| #define NavVerticalClimbMode | ( | _climb | ) |
Definition at line 326 of file navigation.h.
| #define NavVerticalThrottleMode | ( | _speed | ) |
Definition at line 230 of file navigation.h.
| #define NormCourse | ( | x | ) |
Normalize a degree angle between 0 and 359.
Definition at line 154 of file navigation.h.
Referenced by nav_circle_qdr().
| #define NormCourseRad | ( | x | ) |
Normalize a rad angle between 0 and 2*PI.
Definition at line 159 of file navigation.h.
Referenced by gps_uavcan_cb(), nav_land_run(), nav_set_heading_rad(), object_tracking_run(), and recover_gps_data().
| typedef bool(* nav_rover_approaching) (struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time) |
Definition at line 81 of file navigation.h.
| typedef void(* nav_rover_circle) (struct EnuCoor_f *wp_center, float radius) |
Definition at line 82 of file navigation.h.
| typedef void(* nav_rover_goto) (struct EnuCoor_f *wp) |
Definition at line 79 of file navigation.h.
Definition at line 84 of file navigation.h.
| typedef void(* nav_rover_oval_init) (void) |
Definition at line 83 of file navigation.h.
Definition at line 80 of file navigation.h.
| typedef void(* nav_rover_stage_init) (void) |
Definition at line 78 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.
References p, pos_diff, stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.
Referenced by get_dist2_to_waypoint(), nav_goto(), nav_goto(), nav_hybrid_goto(), nav_hybrid_route(), nav_route(), nav_route(), and rotwing_state_choose_state_by_dist().
|
extern |
Returns squared horizontal distance to given waypoint.
Definition at line 318 of file navigation.c.
References get_dist2_to_point(), and waypoints.
Referenced by compute_dist2_to_home().
| bool nav_check_wp_time | ( | struct EnuCoor_f * | wp, |
| float | stay_time ) |
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp.
Definition at line 135 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.
|
extern |
Home mode navigation (circle around HOME).
Home mode navigation (circle around HOME).
Nominal speed
Definition at line 424 of file nav.c.
References autopilot, compute_dist2_to_home(), dist2_to_home, dist2_to_wp, FAILSAFE_HOME_RADIUS, ground_alt, nav, nav_altitude, NAV_HORIZONTAL_MODE_WAYPOINT, NAV_MODE_WAYPOINT, nav_pitch, nav_run(), nav_set_altitude(), NAV_SETPOINT_MODE_POS, NAV_VERTICAL_MODE_ALT, NavCircleWaypoint, v_ctl_mode, V_CTL_MODE_AUTO_ALT, V_CTL_MODE_AUTO_THROTTLE, v_ctl_throttle_setpoint, VECT3_COPY, waypoint_get_alt(), and waypoints.
Referenced by autopilot_static_periodic(), and navigation_task().
|
extern |
Navigation Initialisation.
Definition at line 532 of file nav.c.
References common_flight_plan_init(), DEFAULT_CIRCLE_RADIUS, DefaultPeriodic, empty_approaching(), empty_circle(), empty_goto(), empty_oval(), empty_oval_init(), empty_route(), empty_stage_init(), FAILSAFE_MODE_DISTANCE, flight_altitude, float_quat_identity(), FLOAT_RATES_ZERO, FLOAT_VECT3_ZERO, fp_climb, fp_pitch, fp_throttle, ground_alt, nav, NAV_CLIMB_VSPEED, NAV_DESCEND_VSPEED, NAV_GLIDE_PITCH_TRIM, nav_glide_pitch_trim, nav_ground_speed_pgain, nav_ground_speed_setpoint, NAV_HORIZONTAL_MODE_WAYPOINT, nav_mode, NAV_MODE_COURSE, NAV_MODE_WAYPOINT, nav_radius, NAV_SETPOINT_MODE_POS, nav_survey_shift, NAV_VERTICAL_MODE_ALT, register_periodic_telemetry(), send_circle(), send_nav(), send_nav_ref(), send_segment(), send_survey(), send_wp_moved(), VECT3_COPY, waypoints, and waypoints_init().
|
extern |
Definition at line 277 of file navigation.c.
References autopilot_in_flight().
Referenced by follow_me(), and nav_land_run().
|
inlinestatic |
Definition at line 293 of file navigation.h.
References nav.
|
extern |
Definition at line 115 of file navigation.c.
References nav_goto_block().
|
extern |
Definition at line 121 of file navigation.c.
References LlaCoor_i::alt, LlaCoor_i::lat, LlaCoor_i::lon, stateGetHmslOrigin_i(), stateGetLlaOrigin_i(), stateIsLocalCoordinateValid(), and waypoint_move_lla().
|
extern |
Navigation main: call to the code generated from the XML flight plan.
Definition at line 445 of file nav.c.
References block_time, circle_bank, compute_dist2_to_home(), dist2_to_wp, h_ctl_course_pre_bank, nav_in_circle, nav_run(), nav_set_altitude(), nav_survey_active, NAVIGATION_FREQUENCY, stage_time, V_CTL_AUTO_THROTTLE_STANDARD, v_ctl_auto_throttle_submode, v_ctl_mode, and V_CTL_MODE_AUTO_CLIMB.
Referenced by autopilot_static_periodic(), and navigation_task().
|
extern |
Definition at line 399 of file navigation.c.
References nav, and nav_circle().
Referenced by nav_rotorcraft_hybrid_init(), nav_rotorcraft_init(), and nav_rover_init().
|
extern |
Definition at line 392 of file navigation.c.
References nav, nav_approaching(), nav_goto(), and nav_route().
Referenced by nav_rotorcraft_hybrid_init(), nav_rotorcraft_init(), and nav_rover_init().
|
extern |
Definition at line 404 of file navigation.c.
References _nav_oval_init(), nav, nav_oval(), and nav_oval_init().
Referenced by nav_rotorcraft_init(), and nav_rover_init().
|
extern |
Registering functions.
Registering functions.
Definition at line 387 of file navigation.c.
References nav, and nav_stage_init().
Referenced by nav_rotorcraft_init(), and nav_rover_init().
|
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.
References nav, nav_advance_carrot(), nav_set_altitude(), and VECT2_COPY.
Referenced by nav_home(), and nav_periodic_task().
|
extern |
Definition at line 380 of file navigation.c.
References AP_MODE_FAILSAFE, and autopilot_set_mode().
|
extern |
Set heading to the current yaw angle.
Definition at line 375 of file navigation.c.
References nav, FloatEulers::psi, and stateGetNedToBodyEulers_f().
|
extern |
Set nav_heading in degrees.
Definition at line 344 of file navigation.c.
References nav_set_heading_rad().
Referenced by nav_survey_poly_setup(), and nav_survey_rectangle_rotorcraft_setup().
|
extern |
heading utility functions
heading utility functions
Definition at line 337 of file navigation.c.
References nav, and NormCourseRad.
Referenced by nav_set_heading_deg(), and stereocam_droplet_periodic().
|
extern |
Set heading to point towards x,y position in local coordinates.
Definition at line 350 of file navigation.c.
References nav, pos_diff, stateGetPositionEnu_f(), target, VECT2_DIFF, VECT2_NORM2, FloatVect2::x, and FloatVect2::y.
Referenced by nav_set_heading_towards_target(), and nav_set_heading_towards_waypoint().
|
extern |
Set heading in the direction of the target.
Definition at line 369 of file navigation.c.
References nav, and nav_set_heading_towards().
|
extern |
Set heading in the direction of a waypoint.
Definition at line 363 of file navigation.c.
References nav_set_heading_towards(), WaypointX, and WaypointY.
|
extern |
|
inlinestatic |
Definition at line 263 of file navigation.h.
Definition at line 273 of file navigation.h.
|
inlinestatic |
Definition at line 284 of file navigation.h.
References nav, NAV_MODE_CIRCLE, RoverNavigation::radius, and waypoints.
|
inlinestatic |
Definition at line 246 of file navigation.h.
References nav, NAV_MODE_WAYPOINT, and waypoints.
|
inlinestatic |
Definition at line 194 of file navigation.h.
References AP_MODE_NAV, autopilot_get_mode(), and autopilot_set_motors_on().
|
inlinestatic |
Definition at line 198 of file navigation.h.
References AP_MODE_NAV, autopilot_get_mode(), and autopilot_set_motors_on().
Definition at line 255 of file navigation.h.
References nav, NAV_MODE_ROUTE, and waypoints.
Definition at line 300 of file navigation.h.
References nav, RoverNavigation::radius, and waypoints.
|
extern |
Definition at line 57 of file navigation.c.
References nav, and set_exception_flag().
Referenced by set_exception_flag().
|
extern |
Definition at line 51 of file navigation.c.
Referenced by airspeed_consistency_periodic(), autopilot_static_on_rc_frame(), autopilot_static_periodic(), compute_dist2_to_home(), control_mixing_heewing_nav_run(), follow_diagonal_approach(), follow_me(), guidance_h_from_nav(), guidance_h_nav_enter(), guidance_h_update_reference(), guidance_indi_enter(), guidance_pid_h_enter(), guidance_plane_attitude_from_nav(), guidance_plane_set_pitch(), guidance_plane_thrust_from_nav(), guidance_v_from_nav(), gvf_control_2D(), gvf_nav_approaching(), increase_nav_heading(), increase_nav_heading(), ins_ekf2_publish_attitude(), mission_nav_circle(), mission_nav_circle(), mission_nav_path(), mission_nav_path(), mission_nav_segment(), mission_nav_segment(), mission_nav_wp(), mission_nav_wp(), nav_advance_carrot(), nav_circle(), nav_circle(), nav_goto(), nav_goto(), nav_heli_spinup_run(), nav_heli_spinup_setup(), nav_home(), nav_hybrid_circle(), nav_hybrid_goto(), nav_hybrid_route(), nav_init(), nav_init_stage(), nav_land_run(), nav_oval(), nav_oval(), nav_oval_init(), nav_poles_run(), nav_register_circle(), nav_register_circle(), nav_register_goto_wp(), nav_register_goto_wp(), nav_register_oval(), nav_register_oval(), nav_register_stage_init(), nav_register_stage_init(), nav_route(), nav_route(), nav_run(), nav_set_altitude(), nav_set_heading_current(), nav_set_heading_rad(), nav_set_heading_towards(), nav_set_heading_towards_target(), nav_survey_hybrid_run(), nav_survey_poly_run(), nav_survey_rectangle_rotorcraft_run(), nav_survey_rectangle_rotorcraft_setup(), NavApproaching(), NavApproachingFrom(), NavCircleWaypoint(), NavGotoWaypoint(), NavSegment(), object_tracking_run(), oneloop_andi_run(), oneloop_from_nav(), oneloop_nB_RM(), Oval(), rotorcraft_cam_periodic(), run_avoid_navigation_onvision(), send_fp(), send_nav_status(), send_nav_status(), set_exception_flag(), and stereocam_droplet_periodic().