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

#define	POLY_OSAM_MIN_RADIUS 30
	if 0 never check for min radius

#define	POLY_OSAM_FIRST_SWEEP_DISTANCE 0
	if 0 default to half sweep

#define	POLY_OSAM_POLYGONSIZE 10
	maximum number of polygon corners

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

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)

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.

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

bool	nav_survey_poly_osam_run (void)
	Run polygon survey.

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 77 of file nav_survey_hybrid.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

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 foo, 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, foo, 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(), foo, 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(), foo, 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 foo, nav_survey_poly_osam_setup(), Poly_Size, Poly_Sweep, Sweep, waypoints, point::x, and point::y.

Here is the call graph for this function: