Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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. 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

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

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

◆ 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

enum SurveyStatus CSurveyStatus
static

Definition at line 126 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ dSweep

float dSweep
static

Definition at line 132 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ EdgeMaxY

float EdgeMaxY[PolygonSize]
static

Definition at line 129 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ EdgeMinY

float EdgeMinY[PolygonSize]
static

Definition at line 130 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ Edges

struct Line Edges[PolygonSize]
static

Definition at line 128 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

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

◆ MaxY

float MaxY
static

Definition at line 140 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ Poly_Size

Definition at line 75 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_setup_towards().

◆ Poly_Sweep

◆ PolySurveySweepBackNum

◆ PolySurveySweepNum

uint16_t PolySurveySweepNum

Definition at line 141 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ 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

Definition at line 127 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ 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

uint8_t SurveyEntryWP
static

Definition at line 137 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ SurveyFromWP

struct Point2D SurveyFromWP
static

Definition at line 135 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ 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

uint8_t SurveySize
static

Definition at line 138 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ SurveyTheta

float SurveyTheta
static

Definition at line 131 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

◆ SurveyToWP

struct Point2D SurveyToWP
static

Definition at line 134 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

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