![]() |
Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Rover navigation functions. More...
#include "firmwares/rover/navigation.h"
#include "pprz_debug.h"
#include "modules/gps/gps.h"
#include "modules/ins/ins.h"
#include "state.h"
#include "autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
#include "math/pprz_algebra_int.h"
#include "modules/datalink/downlink.h"
#include "pprzlink/messages.h"
#include "mcu_periph/uart.h"
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Macros | |
#define | NAV_C |
Functions | |
void | set_exception_flag (uint8_t flag_num) |
static void | send_wp_moved (struct transport_tx *trans, struct link_device *dev) |
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) |
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 | |
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_is_in_flight (void) |
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 (nav_rover_stage_init nav_stage_init) |
Register 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) |
Rover navigation functions.
Definition in file navigation.c.
#define NAV_C |
Definition at line 28 of file navigation.c.
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition at line 242 of file navigation.c.
References RotorcraftNavigation::dist2_to_home, foo, get_dist2_to_waypoint(), max_dist2_from_home, nav, stateGetPositionEnu_f(), RotorcraftNavigation::too_far_from_home, EnuCoor_f::x, and EnuCoor_f::y.
Returns squared horizontal distance to given point.
Definition at line 223 of file navigation.c.
References p, pos_diff, stateGetPositionEnu_f(), FloatVect2::x, EnuCoor_f::x, FloatVect2::y, and EnuCoor_f::y.
Returns squared horizontal distance to given waypoint.
Definition at line 234 of file navigation.c.
References get_dist2_to_point(), and waypoints.
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, pprz_autopilot::flight_time, float_vect2_norm(), foo, INT_VECT3_ZERO, stateGetPositionEnu_f(), and VECT2_DIFF.
Home mode navigation.
Home mode navigation (circle around HOME)
Nominal speed
Definition at line 213 of file navigation.c.
References foo, nav, NAV_MODE_WAYPOINT, nav_run(), RotorcraftNavigation::target, VECT3_COPY, and waypoints.
Navigation Initialisation.
Definition at line 79 of file navigation.c.
References RotorcraftNavigation::carrot, common_flight_plan_init(), DEFAULT_CIRCLE_RADIUS, DefaultPeriodic, RotorcraftNavigation::dist2_to_home, foo, RotorcraftNavigation::heading, nav, NAV_MODE_WAYPOINT, RotorcraftNavigation::radius, register_periodic_telemetry(), send_wp_moved(), RotorcraftNavigation::speed, RotorcraftNavigation::target, RotorcraftNavigation::too_far_from_home, VECT3_COPY, waypoints, and waypoints_init().
needs to be implemented by fixedwing and rotorcraft seperately
Definition at line 187 of file navigation.c.
References nav, RotorcraftNavigation::nav_stage_init, stage_time, stateGetPositionEnu_f(), and VECT3_COPY.
Definition at line 207 of file navigation.c.
References autopilot_in_flight().
Definition at line 111 of file navigation.c.
References foo, and nav_goto_block().
Definition at line 117 of file navigation.c.
References LlaCoor_i::alt, foo, LlaCoor_i::lat, LlaCoor_i::lon, stateGetHmslOrigin_i(), stateGetLlaOrigin_i(), stateIsLocalCoordinateValid(), and waypoint_move_lla().
Navigation main: call to the code generated from the XML flight plan.
Definition at line 196 of file navigation.c.
References block_time, foo, nav_run(), NAVIGATION_FREQUENCY, and stage_time.
void nav_register_circle | ( | nav_rover_circle | nav_circle | ) |
Definition at line 318 of file navigation.c.
References nav, RotorcraftNavigation::nav_circle, and nav_circle().
void nav_register_goto_wp | ( | nav_rover_goto | nav_goto, |
nav_rover_route | nav_route, | ||
nav_rover_approaching | nav_approaching | ||
) |
Definition at line 311 of file navigation.c.
References nav, RotorcraftNavigation::nav_approaching, nav_approaching(), RotorcraftNavigation::nav_goto, nav_goto(), RotorcraftNavigation::nav_route, and nav_route().
void nav_register_oval | ( | nav_rover_oval_init | nav_oval_init, |
nav_rover_oval | nav_oval | ||
) |
Definition at line 323 of file navigation.c.
References nav, nav_oval(), RotorcraftNavigation::nav_oval, nav_oval_init(), and RotorcraftNavigation::nav_oval_init.
void nav_register_stage_init | ( | nav_rover_stage_init | nav_stage_init | ) |
Register functions.
Registering functions.
Definition at line 306 of file navigation.c.
References nav, RotorcraftNavigation::nav_stage_init, and nav_stage_init().
Definition at line 181 of file navigation.c.
References foo, INS_RESET_VERTICAL_REF, and waypoints_localize_all().
Reset the geographic reference to the current GPS fix.
Definition at line 174 of file navigation.c.
References foo, INS_RESET_REF, and waypoints_localize_all().
Definition at line 106 of file navigation.c.
References RotorcraftNavigation::carrot, nav, RotorcraftNavigation::target, and VECT2_COPY.
Definition at line 295 of file navigation.c.
References AP_MODE_FAILSAFE, and autopilot_set_mode().
Set heading to the current yaw angle.
Definition at line 290 of file navigation.c.
References RotorcraftNavigation::heading, nav, FloatEulers::psi, and stateGetNedToBodyEulers_f().
Set nav_heading in degrees.
Definition at line 260 of file navigation.c.
References foo, and nav_set_heading_rad().
Set nav_heading in radians.
heading utility functions
Definition at line 253 of file navigation.c.
References RotorcraftNavigation::heading, nav, and NormCourseRad.
Set heading to point towards x,y position in local coordinates.
Definition at line 266 of file navigation.c.
References foo, RotorcraftNavigation::heading, nav, pos_diff, stateGetPositionEnu_f(), target, VECT2_DIFF, VECT2_NORM2, FloatVect2::x, and FloatVect2::y.
Set heading in the direction of the target.
Definition at line 284 of file navigation.c.
References nav, nav_set_heading_towards(), RotorcraftNavigation::target, EnuCoor_f::x, and EnuCoor_f::y.
Set heading in the direction of a waypoint.
Definition at line 278 of file navigation.c.
References nav_set_heading_towards(), WaypointX, and WaypointY.
|
static |
Definition at line 66 of file navigation.c.
References dev, foo, nb_waypoint, waypoints, point::x, and point::y.
Referenced by nav_init().
Definition at line 57 of file navigation.c.
float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE |
Definition at line 54 of file navigation.c.
const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME |
Definition at line 53 of file navigation.c.
const float max_dist_from_home = MAX_DIST_FROM_HOME |
Definition at line 52 of file navigation.c.
struct RoverNavigation nav |
Definition at line 50 of file navigation.c.
Referenced by nav_oval_init(), NavApproaching(), NavApproachingFrom(), NavCircleWaypoint(), NavGotoWaypoint(), NavSegment(), and Oval().