Fixedwing functions to compute navigation. More...

#include <math.h>
#include "firmwares/fixedwing/nav.h"
#include "subsystems/gps.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/autopilot.h"
#include "inter_mcu.h"
#include "subsystems/navigation/traffic_info.h"
#include "generated/flight_plan.h"
#include "subsystems/navigation/common_nav.c"

Include dependency graph for nav.c:

Go to the source code of this file.

Macros
#define	NAV_C

#define	RCLost() bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)

#define	NAV_GLIDE_PITCH_TRIM 0.

#define	PowerVoltage() (vsupply/10.)

#define	RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)

#define	MIN_DX ((int16_t)(MAX_PPRZ * 0.05))

#define	NavGlide(_last_wp, _wp)

#define	MAX_DIST_CARROT 250.

#define	MIN_HEIGHT_CARROT 50.

#define	MAX_HEIGHT_CARROT 150.

#define	Goto3D(radius)

#define	NavFollow(_ac_id, _distance, _height) nav_follow(_ac_id, _distance, _height);

#define	LINE_START_FUNCTION {}

#define	LINE_STOP_FUNCTION {}

#define	FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS

Enumerations
enum	eight_status { R1T, RT2, C2, R2T, RT1, C1 }

Functions
void	nav_init_stage (void)
	needs to be implemented by fixedwing and rotorcraft seperately More...

void	nav_circle_XY (float x, float y, float radius)
	Navigates around (x, y). More...

static void	nav_follow (uint8_t _ac_id, float _distance, float _height)

static bool_t	nav_compute_baseleg (uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)

static bool_t	nav_compute_final_from_glide (uint8_t wp_af, uint8_t wp_td, float glide)

static bool_t	compute_TOD (uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)

bool_t	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	fly_to_xy (float x, float y)
	Computes desired_x, desired_y and desired_course. More...

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...

static void	nav_set_altitude (void)

void	nav_home (void)
	Home mode navigation (circle around HOME) More...

void	nav_periodic_task (void)
	Navigation main: call to the code generated from the XML flight plan. More...

void	nav_init (void)
	Periodic telemetry. More...

void	nav_without_gps (void)
	Failsafe navigation without position estimation. More...

void	nav_eight_init (void)

void	nav_eight (uint8_t target, uint8_t c1, float radius)
	Navigation along a figure 8. More...

void	nav_oval_init (void)

void	nav_oval (uint8_t p1, uint8_t p2, float radius)

Variables
enum oval_status	oval_status

float	last_x

float	last_y

static uint8_t	last_wp
	Index of last waypoint. More...

float	rc_pitch

float	carrot_x

float	carrot_y

float	nav_circle_radians
	Status on the current circle. More...

float	nav_circle_radians_no_rewind

float	nav_circle_trigo_qdr

float	nav_radius

float	nav_course

float	nav_climb

float	nav_shift

