|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Rotorcraft navigation functions. More...
#include "firmwares/rotorcraft/navigation.h"#include "pprz_debug.h"#include "state.h"#include "autopilot.h"#include "generated/modules.h"#include "generated/flight_plan.h"#include "modules/ins/ins.h"#include "firmwares/rotorcraft/guidance/guidance_h.h"#include "math/pprz_algebra_int.h"#include "modules/datalink/downlink.h"#include "pprzlink/messages.h"#include "mcu_periph/uart.h"#include <stdio.h>Go to the source code of this file.
Macros | |
| #define | NAV_C |
| #define | CLOSE_TO_WAYPOINT 15.f |
Functions | |
| static void | empty_stage_init (void) |
| Empty navigation functions set at init. | |
| static void | empty_goto (struct EnuCoor_f *wp UNUSED) |
| static void | empty_route (struct EnuCoor_f *wp_start UNUSED, struct EnuCoor_f *wp_end UNUSED) |
| static bool | empty_approaching (struct EnuCoor_f *wp_to UNUSED, struct EnuCoor_f *wp_from UNUSED, float approaching_time UNUSED) |
| static void | empty_circle (struct EnuCoor_f *wp_center UNUSED, float radius UNUSED) |
| static void | empty_oval_init (void) |
| static void | empty_oval (struct EnuCoor_f *wp1 UNUSED, struct EnuCoor_f *wp2 UNUSED, float radius UNUSED) |
| static void | nav_set_altitude (void) |
| void | nav_init (void) |
| Navigation Initialisation. | |
| void | nav_parse_BLOCK (uint8_t *buf) |
| void | nav_parse_MOVE_WP (uint8_t *buf) |
| static void UNUSED | nav_advance_carrot (void) |
| void | nav_run (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. | |
| void | nav_reset_reference (void) |
| Reset the geographic reference to the current GPS fix. | |
| void | nav_reset_alt (void) |
| void | nav_init_stage (void) |
| needs to be implemented by fixedwing and rotorcraft seperately | |
| void | nav_periodic_task (void) |
| Navigation main: call to the code generated from the XML flight plan. | |
| bool | nav_detect_ground (void) |
| bool | nav_is_in_flight (void) |
| void | nav_glide_points (struct EnuCoor_f *start_point, struct EnuCoor_f *end_point) |
| void | nav_home (void) |
| Home mode navigation. | |
| float | get_dist2_to_point (struct EnuCoor_f *p) |
| Returns squared horizontal distance to given point. | |
| float | get_dist2_to_waypoint (uint8_t wp_id) |
| Returns squared horizontal distance to given waypoint. | |
| void | compute_dist2_to_home (void) |
| Computes squared distance to the HOME waypoint potentially sets too_far_from_home. | |
| void | nav_set_heading_rad (float rad) |
| Set nav_heading in radians. | |
| 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) |
| void | nav_register_stage_init (navigation_stage_init nav_stage_init) |
| Register 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) |
Variables | |
| struct RotorcraftNavigation | nav |
| const float | max_dist_from_home = MAX_DIST_FROM_HOME |
| const float | max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME |
| float | flight_altitude |
| Dynamically adjustable, reset to nav_altitude when it is changing. | |
Rotorcraft navigation functions.
Definition in file navigation.c.
| #define CLOSE_TO_WAYPOINT 15.f |
Definition at line 140 of file navigation.c.
Referenced by nav_advance_carrot().
| #define NAV_C |
Definition at line 29 of file navigation.c.
| void compute_dist2_to_home | ( | void | ) |
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition at line 326 of file navigation.c.
References get_dist2_to_waypoint(), max_dist2_from_home, nav, stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.
Referenced by autopilot_static_periodic(), nav_home(), and nav_periodic_task().
|
static |
Definition at line 63 of file navigation.c.
References UNUSED.
Referenced by nav_init().
|
static |
Definition at line 64 of file navigation.c.
References UNUSED.
Referenced by nav_init().
|
static |
Definition at line 61 of file navigation.c.
References UNUSED.
Referenced by nav_init().
|
static |
Definition at line 66 of file navigation.c.
References UNUSED.
Referenced by nav_init().
|
static |
Definition at line 65 of file navigation.c.
Referenced by nav_init().
Definition at line 62 of file navigation.c.
References UNUSED.
Referenced by nav_init().
|
static |
Empty navigation functions set at init.
Definition at line 60 of file navigation.c.
References empty_stage_init().
Referenced by empty_stage_init(), and nav_init().
| float get_dist2_to_point | ( | struct EnuCoor_f * | p | ) |
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().
| float get_dist2_to_waypoint | ( | uint8_t | wp_id | ) |
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().
|
inlinestatic |
Definition at line 143 of file navigation.c.
References CLOSE_TO_WAYPOINT, float_vect2_norm(), nav, NAV_CARROT_DIST, stateGetPositionEnu_f(), UNUSED, VECT2_COPY, VECT2_DIFF, VECT2_SDIV, VECT2_SMUL, VECT2_STRIM, and VECT2_SUM.
Referenced by nav_run().
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().
| bool nav_detect_ground | ( | void | ) |
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().
| void nav_home | ( | void | ) |
Home mode navigation.
Home mode navigation (circle around HOME).
Nominal speed
Definition at line 294 of file navigation.c.
References nav, NAV_HORIZONTAL_MODE_WAYPOINT, nav_run(), NAV_SETPOINT_MODE_POS, NAV_VERTICAL_MODE_ALT, VECT3_COPY, waypoint_get_alt(), and waypoints.
| void nav_init | ( | void | ) |
Navigation Initialisation.
Definition at line 70 of file navigation.c.
References common_flight_plan_init(), DEFAULT_CIRCLE_RADIUS, 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, nav, NAV_CLIMB_VSPEED, NAV_DESCEND_VSPEED, NAV_HORIZONTAL_MODE_WAYPOINT, NAV_SETPOINT_MODE_POS, NAV_VERTICAL_MODE_ALT, VECT3_COPY, waypoints, and waypoints_init().
| void nav_init_stage | ( | void | ) |
needs to be implemented by fixedwing and rotorcraft seperately
Definition at line 247 of file navigation.c.
References nav, and stage_time.
| bool nav_is_in_flight | ( | void | ) |
Definition at line 277 of file navigation.c.
References autopilot_in_flight().
Referenced by follow_me(), and nav_land_run().
| void nav_parse_BLOCK | ( | uint8_t * | buf | ) |
Definition at line 115 of file navigation.c.
References nav_goto_block().
| void nav_parse_MOVE_WP | ( | uint8_t * | buf | ) |
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().
| void nav_periodic_task | ( | void | ) |
Navigation main: call to the code generated from the XML flight plan.
Definition at line 256 of file navigation.c.
References block_time, nav_run(), NAVIGATION_FREQUENCY, and stage_time.
| void nav_register_circle | ( | navigation_circle | nav_circle | ) |
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().
| void nav_register_goto_wp | ( | navigation_goto | nav_goto, |
| navigation_route | nav_route, | ||
| navigation_approaching | nav_approaching ) |
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().
| void nav_register_oval | ( | navigation_oval_init | _nav_oval_init, |
| navigation_oval | nav_oval ) |
Definition at line 404 of file navigation.c.
References _nav_oval_init(), nav, and nav_oval().
Referenced by nav_rotorcraft_init(), and nav_rover_init().
| void nav_register_stage_init | ( | navigation_stage_init | nav_stage_init | ) |
Register 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().
| void nav_reset_alt | ( | void | ) |
Definition at line 241 of file navigation.c.
References INS_RESET_VERTICAL_REF, and waypoints_localize_all().
| void nav_reset_reference | ( | void | ) |
Reset the geographic reference to the current GPS fix.
Definition at line 234 of file navigation.c.
References INS_RESET_REF, and waypoints_localize_all().
| void nav_run | ( | void | ) |
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().
|
inlinestatic |
Definition at line 217 of file navigation.c.
References flight_altitude, nav, and stateGetHmslOrigin_f().
Referenced by nav_run().
| void nav_set_failsafe | ( | void | ) |
Definition at line 380 of file navigation.c.
References AP_MODE_FAILSAFE, and autopilot_set_mode().
| void nav_set_heading_current | ( | void | ) |
Set heading to the current yaw angle.
Definition at line 375 of file navigation.c.
References nav, FloatEulers::psi, and stateGetNedToBodyEulers_f().
| void nav_set_heading_deg | ( | float | deg | ) |
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().
| void nav_set_heading_rad | ( | float | rad | ) |
Set nav_heading in radians.
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().
| void nav_set_heading_towards | ( | float | x, |
| float | y ) |
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().
| void nav_set_heading_towards_target | ( | void | ) |
Set heading in the direction of the target.
Definition at line 369 of file navigation.c.
References nav, and nav_set_heading_towards().
| void nav_set_heading_towards_waypoint | ( | uint8_t | wp | ) |
Set heading in the direction of a waypoint.
Definition at line 363 of file navigation.c.
References nav_set_heading_towards(), WaypointX, and WaypointY.
| float flight_altitude |
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition at line 56 of file navigation.c.
| const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME |
Definition at line 54 of file navigation.c.
Referenced by compute_dist2_to_home().
| const float max_dist_from_home = MAX_DIST_FROM_HOME |
Definition at line 53 of file navigation.c.
Referenced by mission_point_of_lla(), nav_move_waypoint(), and nav_move_waypoint_enu().
| struct RotorcraftNavigation nav |
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().