Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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

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

Referenced by nav_survey_poly_osam_run().

#define LINE_STOP_FUNCTION   {}

Definition at line 117 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

#define MaxFloat   1000000000

Definition at line 110 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_setup().

#define MinFloat   -1000000000

Definition at line 111 of file nav_survey_poly_osam.c.

#define POLY_OSAM_DEFAULT_SIZE   5

Definition at line 39 of file nav_survey_poly_osam.c.

#define POLY_OSAM_DEFAULT_SWEEP   100

Definition at line 43 of file nav_survey_poly_osam.c.

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

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

#define POLY_OSAM_HALF_SWEEP_ENABLED   TRUE

Definition at line 72 of file nav_survey_poly_osam.c.

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

#define POLY_OSAM_POLYGONSIZE   10

maximum number of polygon corners

Definition at line 63 of file nav_survey_poly_osam.c.

#define POLY_OSAM_USE_FULL_CIRCLE   TRUE

Definition at line 67 of file nav_survey_poly_osam.c.

#define PolygonSize   POLY_OSAM_POLYGONSIZE

Definition at line 109 of file nav_survey_poly_osam.c.

Enumeration Type Documentation

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

float EvaluateLineForX ( float  y,
struct Line  L 
)
static

Definition at line 615 of file nav_survey_poly_osam.c.

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

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

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

Referenced by nav_survey_poly_osam_setup().

+ Here is the caller graph for this function:

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.

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

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, waypoints, point::x, and point::y.

+ Here is the call graph for this function:

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 Point2D::x, and Point2D::y.

Referenced by nav_survey_poly_osam_run().

+ Here is the caller graph for this function:

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

Definition at line 584 of file nav_survey_poly_osam.c.

References Point2D::x, and Point2D::y.

Referenced by nav_survey_poly_osam_run(), and nav_survey_poly_osam_setup().

+ Here is the caller graph for this function:

Variable Documentation

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

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

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

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

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

float EntryRadius

Definition at line 143 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_setup().

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

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

Definition at line 75 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_setup_towards().

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

bool Reset_Sweep = FALSE

Definition at line 79 of file nav_survey_poly_osam.c.

Referenced by nav_survey_poly_osam_ResetSweepNumber().

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

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

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

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

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

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

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

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

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

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