static float	nav_leg_progress
	Status on the current leg (percentage, 0. More...

static float	nav_carrot_leg_progress

static float	nav_leg_length
	length of the current leg (m) More...

bool_t	nav_in_circle = FALSE

bool_t	nav_in_segment = FALSE

float	nav_circle_x

float	nav_circle_y

float	nav_circle_radius

float	nav_segment_x_1

float	nav_segment_y_1

float	nav_segment_x_2

float	nav_segment_y_2

uint8_t	horizontal_mode

float	circle_bank = 0

float	flight_altitude
	Dynamically adjustable, reset to nav_altitude when it is changing. More...

float	nav_glide_pitch_trim

float	nav_ground_speed_setpoint

float	nav_ground_speed_pgain

float	nav_survey_shift

float	nav_survey_west

float	nav_survey_east

float	nav_survey_north

float	nav_survey_south

bool_t	nav_survey_active

int	nav_mode

static unit_t	unit

static float	baseleg_out_qdr

float	nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT

float	desired_x

float	desired_y

pprz_t	nav_throttle_setpoint

float	nav_pitch
	with INT32_ANGLE_FRAC More...

float	fp_pitch

float	fp_throttle

float	fp_climb

static enum eight_status	eight_status

uint8_t	nav_oval_count
	Navigation along a figure O. More...

Detailed Description

Fixedwing functions to compute navigation.

Definition in file nav.c.

Macro Definition Documentation

#define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS

Definition at line 420 of file nav.c.

Referenced by nav_home().

#define Goto3D ( radius )

Value:

{                                                \
    if (pprz_mode == PPRZ_MODE_AUTO2) {                                 \
      int16_t yaw = fbw_state->channels[RADIO_YAW];                     \
      if (yaw > MIN_DX || yaw < -MIN_DX) {                              \
        carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.);                        \
        carrot_x = Min(carrot_x, MAX_DIST_CARROT);                      \
        carrot_x = Max(carrot_x, -MAX_DIST_CARROT);                     \
      }                                                                 \
      int16_t pitch = fbw_state->channels[RADIO_PITCH];                 \
      if (pitch > MIN_DX || pitch < -MIN_DX) {                          \
        carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.);                      \
        carrot_y = Min(carrot_y, MAX_DIST_CARROT);                      \
        carrot_y = Max(carrot_y, -MAX_DIST_CARROT);                     \
      }                                                                 \
      v_ctl_mode = V_CTL_MODE_AUTO_ALT;                                 \
      int16_t roll =  fbw_state->channels[RADIO_ROLL];                  \
      if (roll > MIN_DX || roll < -MIN_DX) {                            \
        nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0);                   \
        nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \
        nav_altitude = Min(nav_altitude, MAX_HEIGHT_CARROT+ground_alt); \
      }                                                                 \
    }                                                                   \
    nav_circle_XY(carrot_x, carrot_y, radius);                          \
  }

Definition at line 174 of file nav.c.

#define LINE_START_FUNCTION {}

Definition at line 292 of file nav.c.

Referenced by nav_oval().

#define LINE_STOP_FUNCTION {}

Definition at line 295 of file nav.c.

Referenced by nav_oval().

#define MAX_DIST_CARROT 250.

Definition at line 170 of file nav.c.

#define MAX_HEIGHT_CARROT 150.

Definition at line 172 of file nav.c.

#define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))

Definition at line 106 of file nav.c.

#define MIN_HEIGHT_CARROT 50.

Definition at line 171 of file nav.c.

#define NAV_C

Definition at line 28 of file nav.c.

#define NAV_GLIDE_PITCH_TRIM 0.

Definition at line 77 of file nav.c.

Referenced by nav_init().

#define NavFollow	(	_ac_id,
		_distance,
		_height
	)	nav_follow(_ac_id, _distance, _height);

Definition at line 200 of file nav.c.

#define NavGlide	(	_last_wp,
		_wp
	)

Value:

{                                       \
    float start_alt = waypoints[_last_wp].a;                            \
    float diff_alt = waypoints[_wp].a - start_alt;                      \
    float alt = start_alt + nav_leg_progress * diff_alt;                \
    float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length; \
    NavVerticalAltitudeMode(alt, pre_climb);                            \
  }

Definition at line 159 of file nav.c.

#define PowerVoltage ( ) (vsupply/10.)

Definition at line 103 of file nav.c.

#define RCLost ( ) bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)

Definition at line 39 of file nav.c.

#define RcRoll ( travel ) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)

Definition at line 104 of file nav.c.

Enumeration Type Documentation

enum eight_status

Enumerator
R1T
RT2
C2
R2T
RT1
C1

Definition at line 585 of file nav.c.

Function Documentation

static bool_t compute_TOD	(	uint8_t	_af,
		uint8_t	_td,
		uint8_t	_tod,
		float	glide_airspeed,
		float	glide_vspeed
	)

inlinestatic

Definition at line 273 of file nav.c.

References FALSE, stateGetHorizontalWindspeed_f(), WaypointAlt, WaypointX, WaypointY, FloatVect2::x, and FloatVect2::y.

Here is the call graph for this function:

void fly_to_xy	(	float	x,
		float	y
	)

Computes desired_x, desired_y and desired_course.

Definition at line 367 of file nav.c.

References CARROT, 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_COURSE, stateGetHorizontalSpeedDir_f(), stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.

Referenced by formation_flight(), mission_nav_wp(), nav_circle_XY(), nav_follow(), nav_route_xy(), and potential_task().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t 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.

Returns: true if the position (x, y) is reached

distance to waypoint in x

distance to waypoint in y

Definition at line 335 of file nav.c.

