#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	POLY_OSAM_HALF_SWEEP_ENABLED TRUE

#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
void	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)

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

bool	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

bool	Half_Sweep_Enabled = POLY_OSAM_HALF_SWEEP_ENABLED

Data Structure Documentation

struct Point2D

Definition at line 90 of file nav_launcher.c.

Data Fields
float	x
float	y

struct Line

Definition at line 94 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 91 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run().

#define LINE_STOP_FUNCTION {}

Definition at line 94 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

#define MaxFloat 1000000000

Definition at line 87 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_setup().

#define MaxPolygonSize POLYSURVEY_MAX_POLYGONSIZE

Definition at line 86 of file nav_survey_poly_rotorcraft.c.

#define MinFloat -1000000000

Definition at line 88 of file nav_survey_poly_rotorcraft.c.

#define POLY_OSAM_HALF_SWEEP_ENABLED TRUE

Definition at line 57 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_DEFAULT_DISTANCE 25

Definition at line 42 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_DEFAULT_SIZE 10

Definition at line 38 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_ENTRY_DISTANCE 0

if 0 default to half sweep

Definition at line 47 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 52 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 101 of file nav_survey_poly_rotorcraft.c.

Function Documentation

float EvaluateLineForX	(	float	y,
		struct Line	L
	)

static

Definition at line 524 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 517 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 nav_survey_poly_run ( void )

Run polygon survey.

Definition at line 307 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(), Half_Sweep_Enabled, horizontal_mode, HORIZONTAL_MODE_ROUTE, Init, LINE_START_FUNCTION, LINE_STOP_FUNCTION, MaxY, nav_approaching_from(), nav_init_stage(), nav_route(), navigation_target, PolySurveySweepBackNum, PolySurveySweepNum, RotateAndTranslateToWorld(), SmallestCorner, stateGetPositionEnu_f(), survey_to_i, SurveyEntry, SurveyEntryWP, SurveyFromWP, SurveySize, SurveyTheta, SurveyToWP, Sweep, Turn, VECT3_COPY, waypoints, EnuCoor_f::x, Point2D::x, EnuCoor_f::y, Point2D::y, and EnuCoor_f::z.

Here is the call graph for this function:

void 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 124 of file nav_survey_poly_rotorcraft.c.

References Line::b, CSurveyStatus, dSweep, EdgeMaxY, EdgeMinY, Edges, Entry, EvaluateLineForX(), 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(), waypoints, point::x, EnuCoor_f::x, Point2D::x, point::y, EnuCoor_f::y, and Point2D::y.

Referenced by nav_survey_poly_setup_towards().

Here is the call graph for this function:

Here is the caller graph for this function:

void 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 64 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: