#include "modules/nav/nav_survey_poly_rotorcraft.h"
#include "firmwares/rotorcraft/navigation.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"

Include dependency graph for nav_survey_poly_rotorcraft.c:

Data Structures
struct	Point2D

struct	Line

Macros
#define	POLYSURVEY_DEFAULT_SIZE 10

#define	POLYSURVEY_DEFAULT_DISTANCE 25

#define	POLYSURVEY_ENTRY_DISTANCE 0
	if 0 default to half sweep More...

#define	POLYSURVEY_MAX_POLYGONSIZE 20
	maximum number of polygon corners More...

#define	MaxPolygonSize POLYSURVEY_MAX_POLYGONSIZE

#define	MaxFloat 1000000000

#define	MinFloat -1000000000

#define	LINE_START_FUNCTION {}

#define	LINE_STOP_FUNCTION {}

Enumerations
enum	SurveyStatus { Init, Entry, Sweep, SweepCircle, Init, Entry, Sweep, Turn }
	This routine will cover the enitre area of any Polygon defined in the flightplan which is a convex polygon. More...

Functions
bool_t	nav_survey_poly_setup_towards (uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
	Setup "dynamic" polygon survey with sweep orientation towards a waypoint. More...

static void	TranslateAndRotateFromWorld (struct EnuCoor_f *p, float Zrot, float transX, float transY)

static void	RotateAndTranslateToWorld (struct EnuCoor_f *p, float Zrot, float transX, float transY)
	Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY) More...

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

static float	EvaluateLineForX (float y, struct Line L)

bool_t	nav_survey_poly_setup (uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
	Setup polygon survey. More...

bool_t	nav_survey_poly_run (void)
	Run polygon survey. More...

Variables
uint8_t	Poly_Size = POLYSURVEY_DEFAULT_SIZE

float	Poly_Distance = POLYSURVEY_DEFAULT_DISTANCE

static enum SurveyStatus	CSurveyStatus

static struct Point2D	SmallestCorner

static struct Line	Edges [MaxPolygonSize]

static float	EdgeMaxY [MaxPolygonSize]

static float	EdgeMinY [MaxPolygonSize]

static float	SurveyTheta

static float	dSweep

static struct EnuCoor_f	SurveyToWP

static struct EnuCoor_f	SurveyFromWP

static struct EnuCoor_f	SurveyEntry

static struct EnuCoor_i survey_from_i	survey_to_i

static uint8_t	SurveyEntryWP

static uint8_t	SurveySize

static float	MaxY

uint16_t	PolySurveySweepNum

uint16_t	PolySurveySweepBackNum

float	EntryRadius

Data Structure Documentation

struct Point2D

Definition at line 44 of file nav_line_osam.c.

Data Fields
float	x
float	y

struct Line

Definition at line 87 of file nav_survey_poly_osam.c.

Data Fields
float	b
float	m
float	x

Macro Definition Documentation

#define LINE_START_FUNCTION {}

Definition at line 96 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run().

#define LINE_STOP_FUNCTION {}

Definition at line 99 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

#define MaxFloat 1000000000

Definition at line 92 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_setup().

#define MaxPolygonSize POLYSURVEY_MAX_POLYGONSIZE

Definition at line 91 of file nav_survey_poly_rotorcraft.c.

#define MinFloat -1000000000

Definition at line 93 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_DEFAULT_DISTANCE 25

Definition at line 52 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_DEFAULT_SIZE 10

Definition at line 48 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_ENTRY_DISTANCE 0

if 0 default to half sweep

Definition at line 57 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_setup().

#define POLYSURVEY_MAX_POLYGONSIZE 20

maximum number of polygon corners

Definition at line 62 of file nav_survey_poly_rotorcraft.c.

Enumeration Type Documentation

enum SurveyStatus

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

Enumerator
Init
Entry
Sweep
SweepCircle
Init
Entry
Sweep
Turn

Definition at line 107 of file nav_survey_poly_rotorcraft.c.

Function Documentation

float EvaluateLineForX	(	float	y,
		struct Line	L
	)

static

Definition at line 537 of file nav_survey_poly_rotorcraft.c.

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

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

Here is the caller graph for this function:

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

static

Definition at line 530 of file nav_survey_poly_rotorcraft.c.

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

Referenced by nav_survey_poly_setup().

Here is the caller graph for this function:

bool_t nav_survey_poly_run ( void )

Run polygon survey.

Definition at line 316 of file nav_survey_poly_rotorcraft.c.

References CSurveyStatus, dc_distance_interval, dc_send_command(), DC_SHOOT, dSweep, EdgeMaxY, EdgeMinY, Edges, Entry, ENU_BFP_OF_REAL, EvaluateLineForX(), FALSE, horizontal_mode, HORIZONTAL_MODE_ROUTE, Init, LINE_START_FUNCTION, LINE_STOP_FUNCTION, MaxY, nav_approaching_from(), nav_init_stage(), nav_route(), navigation_target, PolySurveySweepNum, RotateAndTranslateToWorld(), SmallestCorner, stateGetPositionEnu_f(), survey_to_i, SurveyEntry, SurveyEntryWP, SurveyFromWP, SurveySize, SurveyTheta, SurveyToWP, Sweep, TRUE, Turn, VECT3_COPY, waypoints, Point2D::x, EnuCoor_f::x, Point2D::y, EnuCoor_f::y, and EnuCoor_f::z.

Here is the call graph for this function:

bool_t nav_survey_poly_setup	(	uint8_t	FirstWP,
		uint8_t	Size,
		float	Sweep,
		float	Orientation
	)

Setup polygon survey.

Parameters

FirstWP	first waypoint/corner of the polygon
Size	number of waypoints/corners used to define the polygon
Sweep	distance between scan lines
Orientation	angle of scan lines in degrees (CCW, east)

Definition at line 131 of file nav_survey_poly_rotorcraft.c.

References Line::b, CSurveyStatus, dSweep, EdgeMaxY, EdgeMinY, Edges, Entry, EvaluateLineForX(), FALSE, FindInterceptOfTwoLines(), Init, LINE_STOP_FUNCTION, Line::m, MaxFloat, MaxY, nav_set_heading_deg(), NavVerticalAltitudeMode, POLYSURVEY_ENTRY_DISTANCE, PolySurveySweepBackNum, PolySurveySweepNum, SmallestCorner, SurveyEntry, SurveyEntryWP, SurveyFromWP, SurveySize, SurveyTheta, SurveyToWP, TranslateAndRotateFromWorld(), TRUE, waypoints, point::x, Point2D::x, EnuCoor_f::x, point::y, Point2D::y, and EnuCoor_f::y.

Referenced by nav_survey_poly_setup_towards().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t nav_survey_poly_setup_towards	(	uint8_t	FirstWP,
		uint8_t	Size,
		float	Sweep,
		int	SecondWP
	)

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

Computes the sweep orientation angle from the line FirstWP-SecondWP. If you pass zero for Size and/or Sweep it will use the global Poly_Size and Poly_Sweep variables respectively (which can be changed via telemetry/settings).

Parameters

FirstWP	first waypoint/corner of the polygon
Size	number of waypoints/corners used to define the polygon, if zero uses Poly_Size
Sweep	distance between scan lines, if zero uses Poly_Distance
SecondWP	second waypoint towards which the sweep orientation is computed

Definition at line 69 of file nav_survey_poly_rotorcraft.c.

References nav_survey_poly_setup(), Poly_Distance, Poly_Size, waypoints, point::x, and point::y.

Here is the call graph for this function: