 |
Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
|
Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Fixedwing Navigation library. More...
#include "std.h"#include "paparazzi.h"#include "state.h"#include "modules/nav/nav_survey_rectangle.h"#include "modules/nav/common_flight_plan.h"#include "modules/nav/common_nav.h"#include "autopilot.h"#include "pprzlink/pprzlink_device.h"#include "pprzlink/pprzlink_transport.h"
Include dependency graph for nav.h: This graph shows which files directly or indirectly include this file:Go to the source code of this file.
Macros | |
| #define | NAVIGATION_FREQUENCY 20 |
| Default fixedwing navigation frequency. More... | |
| #define | NAV_GRAVITY 9.806 |
| #define | Square(_x) ((_x)*(_x)) |
| #define | DistanceSquare(p1_x, p1_y, p2_x, p2_y) (Square(p1_x-p2_x)+Square(p1_y-p2_y)) |
| #define | PowerVoltage() (electrical.vsupply) |
| #define | RcRoll(travel) (radio_control_get(RADIO_ROLL) * (float)travel /(float)MAX_PPRZ) |
| #define | NAV_MODE_ROLL 1 |
| #define | NAV_MODE_COURSE 2 |
| #define | HORIZONTAL_MODE_WAYPOINT 0 |
| #define | HORIZONTAL_MODE_ROUTE 1 |
| #define | HORIZONTAL_MODE_CIRCLE 2 |
| #define | NavGotoWaypoint(_wp) |
| #define | NavGotoPoint(_point) |
| #define | Eight(a, b, c) nav_eight((a), (b), (c)) |
| #define | Oval(a, b, c) nav_oval((b), (a), (c)) |
| #define | RCLost() RadioControlIsLost() |
| #define | NavFollow(_ac_id, _distance, _height) nav_follow(_ac_id, _distance, _height) |
| #define | NavGlide(_start_wp, _wp) nav_glide(_start_wp, _wp) |
| #define | NavCircleWaypoint(wp, radius) nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius) |
| #define | NavCircleCountNoRewind() (nav_circle_radians_no_rewind / (2*M_PI)) |
| #define | NavCircleCount() (fabs(nav_circle_radians) / (2*M_PI)) |
| #define | NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; }) |
| #define | CloseDegAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; }) |
| #define | NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr()) |
| True if x (in degrees) is close to the current QDR (less than 10 degrees) More... | |
| #define | NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(stateGetHorizontalSpeedDir_f())) |
| #define | NavSegment(_start, _end) nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y) |
| #define | NavApproaching(wp, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, last_x, last_y, time) |
| #define | NavApproachingFrom(wp, from, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, waypoints[from].x, waypoints[from].y, time) |
| #define | NavVerticalAutoThrottleMode(_pitch) |
| Set the climb control to auto-throttle with the specified pitch pre-command. More... | |
| #define | NavVerticalAutoPitchMode(_throttle) |
| Set the climb control to auto-pitch with the specified throttle pre-command. More... | |
| #define | NavVerticalAltitudeMode(_alt, _pre_climb) |
| Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command. More... | |
| #define | NavVerticalClimbMode(_climb) |
| Set the vertical mode to climb control with the specified climb setpoint. More... | |
| #define | NavVerticalThrottleMode(_throttle) |
| Set the vertical mode to fixed throttle with the specified setpoint. More... | |
| #define | NavHeading(_course) |
| #define | NavAttitude(_roll) |
| #define | NavSetMaxSpeed(_speed) {} |
| #define | nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; } |
| #define | nav_SetNavRadius(x) { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; } |
| #define | NavKillThrottle() { autopilot_set_kill_throttle(true); } |
| #define | GetPosX() (stateGetPositionEnu_f()->x) |
| Get current x (east) position in local coordinates. More... | |
| #define | GetPosY() (stateGetPositionEnu_f()->y) |
| Get current y (north) position in local coordinates. More... | |
| #define | GetPosAlt() (stateGetPositionUtm_f()->alt) |
| Get current altitude above MSL. More... | |
| #define | GetPosHeight() (stateGetPositionEnu_f()->z) |
| Get current height above reference. More... | |
| #define | GetAltRef() (ground_alt) |
| Get current altitude reference for local coordinates. More... | |
| #define | SEND_NAVIGATION(_trans, _dev) |
Enumerations | |
| enum | oval_status { OR12 , OC2 , OR21 , OC1 , OR12 , OC2 , OR21 , OC1 , OR12 , OC2 , OR21 , OC1 } |
Functions | |
| void | fly_to_xy (float x, float y) |
| Computes desired_x, desired_y and desired_course. More... | |
| void | nav_eight_init (void) |
| void | nav_eight (uint8_t, uint8_t, float) |
| Navigation along a figure 8. More... | |
| void | nav_oval_init (void) |
| void | nav_oval (uint8_t, uint8_t, float) |
| void | nav_periodic_task (void) |
| Navigation main: call to the code generated from the XML flight plan. More... | |
| void | nav_home (void) |
| Home mode navigation (circle around HOME) More... | |
| void | nav_init (void) |
| Navigation Initialisation. More... | |
| void | nav_without_gps (void) |
| Failsafe navigation without position estimation. More... | |
| void | nav_parse_BLOCK (struct link_device *dev, struct transport_tx *trans, uint8_t *buf) |
| void | nav_parse_MOVE_WP (struct link_device *dev, struct transport_tx *trans, uint8_t *buf) |
| void | nav_circle_XY (float x, float y, float radius) |
| Angle from center to mobile. More... | |
| void | nav_compute_baseleg (uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius) |
| void | nav_compute_final_from_glide (uint8_t wp_af, uint8_t wp_td, float glide) |
| void | nav_follow (uint8_t _ac_id, float _distance, float _height) |
| void | nav_glide (uint8_t start_wp, uint8_t wp) |
| void | nav_glide_alt (float start_alt, float final_alt) |
| void | nav_route_xy (float last_wp_x, float last_wp_y, float wp_x, float wp_y) |
| Computes the carrot position along the desired segment. More... | |
| bool | nav_approaching_xy (float x, float y, float from_x, float from_y, float approaching_time) |
| Decide if the UAV is approaching the current waypoint. More... | |
| void | DownlinkSendWpNr (uint8_t _wp) |
Fixedwing Navigation library.
This collection of macros and functions is used by the C code generated from the XML flight plan.
Definition in file nav.h.
| #define CloseDegAngles | ( | _c1, | |
| _c2 | |||
| ) | ({ float _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; }) |
| #define GetAltRef | ( | ) | (ground_alt) |
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.
| #define GetPosAlt | ( | ) | (stateGetPositionUtm_f()->alt) |
| #define GetPosHeight | ( | ) | (stateGetPositionEnu_f()->z) |
| #define GetPosX | ( | ) | (stateGetPositionEnu_f()->x) |
| #define GetPosY | ( | ) | (stateGetPositionEnu_f()->y) |
| #define nav_SetNavRadius | ( | x | ) | { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; } |
| #define NavAttitude | ( | _roll | ) |
| #define NavCircleCount | ( | ) | (fabs(nav_circle_radians) / (2*M_PI)) |
| #define NavCircleCountNoRewind | ( | ) | (nav_circle_radians_no_rewind / (2*M_PI)) |
| #define NavCircleQdr | ( | ) | ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; }) |
| #define NavCircleWaypoint | ( | wp, | |
| radius | |||
| ) | nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius) |
| #define NavCourseCloseTo | ( | x | ) | CloseDegAngles(x, DegOfRad(stateGetHorizontalSpeedDir_f())) |
| #define NavFollow | ( | _ac_id, | |
| _distance, | |||
| _height | |||
| ) | nav_follow(_ac_id, _distance, _height) |
| #define NavGotoPoint | ( | _point | ) |
| #define NavGotoWaypoint | ( | _wp | ) |
| #define NavHeading | ( | _course | ) |
| #define NAVIGATION_FREQUENCY 20 |
| #define NavKillThrottle | ( | void | ) | { autopilot_set_kill_throttle(true); } |
| #define NavQdrCloseTo | ( | x | ) | CloseDegAngles(x, NavCircleQdr()) |
| #define NavSegment | ( | _start, | |
| _end | |||
| ) | nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y) |
| #define NavVerticalAltitudeMode | ( | _alt, | |
| _pre_climb | |||
| ) |
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
| #define NavVerticalAutoPitchMode | ( | _throttle | ) |
Set the climb control to auto-pitch with the specified throttle pre-command.
| #define NavVerticalAutoThrottleMode | ( | _pitch | ) |
Set the climb control to auto-throttle with the specified pitch 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 RCLost | ( | ) | RadioControlIsLost() |
| #define RcRoll | ( | travel | ) | (radio_control_get(RADIO_ROLL) * (float)travel /(float)MAX_PPRZ) |
| #define SEND_NAVIGATION | ( | _trans, | |
| _dev | |||
| ) |
| enum oval_status |
| void DownlinkSendWpNr | ( | uint8_t | _wp | ) |
Definition at line 497 of file nav.c.
References DefaultChannel, DefaultDevice, DownlinkSendWp(), and nb_waypoint.
Referenced by nav_catapult_run().
Here is the call graph for this function: Here is the caller graph for this function:| void fly_to_xy | ( | float | x, |
| float | y | ||
| ) |
Computes desired_x, desired_y and desired_course.
Definition at line 356 of file nav.c.
References CARROT, desired_x, desired_y, h_ctl_course_pgain, h_ctl_course_setpoint, h_ctl_roll_max_setpoint, h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_COURSE, LATERAL_MODE_ROLL, nav_mode, NAV_MODE_COURSE, s, stateGetHorizontalSpeedDir_f(), stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.
Referenced by formation_flight(), mission_nav_wp(), nav_circle_XY(), nav_route_xy(), and potential_task().
Here is the call graph for this function: Here is the caller graph for this function:| bool nav_approaching_xy | ( | float | x, |
| float | y, | ||
| float | from_x, | ||
| float | from_y, | ||
| float | approaching_time | ||
| ) |
Decide if the UAV is approaching the current waypoint.
Computes dist2_to_wp and compare it to square carrot. Return true if it is smaller. Else computes by scalar products if uav has not gone past waypoint. approaching_time can be negative and in this case, the UAV will fly after the waypoint for the given number of seconds.
distance to waypoint in x
distance to waypoint in y
Definition at line 325 of file nav.c.
References dist2_to_wp, stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.
Referenced by gvf_nav_survey_polygon_run(), mission_nav_path(), mission_nav_segment(), mission_nav_wp(), nav_catapult_run(), nav_eight(), nav_land_run(), nav_line_osam_run(), nav_oval(), nav_spiral_run(), nav_survey_poly_osam_run(), nav_survey_polygon_run(), nav_survey_zamboni_run(), nav_takeoff_run(), and snav_route().
Here is the call graph for this function: Here is the caller graph for this function:| void nav_circle_XY | ( | float | x, |
| float | y, | ||
| float | radius | ||
| ) |
Angle from center to mobile.
Angle from center to mobile.
Clockwise iff radius > 0
Computes a prebank. Go straight if inside or outside the circle
Definition at line 108 of file nav.c.
References CARROT, circle_bank, DistanceSquare, fly_to_xy(), horizontal_mode, HORIZONTAL_MODE_CIRCLE, Min, nav_circle_radians, nav_circle_radians_no_rewind, nav_circle_radius, nav_circle_trigo_qdr, nav_circle_x, nav_circle_y, NAV_GRAVITY, nav_in_circle, nav_in_segment, nav_mode, NAV_MODE_COURSE, nav_shift, Square, stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.
Referenced by mission_nav_circle(), nav_eight(), nav_flower_run(), nav_launcher_run(), nav_line_border_run(), nav_line_osam_run(), nav_line_run(), nav_oval(), nav_skid_landing_run(), nav_spiral_run(), nav_survey_poly_osam_run(), nav_survey_polygon_run(), nav_survey_zamboni_run(), nav_vertical_raster_run(), snav_circle1(), snav_circle2(), and snav_on_time().
Here is the call graph for this function: Here is the caller graph for this function:Definition at line 220 of file nav.c.
References point::a, baseleg_out_qdr, nav_radius, waypoints, point::x, and point::y.
Navigation along a figure 8.
The cross center is defined by the waypoint [target], the center of one of the circles is defined by [c1]. Altitude is given by [target]. The navigation goes through 6 states: C1 (circle around [c1]), R1T, RT2 (route from circle 1 to circle 2 over [target]), C2 and R2T, RT1. If necessary, the [c1] waypoint is moved in the direction of [target] to be not far than [2*radius].
Definition at line 633 of file nav.c.
References point::a, UtmCoor_f::alt, C1, c1, C2, c2, CARROT, InitStage, nav_approaching_xy(), nav_circle_XY(), nav_route_xy(), NavCircleWaypoint, NavQdrCloseTo, R1T, R2T, RT1, RT2, target, waypoints, point::x, and point::y.
Here is the call graph for this function:| void nav_follow | ( | uint8_t | _ac_id, |
| float | _distance, | ||
| float | _height | ||
| ) |
Definition at line 158 of file nav.c.
References nav_glide_alt(), and waypoints.
Here is the call graph for this function:| void nav_glide_alt | ( | float | start_alt, |
| float | final_alt | ||
| ) |
Definition at line 163 of file nav.c.
References nav_leg_length, nav_leg_progress, NavVerticalAltitudeMode, and stateGetHorizontalSpeedNorm_f().
Referenced by nav_glide(), and nav_land_run().
Here is the call graph for this function: Here is the caller graph for this function:| void nav_home | ( | void | ) |
Definition at line 762 of file nav.c.
References point::a, CARROT, InitStage, LINE_START_FUNCTION, LINE_STOP_FUNCTION, nav_approaching_xy(), nav_circle_XY(), nav_oval_count, nav_route_xy(), NavQdrCloseTo, OC1, OC2, OR12, OR21, waypoints, point::x, and point::y.
Referenced by nav_register_oval().
Here is the call graph for this function: Here is the caller graph for this function:| void nav_oval_init | ( | void | ) |
Definition at line 756 of file nav.c.
References nav_oval_count, and OC2.
Referenced by nav_register_oval().
Here is the caller graph for this function:| void nav_parse_BLOCK | ( | struct link_device * | dev, |
| struct transport_tx * | trans, | ||
| uint8_t * | buf | ||
| ) |
Definition at line 585 of file nav.c.
References dev, nav_goto_block(), and SEND_NAVIGATION.
Here is the call graph for this function:| void nav_parse_MOVE_WP | ( | struct link_device * | dev, |
| struct transport_tx * | trans, | ||
| uint8_t * | buf | ||
| ) |
| void nav_periodic_task | ( | void | ) |
| void nav_route_xy | ( | float | last_wp_x, |
| float | last_wp_y, | ||
| float | wp_x, | ||
| float | wp_y | ||
| ) |
Computes the carrot position along the desired segment.
distance of carrot (in meter)
Definition at line 382 of file nav.c.
References CARROT, fly_to_xy(), horizontal_mode, HORIZONTAL_MODE_ROUTE, nav_carrot_leg_progress, nav_in_circle, nav_in_segment, nav_leg_length, nav_leg_progress, nav_segment_x_1, nav_segment_x_2, nav_segment_y_1, nav_segment_y_2, nav_shift, stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.
Referenced by mission_nav_path(), mission_nav_segment(), nav_bungee_takeoff_run(), nav_eight(), nav_flower_run(), nav_land_run(), nav_launcher_run(), nav_line_osam_run(), nav_oval(), nav_points(), nav_skid_landing_run(), nav_spiral_run(), nav_survey_poly_osam_run(), nav_survey_rectangle(), nav_survey_zamboni_run(), and snav_route().
Here is the call graph for this function: Here is the caller graph for this function:| void nav_without_gps | ( | void | ) |
Failsafe navigation without position estimation.
Just set attitude and throttle to FAILSAFE values to prevent the plane from crashing.
Definition at line 569 of file nav.c.
References h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_ROLL, MAX_PPRZ, nav_pitch, nav_throttle_setpoint, TRIM_UPPRZ, v_ctl_mode, and V_CTL_MODE_AUTO_THROTTLE.
Referenced by navigation_task().
Here is the caller graph for this function:
|
extern |
Definition at line 219 of file nav.c.
Referenced by nav_compute_baseleg(), and nav_land_run().
|
extern |
|
extern |
|
extern |
Definition at line 308 of file nav.c.
Referenced by cam_segment_periodic(), fly_to_xy(), formation_flight(), gvf_parametric_control_2D(), potential_task(), and send_desired().
| float flight_altitude |
|
extern |
Definition at line 313 of file nav.c.
Referenced by nav_init().
|
extern |
Definition at line 311 of file nav.c.
Referenced by nav_init().
|
extern |
Definition at line 312 of file nav.c.
Referenced by nav_init().
|
extern |
Definition at line 70 of file nav.c.
Referenced by nav_circle_XY(), nav_route_xy(), and send_mode().
|
extern |
Definition at line 47 of file nav.c.
Referenced by gvf_nav_survey_polygon_run(), nav_init_stage(), and nav_survey_polygon_run().
|
extern |
Status on the current circle.
Definition at line 53 of file nav.c.
Referenced by nav_circle_XY(), and nav_init_stage().
|
extern |
Definition at line 54 of file nav.c.
Referenced by nav_circle_XY(), and nav_init_stage().
|
extern |
Definition at line 55 of file nav.c.
Referenced by nav_circle_XY(), and nav_land_run().
|
extern |
Definition at line 68 of file nav.c.
Referenced by flight_benchmark_periodic(), nav_circle_XY(), and send_circle().
|
extern |
Definition at line 83 of file nav.c.
Referenced by nav_init().
|
extern |
Definition at line 66 of file nav.c.
Referenced by calculate_wind_no_airspeed(), nav_circle_XY(), nav_init_stage(), nav_periodic_task(), nav_route_xy(), nav_survey_rectangle(), nav_survey_rectangle_rotorcraft_run(), and send_circle().
|
extern |
Definition at line 67 of file nav.c.
Referenced by nav_circle_XY(), nav_init_stage(), nav_route_xy(), nav_survey_rectangle(), nav_survey_rectangle_rotorcraft_run(), and send_segment().
|
extern |
Definition at line 90 of file nav.c.
Referenced by fly_to_xy(), nav_circle_XY(), and nav_init().
|
extern |
Navigation along a figure O.
One side leg is defined by waypoints [p1] and [p2]. The navigation goes through 4 states: OC1 (half circle next to [p1]), OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg).
Initial state is the route along the desired segment (OC2).
Definition at line 754 of file nav.c.
Referenced by nav_oval(), and nav_oval_init().
|
extern |
Definition at line 310 of file nav.c.
Referenced by attitude_loop(), nav_home(), nav_without_gps(), v_ctl_climb_auto_throttle_loop(), v_ctl_climb_loop(), v_ctl_guidance_loop(), v_ctl_landing_loop(), and v_ctl_set_pitch().
|
extern |
Definition at line 56 of file nav.c.
Referenced by nav_compute_baseleg(), nav_flower_run(), nav_init(), and parachute_compute_approach().
|
extern |
Definition at line 69 of file nav.c.
Referenced by flight_benchmark_periodic(), nav_route_xy(), and send_segment().
|
extern |
Definition at line 56 of file nav.c.
Referenced by nav_circle_XY(), nav_init_stage(), and nav_route_xy().
|
extern |
Definition at line 88 of file nav.c.
Referenced by nav_periodic_task(), nav_survey_rectangle(), and send_survey().
|
extern |
Definition at line 86 of file nav.c.
Referenced by nav_init(), nav_survey_disc_setup(), nav_survey_poly_osam_run(), nav_survey_poly_run(), nav_survey_rectangle(), nav_survey_rectangle_init(), nav_survey_rectangle_rotorcraft_run(), and nav_survey_rectangle_rotorcraft_setup().
|
extern |
Definition at line 87 of file nav.c.
Referenced by nav_survey_rectangle(), nav_survey_rectangle_init(), nav_survey_rectangle_rotorcraft_run(), nav_survey_rectangle_rotorcraft_setup(), and send_survey().
|
extern |
Definition at line 309 of file nav.c.
Referenced by attitude_loop(), nav_without_gps(), and v_ctl_guidance_loop().
|
extern |
1.9.1 
