nav_survey_hybrid.c File Reference

This routine will cover the enitre area of any Polygon defined in the flightplan which is a convex polygon. More...

#include "modules/nav/nav_survey_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
#include "modules/nav/nav_rotorcraft_base.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"

Include dependency graph for nav_survey_hybrid.c:

Go to the source code of this file.

Data Structures
struct	Line
struct	SurveyHybridPrivate

Macros
#define	SURVEY_HYBRID_APPROACHING_TIME   3.f
#define	SURVEY_HYBRID_MAX_POLYGON_SIZE   20
#define	SURVEY_HYBRID_HALF_SWEEP_ENABLED   true
#define	SURVEY_HYBRID_MAX_SWEEP   0
#define	SURVEY_HYBRID_MAX_SWEEP_BACK   0
#define	SURVEY_HYBRID_ENTRY_DISTANCE   (survey_private.sweep_distance / 2.f)
#define	SURVEY_HYBRID_ENTRY_CIRCLE   TRUE
#define	MaxFloat   1000000000
#define	MinFloat   -1000000000
#define	LINE_START_FUNCTION   {}
#define	LINE_STOP_FUNCTION   {}

Enumerations
enum	SurveyStatus { Init , Entry , Sweep , Turn }

Functions
static void	nav_survey_hybrid_setup (float orientation, float sweep, float radius, float height)
	finish preparation of survey based on private structure
static void	TranslateAndRotateFromWorld (struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans)
static void	RotateAndTranslateToWorld (struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans)
	Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY)
static void	FindInterceptOfTwoLines (float *x, float *y, struct Line L1, struct Line L2)
static float	EvaluateLineForX (float y, struct Line L)
static float	CrossProductZ (struct EnuCoor_f *p1_start, struct EnuCoor_f *p1_end, struct EnuCoor_f *p2_start, struct EnuCoor_f *p2_end)
void	nav_survey_hybrid_init (void)
	Init function.
void	nav_survey_hybrid_setup_orientation (uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius, float height)
	Setup polygon survey.
void	nav_survey_hybrid_setup_towards (uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius, float height)
	Setup "dynamic" polygon survey with sweep orientation towards a waypoint.
bool	nav_survey_hybrid_run (void)
	Run polygon hybrid survey.

Variables
struct SurveyHybrid	survey_hybrid
static struct SurveyHybridPrivate	survey_private

Detailed Description

This routine will cover the enitre area of any Polygon defined in the flightplan which is a convex polygon.

Definition in file nav_survey_hybrid.c.

---

Data Structure Documentation

Line

struct Line

Definition at line 77 of file nav_survey_hybrid.c.

Data Fields
float	b
float	m
float	x

SurveyHybridPrivate

struct SurveyHybridPrivate

Definition at line 80 of file nav_survey_hybrid.c.

Collaboration diagram for SurveyHybridPrivate:

Data Fields
struct EnuCoor_f	circle	circle center
bool	circle_turns	turns with circles (or lines between points otherwise)
struct EnuCoor_f	corners[SURVEY_HYBRID_MAX_POLYGON_SIZE]	corners location
float	edge_max_y[SURVEY_HYBRID_MAX_POLYGON_SIZE]	tmp point in rotated frame
float	edge_min_y[SURVEY_HYBRID_MAX_POLYGON_SIZE]	tmp point in rotated frame
struct Line	edges[SURVEY_HYBRID_MAX_POLYGON_SIZE]	polygon edges
struct EnuCoor_f	entry	entry point
struct EnuCoor_f	from_wp	tmp point in rotated frame
float	max_y	tmp value
float	orientation	requested orientation in radians
float	radius	turn radius
struct EnuCoor_f	segment_from	start of current segment
struct EnuCoor_f	segment_to	end of current segment
uint8_t	size	size of the polygon
struct EnuCoor_f	smallest_corner	tmp point in rotated frame
enum SurveyStatus	status	current state
float	sweep	oriented sweep distance
float	sweep_distance	requested sweep distance
struct EnuCoor_f	to_wp	tmp point in rotated frame
bool	valid	setup is valid

Macro Definition Documentation

LINE_START_FUNCTION

#define LINE_START_FUNCTION   {}

Definition at line 118 of file nav_survey_hybrid.c.

LINE_STOP_FUNCTION

#define LINE_STOP_FUNCTION   {}

Definition at line 121 of file nav_survey_hybrid.c.

MaxFloat

#define MaxFloat   1000000000

Definition at line 114 of file nav_survey_hybrid.c.

MinFloat

#define MinFloat   -1000000000

Definition at line 115 of file nav_survey_hybrid.c.

SURVEY_HYBRID_APPROACHING_TIME

#define SURVEY_HYBRID_APPROACHING_TIME   3.f

Definition at line 44 of file nav_survey_hybrid.c.