References dist2_to_wp, Max, stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), TRUE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by mission_nav_path(), mission_nav_segment(), mission_nav_wp(), nav_eight(), nav_line_osam_run(), nav_oval(), nav_spiral_run(), nav_survey_disc_run(), nav_survey_poly_osam_run(), nav_survey_polygon_run(), 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_circle_XY	(	float	x,
		float	y,
		float	radius
	)

Navigates around (x, y).

Angle from center to mobile.

Clockwise iff radius > 0

Computes a prebank. Go straight if inside or outside the circle

Definition at line 110 of file nav.c.

References CARROT, DistanceSquare, fly_to_xy(), HORIZONTAL_MODE_CIRCLE, Max, Min, nav_circle_trigo_qdr, NAV_GRAVITY, NAV_MODE_COURSE, nav_shift, radius, Square, stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), TRUE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by mission_nav_circle(), nav_eight(), nav_flower_run(), nav_line_border_run(), nav_line_osam_run(), nav_line_run(), nav_oval(), nav_spiral_run(), nav_survey_disc_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:

static bool_t nav_compute_baseleg	(	uint8_t	wp_af,
		uint8_t	wp_td,
		uint8_t	wp_baseleg,
		float	radius
	)

inlinestatic

Definition at line 227 of file nav.c.

References point::a, FALSE, nav_radius, radius, waypoints, point::x, and point::y.

static bool_t nav_compute_final_from_glide	(	uint8_t	wp_af,
		uint8_t	wp_td,
		float	glide
	)

inlinestatic

Definition at line 250 of file nav.c.

References point::a, FALSE, waypoints, point::x, and point::y.

void nav_eight	(	uint8_t	target,
		uint8_t	c1,
		float	radius
	)

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 601 of file nav.c.

References point::a, c1, C1, C2, CARROT, InitStage, Max, 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_eight_init ( void )

Definition at line 588 of file nav.c.

References C1.

static void nav_follow	(	uint8_t	_ac_id,
		float	_distance,
		float	_height
	)

inlinestatic

Definition at line 300 of file nav.c.

References ac_info_::alt, ac_info_::course, ac_info_::east, fly_to_xy(), get_ac_info(), ground_alt, ac_info_::gspeed, Max, NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, ac_info_::north, stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

void nav_home ( void )

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 434 of file nav.c.

Referenced by autopilot_periodic(), and navigation_task().

Here is the caller graph for this function:

void nav_init ( void )

Periodic telemetry.

Navigation Initialisation

Definition at line 530 of file nav.c.

Referenced by autopilot_init(), and init_ap().

Here is the caller graph for this function:

void nav_init_stage ( void )

needs to be implemented by fixedwing and rotorcraft seperately

Definition at line 91 of file nav.c.

Referenced by nav_anemotaxis(), nav_bungee_takeoff_run(), nav_flower_run(), nav_line_border_run(), nav_line_osam_run(), nav_line_run(), nav_survey_disc_run(), nav_survey_poly_osam_run(), nav_survey_poly_run(), nav_survey_polygon_run(), nav_survey_zamboni_run(), and nav_vertical_raster_run().

Here is the caller graph for this function:

void nav_oval	(	uint8_t	p1,
		uint8_t	p2,
		float	radius
	)

Definition at line 730 of file nav.c.

References point::a, CARROT, InitStage, LINE_START_FUNCTION, LINE_STOP_FUNCTION, nav_approaching_xy(), nav_circle_XY(), nav_route_xy(), NavQdrCloseTo, OC1, OC2, OR12, OR21, radius, waypoints, point::x, and point::y.

Here is the call graph for this function:

void nav_oval_init ( void )

Definition at line 724 of file nav.c.

References OC2.

void nav_periodic_task ( void )

Navigation main: call to the code generated from the XML flight plan.

Definition at line 450 of file nav.c.

Referenced by autopilot_periodic(), and navigation_task().

Here is the caller graph for this function:

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 393 of file nav.c.

References CARROT, fly_to_xy(), HORIZONTAL_MODE_ROUTE, Max, stateGetPositionEnu_f(), TRUE, 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_line_osam_run(), nav_oval(), nav_points(), nav_spiral_run(), nav_survey_disc_run(), nav_survey_losange_carto(), 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:

static void nav_set_altitude ( void )

static

Definition at line 423 of file nav.c.

References flight_altitude, nav_altitude, and v_ctl_altitude_setpoint.

Referenced by nav_home(), and nav_periodic_task().

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 565 of file nav.c.

References h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_ROLL, MAX_PPRZ, TRIM_UPPRZ, v_ctl_mode, and V_CTL_MODE_AUTO_THROTTLE.

Referenced by navigation_task().

Here is the caller graph for this function:

Variable Documentation

float baseleg_out_qdr

static

Definition at line 226 of file nav.c.

float carrot_x

Definition at line 49 of file nav.c.

float carrot_y

Definition at line 49 of file nav.c.

float circle_bank = 0

Definition at line 70 of file nav.c.

float desired_x

Definition at line 318 of file nav.c.

Referenced by cam_segment_periodic(), formation_flight(), and potential_task().

float desired_y

Definition at line 318 of file nav.c.

Referenced by cam_segment_periodic(), formation_flight(), and potential_task().

enum eight_status eight_status

static

Definition at line 587 of file nav.c.

float flight_altitude

Dynamically adjustable, reset to nav_altitude when it is changing.

Definition at line 73 of file nav.c.

Referenced by formation_flight(), and nav_set_altitude().

float fp_climb

Definition at line 323 of file nav.c.

float fp_pitch

Definition at line 321 of file nav.c.

float fp_throttle

Definition at line 322 of file nav.c.

uint8_t horizontal_mode

Definition at line 69 of file nav.c.

Referenced by guidance_h_run(), mission_nav_circle(), mission_nav_path(), mission_nav_segment(), mission_nav_wp(), nav_survey_poly_run(), nav_survey_rectangle_rotorcraft_run(), and nav_survey_rectangle_rotorcraft_setup().

uint8_t last_wp

static

Index of last waypoint.

Used only in "go" stage in "route" horiz mode

Definition at line 46 of file nav.c.

float last_x

Definition at line 43 of file nav.c.

Referenced by nav_init_stage(), and nav_survey_polygon_run().

float last_y

Definition at line 43 of file nav.c.

Referenced by nav_init_stage(), and nav_survey_polygon_run().

float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT

Definition at line 317 of file nav.c.

Referenced by formation_flight(), nav_set_altitude(), start_formation(), stop_formation(), and tcas_periodic_task_4Hz().

float nav_carrot_leg_progress

static

Definition at line 60 of file nav.c.

float nav_circle_radians

Status on the current circle.

Definition at line 52 of file nav.c.

float nav_circle_radians_no_rewind

Definition at line 53 of file nav.c.

float nav_circle_radius

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_circle_trigo_qdr

Definition at line 54 of file nav.c.

Referenced by nav_circle_XY().

float nav_circle_x

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_circle_y

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_climb

Definition at line 55 of file nav.c.

Referenced by guidance_v_run().

float nav_course

Definition at line 55 of file nav.c.

float nav_glide_pitch_trim

Definition at line 75 of file nav.c.

float nav_ground_speed_pgain

Definition at line 82 of file nav.c.

float nav_ground_speed_setpoint

Definition at line 82 of file nav.c.

bool_t nav_in_circle = FALSE

Definition at line 65 of file nav.c.

Referenced by nav_survey_rectangle().

bool_t nav_in_segment = FALSE

Definition at line 66 of file nav.c.

Referenced by nav_survey_rectangle().

float nav_leg_length

static

length of the current leg (m)

Definition at line 63 of file nav.c.

float nav_leg_progress

static

Status on the current leg (percentage, 0.

< < 1.) in route mode

Definition at line 59 of file nav.c.

int nav_mode

Definition at line 89 of file nav.c.

uint8_t nav_oval_count

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 722 of file nav.c.

float nav_pitch

with INT32_ANGLE_FRAC

Definition at line 320 of file nav.c.

Referenced by attitude_loop(), guidance_h_run(), v_ctl_climb_auto_throttle_loop(), v_ctl_climb_loop(), and v_ctl_set_pitch().

float nav_radius

Definition at line 55 of file nav.c.

Referenced by nav_compute_baseleg(), and nav_flower_run().

float nav_segment_x_1

Definition at line 68 of file nav.c.

Referenced by flight_benchmark_periodic().