Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav_survey_poly_osam.c File Reference
#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:

Go to the source code of this file.

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.

Referenced by nav_survey_poly_osam_setup().

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

Referenced by nav_survey_poly_osam_setup().

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

Referenced by nav_survey_poly_osam_run().

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

Referenced by nav_survey_poly_osam_setup().

Enumeration Type Documentation

◆ 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 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()

◆ 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
FirstWPfirst waypoint/corner of the polygon
Sizenumber of waypoints/corners used to define the polygon
Sweepdistance between scan lines
Orientationangle of scan lines in degrees (CCW, east)

Definition at line 146 of file nav_survey_poly_osam.c.

References CSurveyStatus, dSweep, EdgeMaxY, EdgeMinY, Edges, Entry, EntryRadius, EvaluateLineForX(), FindInterceptOfTwoLines(), Init, LINE_STOP_FUNCTION, 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, Point2D::x, 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
FirstWPfirst waypoint/corner of the polygon
Sizenumber of waypoints/corners used to define the polygon, if zero uses Poly_Size
Sweepdistance between scan lines, if zero uses Poly_Sweep
SecondWPsecond waypoint towards which the sweep orientation is computed

Definition at line 81 of file nav_survey_poly_osam.c.

References nav_survey_poly_osam_setup(), Poly_Size, Poly_Sweep, Sweep, and waypoints.

Here is the call graph for this function:

◆ RotateAndTranslateToWorld()

void RotateAndTranslateToWorld ( struct Point2D * p,
float Zrot,
float transX,
float transY )
static

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

Definition at line 597 of file nav_survey_poly_osam.c.

References p.

Referenced by nav_survey_poly_osam_run().

Here is the caller graph for this function:

◆ TranslateAndRotateFromWorld()

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

Definition at line 584 of file nav_survey_poly_osam.c.

References p.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

Here is the caller graph for this function:

Variable Documentation

◆ CSurveyStatus

◆ dSweep

◆ EdgeMaxY

◆ EdgeMinY

◆ Edges

◆ EntryRadius

float EntryRadius

Definition at line 143 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_setup().

◆ Half_Sweep_Enabled

bool Half_Sweep_Enabled = POLY_OSAM_HALF_SWEEP_ENABLED

Definition at line 78 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_run().

◆ MaxY

◆ Poly_Size

◆ Poly_Sweep

◆ PolySurveySweepBackNum

◆ PolySurveySweepNum

◆ Reset_Sweep

bool Reset_Sweep = FALSE

Definition at line 79 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_ResetSweepNumber().

◆ SmallestCorner

struct Point2D SmallestCorner
static

◆ SurveyCircle

struct Point2D SurveyCircle
static

Definition at line 136 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ SurveyCircleQdr

float SurveyCircleQdr
static

Definition at line 139 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ SurveyEntryWP

◆ SurveyFromWP

◆ SurveyRadius

float SurveyRadius
static

Definition at line 133 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ SurveySize

◆ SurveyTheta

float SurveyTheta
static

◆ SurveyToWP

◆ use_full_circle

bool use_full_circle = POLY_OSAM_USE_FULL_CIRCLE

Definition at line 77 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run().