SURVEY_HYBRID_ENTRY_CIRCLE

#define SURVEY_HYBRID_ENTRY_CIRCLE   TRUE

Definition at line 74 of file nav_survey_hybrid.c.

SURVEY_HYBRID_ENTRY_DISTANCE

#define SURVEY_HYBRID_ENTRY_DISTANCE   (survey_private.sweep_distance / 2.f)

Definition at line 69 of file nav_survey_hybrid.c.

SURVEY_HYBRID_HALF_SWEEP_ENABLED

#define SURVEY_HYBRID_HALF_SWEEP_ENABLED   true

Definition at line 54 of file nav_survey_hybrid.c.

SURVEY_HYBRID_MAX_POLYGON_SIZE

#define SURVEY_HYBRID_MAX_POLYGON_SIZE   20

Definition at line 49 of file nav_survey_hybrid.c.

SURVEY_HYBRID_MAX_SWEEP

#define SURVEY_HYBRID_MAX_SWEEP   0

Definition at line 59 of file nav_survey_hybrid.c.

SURVEY_HYBRID_MAX_SWEEP_BACK

#define SURVEY_HYBRID_MAX_SWEEP_BACK   0

Definition at line 64 of file nav_survey_hybrid.c.

Enumeration Type Documentation

SurveyStatus

enum SurveyStatus

Enumerator
Init
Entry
Sweep
Turn

Definition at line 78 of file nav_survey_hybrid.c.

Function Documentation

CrossProductZ()

float CrossProductZ ( struct EnuCoor_f *  p1_start, struct EnuCoor_f *  p1_end, struct EnuCoor_f *  p2_start, struct EnuCoor_f *  p2_end  )

static

Definition at line 651 of file nav_survey_hybrid.c.

References foo.

Referenced by nav_survey_hybrid_run().

Here is the caller graph for this function:

EvaluateLineForX()

float EvaluateLineForX ( float  y, struct Line  L  )

static

Definition at line 646 of file nav_survey_hybrid.c.

References Line::b, Line::m, and EnuCoor_f::y.

Referenced by nav_survey_hybrid_run(), and nav_survey_hybrid_setup().

Here is the caller graph for this function:

FindInterceptOfTwoLines()

void FindInterceptOfTwoLines ( float *  x, float *  y, struct Line  L1, struct Line  L2  )

static

Definition at line 639 of file nav_survey_hybrid.c.

References foo, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by nav_survey_hybrid_setup().

Here is the caller graph for this function:

nav_survey_hybrid_init()

void nav_survey_hybrid_init

(

void

)

Init function.

Definition at line 184 of file nav_survey_hybrid.c.

References foo, SurveyHybrid::half_sweep_enabled, mission_register(), survey_hybrid, SURVEY_HYBRID_HALF_SWEEP_ENABLED, SURVEY_HYBRID_MAX_SWEEP, SURVEY_HYBRID_MAX_SWEEP_BACK, survey_private, SurveyHybrid::sweep_back_nb, SurveyHybrid::sweep_back_nb_max, SurveyHybrid::sweep_nb, and SurveyHybrid::sweep_nb_max.

Here is the call graph for this function:

nav_survey_hybrid_run()

bool nav_survey_hybrid_run

(

void

)

Run polygon hybrid survey.

Definition at line 399 of file nav_survey_hybrid.c.

References SurveyHybridPrivate::circle, SurveyHybridPrivate::circle_turns, CloseRadAngles, CrossProductZ(), dc_distance_interval, dc_send_command(), DC_SHOOT, dir, SurveyHybridPrivate::edge_max_y, SurveyHybridPrivate::edge_min_y, SurveyHybridPrivate::edges, Entry, SurveyHybridPrivate::entry, EvaluateLineForX(), foo, SurveyHybridPrivate::from_wp, SurveyHybrid::half_sweep_enabled, Init, LINE_START_FUNCTION, LINE_STOP_FUNCTION, SurveyHybridPrivate::max_y, nav, RotorcraftNavigation::nav_approaching, RotorcraftNavigation::nav_circle, RotorcraftNavigation::nav_goto, nav_init_stage(), RotorcraftNavigation::nav_route, NavCircleCount, SurveyHybridPrivate::orientation, SurveyHybridPrivate::radius, RotateAndTranslateToWorld(), SurveyHybridPrivate::segment_from, SurveyHybridPrivate::segment_to, SurveyHybridPrivate::size, SurveyHybridPrivate::smallest_corner, stateGetPositionEnu_f(), SurveyHybridPrivate::status, survey_hybrid, SURVEY_HYBRID_APPROACHING_TIME, SURVEY_HYBRID_ENTRY_CIRCLE, survey_private, Sweep, SurveyHybridPrivate::sweep, SurveyHybrid::sweep_back_nb, SurveyHybrid::sweep_back_nb_max, SurveyHybrid::sweep_nb, SurveyHybrid::sweep_nb_max, SurveyHybridPrivate::to_wp, Turn, SurveyHybridPrivate::valid, VECT3_COPY, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Here is the call graph for this function:

