#include "modules/nav/nav_survey_poly_osam.h"
#include "firmwares/fixedwing/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"

Include dependency graph for nav_survey_poly_osam.c:

Data Structures
struct	Point2D

struct	Line

Macros
#define	POLY_OSAM_DEFAULT_SIZE 5

#define	POLY_OSAM_DEFAULT_SWEEP 100

#define	POLY_OSAM_ENTRY_RADIUS 0
	Default entry radius, if 0 default to half sweep. More...

#define	POLY_OSAM_MIN_RADIUS 30
	if 0 never check for min radius More...

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

#define	POLY_OSAM_POLYGONSIZE 10
	maximum number of polygon corners More...

#define	POLY_OSAM_USE_FULL_CIRCLE TRUE

#define	POLY_OSAM_HALF_SWEEP_ENABLED TRUE

#define	PolygonSize POLY_OSAM_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_osam_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 Point2D *p, float Zrot, float transX, float transY)

static void	RotateAndTranslateToWorld (struct Point2D *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_osam_ResetSweepNumber (bool rst)
	Reset sweep number. More...

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

bool	nav_survey_poly_osam_run (void)
	Run polygon survey. More...

Variables
uint8_t	Poly_Size = POLY_OSAM_DEFAULT_SIZE

float	Poly_Sweep = POLY_OSAM_DEFAULT_SWEEP

bool	use_full_circle = POLY_OSAM_USE_FULL_CIRCLE

bool	Half_Sweep_Enabled = POLY_OSAM_HALF_SWEEP_ENABLED

bool	Reset_Sweep = FALSE

static enum SurveyStatus	CSurveyStatus

static struct Point2D	SmallestCorner

static struct Line	Edges [PolygonSize]

static float	EdgeMaxY [PolygonSize]

static float	EdgeMinY [PolygonSize]

static float	SurveyTheta

static float	dSweep

static float	SurveyRadius

static struct Point2D	SurveyToWP

static struct Point2D	SurveyFromWP

static struct Point2D	SurveyCircle

static uint8_t	SurveyEntryWP

static uint8_t	SurveySize

static float	SurveyCircleQdr

static float	MaxY

uint16_t	PolySurveySweepNum

uint16_t	PolySurveySweepBackNum

float	EntryRadius

Data Structure Documentation

◆ Point2D

struct Point2D

Definition at line 90 of file nav_launcher.c.

Data Fields
float	x
float	y

◆ Line

struct Line

Definition at line 94 of file nav_survey_poly_osam.c.

Data Fields
float	b
float	m
float	x

Macro Definition Documentation

◆ LINE_START_FUNCTION

#define LINE_START_FUNCTION {}

Definition at line 114 of file nav_survey_poly_osam.c.

◆ LINE_STOP_FUNCTION

#define LINE_STOP_FUNCTION {}

Definition at line 117 of file nav_survey_poly_osam.c.

◆ MaxFloat

#define MaxFloat 1000000000

Definition at line 110 of file nav_survey_poly_osam.c.

◆ MinFloat

#define MinFloat -1000000000

Definition at line 111 of file nav_survey_poly_osam.c.

◆ POLY_OSAM_DEFAULT_SIZE

#define POLY_OSAM_DEFAULT_SIZE 5

Definition at line 39 of file nav_survey_poly_osam.c.

◆ POLY_OSAM_DEFAULT_SWEEP

#define POLY_OSAM_DEFAULT_SWEEP 100

Definition at line 43 of file nav_survey_poly_osam.c.

◆ POLY_OSAM_ENTRY_RADIUS

#define POLY_OSAM_ENTRY_RADIUS 0

Default entry radius, if 0 default to half sweep.

Definition at line 48 of file nav_survey_poly_osam.c.

◆ POLY_OSAM_FIRST_SWEEP_DISTANCE

#define POLY_OSAM_FIRST_SWEEP_DISTANCE 0

if 0 default to half sweep

Definition at line 58 of file nav_survey_poly_osam.c.

◆ POLY_OSAM_HALF_SWEEP_ENABLED

#define POLY_OSAM_HALF_SWEEP_ENABLED TRUE

Definition at line 72 of file nav_survey_poly_osam.c.

◆ POLY_OSAM_MIN_RADIUS

#define POLY_OSAM_MIN_RADIUS 30

if 0 never check for min radius

Definition at line 53 of file nav_survey_poly_osam.c.

◆ POLY_OSAM_POLYGONSIZE

#define POLY_OSAM_POLYGONSIZE 10

maximum number of polygon corners

Definition at line 63 of file nav_survey_poly_osam.c.

◆ POLY_OSAM_USE_FULL_CIRCLE

#define POLY_OSAM_USE_FULL_CIRCLE TRUE

Definition at line 67 of file nav_survey_poly_osam.c.

◆ PolygonSize

#define PolygonSize POLY_OSAM_POLYGONSIZE

Definition at line 109 of file nav_survey_poly_osam.c.

Enumeration Type Documentation

◆ SurveyStatus

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 125 of file nav_survey_poly_osam.c.

Function Documentation

◆ EvaluateLineForX()

float EvaluateLineForX	(	float	y,
		struct Line	L
	)

static

Definition at line 615 of file nav_survey_poly_osam.c.

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

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_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 608 of file nav_survey_poly_osam.c.

References Line::b, Line::m, Point2D::x, and Point2D::y.

Referenced by nav_survey_poly_osam_setup().

Here is the caller graph for this function:

◆ nav_survey_poly_osam_ResetSweepNumber()

void nav_survey_poly_osam_ResetSweepNumber ( bool rst )

Reset sweep number.

Definition at line 101 of file nav_survey_poly_osam.c.

References FALSE, PolySurveySweepBackNum, and Reset_Sweep.

◆ nav_survey_poly_osam_run()

bool nav_survey_poly_osam_run ( void )

Run polygon survey.

Definition at line 349 of file nav_survey_poly_osam.c.

References CSurveyStatus, dc_autoshoot, DC_AUTOSHOOT_STOP, dSweep, EdgeMaxY, EdgeMinY, Edges, Entry, EvaluateLineForX(), Half_Sweep_Enabled, Init, LINE_START_FUNCTION, LINE_STOP_FUNCTION, MaxY, nav_approaching_xy(), nav_circle_XY(), nav_init_stage(), nav_route_xy(), nav_survey_shift, NavCircleCount, NavCircleCountNoRewind, NavQdrCloseTo, NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, POLY_OSAM_MIN_RADIUS, Poly_Sweep, PolySurveySweepBackNum, PolySurveySweepNum, RotateAndTranslateToWorld(), SmallestCorner, stateGetPositionEnu_f(), stateGetPositionUtm_f(), SurveyCircle, SurveyCircleQdr, SurveyEntryWP, SurveyFromWP, SurveyRadius, SurveySize, SurveyTheta, SurveyToWP, Sweep, SweepCircle, TranslateAndRotateFromWorld(), use_full_circle, waypoints, EnuCoor_f::x, Point2D::x, EnuCoor_f::y, and Point2D::y.

Here is the call graph for this function:

◆ nav_survey_poly_osam_setup()

void nav_survey_poly_osam_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 146 of file nav_survey_poly_osam.c.

References Line::b, CSurveyStatus, dSweep, EdgeMaxY, EdgeMinY, Edges, Entry, EntryRadius, EvaluateLineForX(), FindInterceptOfTwoLines(), Init, LINE_STOP_FUNCTION, Line::m, MaxFloat, MaxY, POLY_OSAM_ENTRY_RADIUS, POLY_OSAM_FIRST_SWEEP_DISTANCE, Poly_Sweep, PolygonSize, PolySurveySweepBackNum, PolySurveySweepNum, SmallestCorner, SurveyCircle, SurveyCircleQdr, SurveyEntryWP, SurveyFromWP, SurveyRadius, SurveySize, SurveyTheta, SurveyToWP, TranslateAndRotateFromWorld(), waypoints, point::x, Point2D::x, point::y, and Point2D::y.

Referenced by nav_survey_poly_osam_setup_towards().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_survey_poly_osam_setup_towards()

void nav_survey_poly_osam_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_Sweep
SecondWP	second waypoint towards which the sweep orientation is computed

Definition at line 81 of file nav_survey_poly_osam.c.

References f, nav_survey_poly_osam_setup(), Poly_Size, Poly_Sweep, Sweep, waypoints, point::x, and point::y.

Here is the call graph for this function: