Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
nav_survey_poly_rotorcraft.c File Reference
#include "modules/nav/nav_survey_poly_rotorcraft.h"
#include "firmwares/rotorcraft/navigation.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
+ Include dependency graph for nav_survey_poly_rotorcraft.c:

Go to the source code of this file.

Data Structures

struct  Point2D
 
struct  Line
 

Macros

#define POLYSURVEY_DEFAULT_SIZE   10
 
#define POLYSURVEY_DEFAULT_DISTANCE   25
 
#define POLYSURVEY_ENTRY_DISTANCE   0
 if 0 default to half sweep More...
 
#define POLYSURVEY_MAX_POLYGONSIZE   20
 maximum number of polygon corners More...
 
#define POLY_OSAM_HALF_SWEEP_ENABLED   TRUE
 
#define MaxPolygonSize   POLYSURVEY_MAX_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_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 EnuCoor_f *p, float Zrot, float transX, float transY)
 
static void RotateAndTranslateToWorld (struct EnuCoor_f *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_setup (uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
 Setup polygon survey. More...
 
bool nav_survey_poly_run (void)
 Run polygon survey. More...
 

Variables

uint8_t Poly_Size = POLYSURVEY_DEFAULT_SIZE
 
float Poly_Distance = POLYSURVEY_DEFAULT_DISTANCE
 
static enum SurveyStatus CSurveyStatus
 
static struct Point2D SmallestCorner
 
static struct Line Edges [MaxPolygonSize]
 
static float EdgeMaxY [MaxPolygonSize]
 
static float EdgeMinY [MaxPolygonSize]
 
static float SurveyTheta
 
static float dSweep
 
static struct EnuCoor_f SurveyToWP
 
static struct EnuCoor_f SurveyFromWP
 
static struct EnuCoor_f SurveyEntry
 
static struct EnuCoor_i survey_from_i survey_to_i
 
static uint8_t SurveyEntryWP
 
static uint8_t SurveySize
 
static float MaxY
 
uint16_t PolySurveySweepNum
 
uint16_t PolySurveySweepBackNum
 
float EntryRadius
 
bool Half_Sweep_Enabled = POLY_OSAM_HALF_SWEEP_ENABLED
 

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 90 of file nav_survey_poly_rotorcraft.c.

◆ LINE_STOP_FUNCTION

#define LINE_STOP_FUNCTION   {}

Definition at line 93 of file nav_survey_poly_rotorcraft.c.

◆ MaxFloat

#define MaxFloat   1000000000

Definition at line 86 of file nav_survey_poly_rotorcraft.c.

◆ MaxPolygonSize

#define MaxPolygonSize   POLYSURVEY_MAX_POLYGONSIZE

Definition at line 85 of file nav_survey_poly_rotorcraft.c.

◆ MinFloat

#define MinFloat   -1000000000

Definition at line 87 of file nav_survey_poly_rotorcraft.c.

◆ POLY_OSAM_HALF_SWEEP_ENABLED

#define POLY_OSAM_HALF_SWEEP_ENABLED   TRUE

Definition at line 57 of file nav_survey_poly_rotorcraft.c.

◆ POLYSURVEY_DEFAULT_DISTANCE

#define POLYSURVEY_DEFAULT_DISTANCE   25

Definition at line 42 of file nav_survey_poly_rotorcraft.c.

◆ POLYSURVEY_DEFAULT_SIZE

#define POLYSURVEY_DEFAULT_SIZE   10

Definition at line 38 of file nav_survey_poly_rotorcraft.c.

◆ POLYSURVEY_ENTRY_DISTANCE

#define POLYSURVEY_ENTRY_DISTANCE   0

if 0 default to half sweep

Definition at line 47 of file nav_survey_poly_rotorcraft.c.

◆ POLYSURVEY_MAX_POLYGONSIZE

#define POLYSURVEY_MAX_POLYGONSIZE   20

maximum number of polygon corners

Definition at line 52 of file nav_survey_poly_rotorcraft.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 100 of file nav_survey_poly_rotorcraft.c.

Function Documentation

◆ EvaluateLineForX()

float EvaluateLineForX ( float  y,
struct Line  L 
)
static

Definition at line 528 of file nav_survey_poly_rotorcraft.c.

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

Referenced by nav_survey_poly_run(), and nav_survey_poly_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 521 of file nav_survey_poly_rotorcraft.c.

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

Referenced by nav_survey_poly_setup().

+ Here is the caller graph for this function:

◆ nav_survey_poly_run()

◆ nav_survey_poly_setup()

void nav_survey_poly_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 123 of file nav_survey_poly_rotorcraft.c.

References Line::b, CSurveyStatus, dSweep, EdgeMaxY, EdgeMinY, Edges, Entry, EvaluateLineForX(), FindInterceptOfTwoLines(), Init, LINE_STOP_FUNCTION, Line::m, MaxFloat, MaxPolygonSize, MaxY, nav_set_heading_deg(), NavVerticalAltitudeMode, Poly_Distance, POLYSURVEY_ENTRY_DISTANCE, PolySurveySweepBackNum, PolySurveySweepNum, SmallestCorner, SurveyEntry, SurveyEntryWP, SurveyFromWP, SurveySize, SurveyTheta, SurveyToWP, TranslateAndRotateFromWorld(), waypoints, point::x, EnuCoor_f::x, Point2D::x, point::y, EnuCoor_f::y, and Point2D::y.

Referenced by nav_survey_poly_setup_towards().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ nav_survey_poly_setup_towards()

void nav_survey_poly_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_Distance 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_Distance
SecondWPsecond waypoint towards which the sweep orientation is computed

Definition at line 63 of file nav_survey_poly_rotorcraft.c.

References f, nav_survey_poly_setup(), Poly_Distance, Poly_Size, Sweep, waypoints, point::x, and point::y.

+ Here is the call graph for this function:

◆ RotateAndTranslateToWorld()

void RotateAndTranslateToWorld ( struct EnuCoor_f 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 510 of file nav_survey_poly_rotorcraft.c.

References p.

Referenced by nav_survey_poly_run().

+ Here is the caller graph for this function:

◆ TranslateAndRotateFromWorld()

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

Definition at line 497 of file nav_survey_poly_rotorcraft.c.

References p.

Referenced by nav_survey_poly_setup().

+ Here is the caller graph for this function:

Variable Documentation

◆ CSurveyStatus

enum SurveyStatus CSurveyStatus
static

Definition at line 101 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ dSweep

float dSweep
static

Definition at line 107 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ EdgeMaxY

float EdgeMaxY[MaxPolygonSize]
static

Definition at line 104 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ EdgeMinY

float EdgeMinY[MaxPolygonSize]
static

Definition at line 105 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ Edges

struct Line Edges[MaxPolygonSize]
static

Definition at line 103 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ EntryRadius

float EntryRadius

Definition at line 119 of file nav_survey_poly_rotorcraft.c.

◆ Half_Sweep_Enabled

bool Half_Sweep_Enabled = POLY_OSAM_HALF_SWEEP_ENABLED

Definition at line 120 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run().

◆ MaxY

float MaxY
static

Definition at line 116 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ Poly_Distance

◆ Poly_Size

Definition at line 60 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_setup_towards().

◆ PolySurveySweepBackNum

uint16_t PolySurveySweepBackNum

Definition at line 118 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ PolySurveySweepNum

uint16_t PolySurveySweepNum

Definition at line 117 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ SmallestCorner

struct Point2D SmallestCorner
static

Definition at line 102 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ survey_to_i

struct EnuCoor_i survey_from_i survey_to_i
static

Definition at line 112 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run().

◆ SurveyEntry

struct EnuCoor_f SurveyEntry
static

Definition at line 110 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ SurveyEntryWP

uint8_t SurveyEntryWP
static

Definition at line 114 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ SurveyFromWP

struct EnuCoor_f SurveyFromWP
static

Definition at line 109 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ SurveySize

uint8_t SurveySize
static

Definition at line 115 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ SurveyTheta

float SurveyTheta
static

Definition at line 106 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

◆ SurveyToWP

struct EnuCoor_f SurveyToWP
static

Definition at line 108 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().