nav_survey_hybrid_setup()

static void nav_survey_hybrid_setup ( float  orientation, float  sweep, float  radius, float  height  )

static

finish preparation of survey based on private structure

Definition at line 202 of file nav_survey_hybrid.c.

References Line::b, SurveyHybridPrivate::circle_turns, SurveyHybridPrivate::corners, SurveyHybridPrivate::edge_max_y, SurveyHybridPrivate::edge_min_y, SurveyHybridPrivate::edges, Entry, SurveyHybridPrivate::entry, EvaluateLineForX(), FindInterceptOfTwoLines(), FLOAT_VECT2_ZERO, foo, SurveyHybridPrivate::from_wp, Init, LINE_STOP_FUNCTION, Line::m, SurveyHybridPrivate::max_y, MaxFloat, NavVerticalAltitudeMode, SurveyHybridPrivate::orientation, SurveyHybridPrivate::radius, SurveyHybridPrivate::size, SurveyHybridPrivate::smallest_corner, SurveyHybridPrivate::status, survey_hybrid, SURVEY_HYBRID_ENTRY_DISTANCE, survey_private, SurveyHybridPrivate::sweep, sweep, SurveyHybrid::sweep_back_nb, SurveyHybridPrivate::sweep_distance, SurveyHybrid::sweep_nb, SurveyHybridPrivate::to_wp, TranslateAndRotateFromWorld(), SurveyHybridPrivate::valid, VECT2_COPY, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by nav_survey_hybrid_setup_orientation().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_survey_hybrid_setup_orientation()

void nav_survey_hybrid_setup_orientation	(	uint8_t	start_wp,
		float	orientation,
		uint8_t	size,
		float	sweep,
		float	radius,
		float	height
	)

Setup polygon survey.

Parameters

start_wp	first waypoint/corner of the polygon
orientation	angle of scan lines in degrees (CCW, east)
size	number of waypoints/corners used to define the polygon
sweep	distance between scan lines
radius	turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius)
height	starting height in meters

Definition at line 365 of file nav_survey_hybrid.c.

References SurveyHybridPrivate::corners, foo, nav_survey_hybrid_setup(), SurveyHybridPrivate::size, SURVEY_HYBRID_MAX_POLYGON_SIZE, survey_private, sweep, SurveyHybridPrivate::valid, and waypoint_get_enu_f().

Referenced by nav_survey_hybrid_setup_towards().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_survey_hybrid_setup_towards()

void nav_survey_hybrid_setup_towards	(	uint8_t	start_wp,
		uint8_t	second_wp,
		uint8_t	size,
		float	sweep,
		float	radius,
		float	height
	)

Setup "dynamic" polygon survey with sweep orientation towards a waypoint.

Computes the sweep orientation angle from the line first-second WP.

Parameters

start_wp	first waypoint/corner of the polygon
second_wp	second waypoint towards which the sweep orientation is computed
size	number of waypoints/corners used to define the polygon
sweep	distance between scan lines, if zero uses Poly_Distance
radius	turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius)
height	starting height in meters

Definition at line 383 of file nav_survey_hybrid.c.

References foo, nav_survey_hybrid_setup_orientation(), survey_private, sweep, SurveyHybridPrivate::valid, waypoint_get_enu_f(), EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

RotateAndTranslateToWorld()

void RotateAndTranslateToWorld ( struct EnuCoor_f *  p, float  Zrot, struct EnuCoor_f *  trans  )

static

Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY)

Definition at line 628 of file nav_survey_hybrid.c.

References foo, p, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by nav_survey_hybrid_run().

Here is the caller graph for this function:

TranslateAndRotateFromWorld()

void TranslateAndRotateFromWorld ( struct EnuCoor_f *  p, float  Zrot, struct EnuCoor_f *  trans  )

static

Definition at line 615 of file nav_survey_hybrid.c.

References foo, p, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by nav_survey_hybrid_setup().

Here is the caller graph for this function:

Variable Documentation

survey_hybrid

struct SurveyHybrid survey_hybrid

Definition at line 103 of file nav_survey_hybrid.c.

Referenced by nav_survey_hybrid_init(), nav_survey_hybrid_run(), and nav_survey_hybrid_setup().

survey_private

struct SurveyHybridPrivate survey_private

static

Definition at line 104 of file nav_survey_hybrid.c.

Referenced by nav_survey_hybrid_init(), nav_survey_hybrid_run(), nav_survey_hybrid_setup(), nav_survey_hybrid_setup_orientation(), and nav_survey_hybrid_setup_towards().