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. More... | |
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 More... | |
void | nav_reset_reference (void) |
Reset the geographic reference to the current GPS fix. More... | |
void | nav_reset_alt (void) |
void | nav_init_stage (void) |
needs to be implemented by fixedwing and rotorcraft seperately More... | |
void | nav_periodic_task (void) |
Navigation main: call to the code generated from the XML flight plan. More... | |
bool | nav_is_in_flight (void) |
void | nav_home (void) |
Home mode navigation. More... | |
float | get_dist2_to_point (struct EnuCoor_f *p) |
Returns squared horizontal distance to given point. More... | |
float | get_dist2_to_waypoint (uint8_t wp_id) |
Returns squared horizontal distance to given waypoint. More... | |
void | compute_dist2_to_home (void) |
Computes squared distance to the HOME waypoint potentially sets too_far_from_home. More... | |
void | nav_set_heading_rad (float rad) |
Set nav_heading in radians. More... | |
void | nav_set_heading_deg (float deg) |
Set nav_heading in degrees. More... | |
void | nav_set_heading_towards (float x, float y) |
Set heading to point towards x,y position in local coordinates. More... | |
void | nav_set_heading_towards_waypoint (uint8_t wp) |
Set heading in the direction of a waypoint. More... | |
void | nav_set_heading_towards_target (void) |
Set heading in the direction of the target. More... | |
void | nav_set_heading_current (void) |
Set heading to the current yaw angle. More... | |
void | nav_set_failsafe (void) |
void | nav_register_goto_wp (nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching) |
Register functions. More... | |
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) |
Variables | |
struct RoverNavigation | 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 | failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE |
Rover navigation functions.
Definition in file navigation.c.
#define NAV_C |
Definition at line 28 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 246 of file navigation.c.
References RotorcraftNavigation::dist2_to_home, get_dist2_to_waypoint(), max_dist2_from_home, nav, stateGetPositionEnu_f(), RotorcraftNavigation::too_far_from_home, EnuCoor_f::x, and EnuCoor_f::y.
float get_dist2_to_point | ( | struct EnuCoor_f * | p | ) |
Returns squared horizontal distance to given point.
Definition at line 227 of file navigation.c.
References p, pos_diff, stateGetPositionEnu_f(), FloatVect2::x, EnuCoor_f::x, FloatVect2::y, and EnuCoor_f::y.
float get_dist2_to_waypoint | ( | uint8_t | wp_id | ) |
Returns squared horizontal distance to given waypoint.
Definition at line 238 of file navigation.c.
References get_dist2_to_point(), and waypoints.
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.
void nav_home | ( | void | ) |
Home mode navigation.
Home mode navigation (circle around HOME)
Nominal speed
Definition at line 215 of file navigation.c.
References nav, NAV_MODE_WAYPOINT, nav_run(), RotorcraftNavigation::target, VECT3_COPY, and waypoints.
void nav_init | ( | void | ) |
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, 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().
void nav_init_stage | ( | void | ) |
needs to be implemented by fixedwing and rotorcraft seperately
Definition at line 189 of file navigation.c.
References nav, stage_time, stateGetPositionEnu_f(), and VECT3_COPY.
bool nav_is_in_flight | ( | void | ) |
Definition at line 209 of file navigation.c.
References autopilot_in_flight().
void nav_parse_BLOCK | ( | uint8_t * | buf | ) |
Definition at line 111 of file navigation.c.
References nav_goto_block().
void nav_parse_MOVE_WP | ( | uint8_t * | buf | ) |
Definition at line 117 of file navigation.c.
References LlaCoor_i::alt, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, State::ned_origin_i, state, 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 196 of file navigation.c.
References block_time, nav_run(), NAVIGATION_FREQUENCY, and stage_time.
void nav_register_circle | ( | nav_rover_circle | nav_circle | ) |
Definition at line 317 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 | ||
) |
Register functions.
Registering functions.
Definition at line 310 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 322 of file navigation.c.
References nav, nav_oval(), RotorcraftNavigation::nav_oval, nav_oval_init(), and RotorcraftNavigation::nav_oval_init.
void nav_reset_alt | ( | void | ) |
Definition at line 183 of file navigation.c.
References ins_reset_altitude_ref(), and waypoints_localize_all().
void nav_reset_reference | ( | void | ) |
Reset the geographic reference to the current GPS fix.
Definition at line 176 of file navigation.c.
References ins_reset_local_origin(), and waypoints_localize_all().
void nav_run | ( | void | ) |
Definition at line 106 of file navigation.c.
References RotorcraftNavigation::carrot, nav, RotorcraftNavigation::target, and VECT2_COPY.
void nav_set_failsafe | ( | void | ) |
Definition at line 299 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 294 of file navigation.c.
References RotorcraftNavigation::heading, nav, FloatEulers::psi, and stateGetNedToBodyEulers_f().
void nav_set_heading_deg | ( | float | deg | ) |
Set nav_heading in degrees.
Definition at line 264 of file navigation.c.
References nav_set_heading_rad().
void nav_set_heading_rad | ( | float | rad | ) |
Set nav_heading in radians.
heading utility functions
Definition at line 257 of file navigation.c.
References RotorcraftNavigation::heading, nav, and NormCourseRad.
void nav_set_heading_towards | ( | float | x, |
float | y | ||
) |
Set heading to point towards x,y position in local coordinates.
Definition at line 270 of file navigation.c.
References RotorcraftNavigation::heading, nav, pos_diff, stateGetPositionEnu_f(), target, VECT2_DIFF, VECT2_NORM2, FloatVect2::x, and FloatVect2::y.
void nav_set_heading_towards_target | ( | void | ) |
Set heading in the direction of the target.
Definition at line 288 of file navigation.c.
References nav, nav_set_heading_towards(), RotorcraftNavigation::target, EnuCoor_f::x, and EnuCoor_f::y.
void nav_set_heading_towards_waypoint | ( | uint8_t | wp | ) |
Set heading in the direction of a waypoint.
Definition at line 282 of file navigation.c.
References nav_set_heading_towards(), WaypointX, and WaypointY.
|
static |
Definition at line 66 of file navigation.c.
References dev, nb_waypoint, waypoints, point::x, and point::y.
Referenced by nav_init().
void set_exception_flag | ( | uint8_t | flag_num | ) |
Definition at line 57 of file navigation.c.
References nav.
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 1 of file navigation.c.
Referenced by nav_oval_init(), NavApproaching(), NavApproachingFrom(), NavCircleWaypoint(), NavGotoWaypoint(), NavSegment(), and Oval().