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

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

Referenced by nav_survey_poly_run().

#define LINE_STOP_FUNCTION   {}

Definition at line 94 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

#define MaxFloat   1000000000

Definition at line 87 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_setup().

#define MaxPolygonSize   POLYSURVEY_MAX_POLYGONSIZE

Definition at line 86 of file nav_survey_poly_rotorcraft.c.

#define MinFloat   -1000000000

Definition at line 88 of file nav_survey_poly_rotorcraft.c.

#define POLY_OSAM_HALF_SWEEP_ENABLED   TRUE

Definition at line 57 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_DEFAULT_DISTANCE   25

Definition at line 42 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_DEFAULT_SIZE   10

Definition at line 38 of file nav_survey_poly_rotorcraft.c.

#define POLYSURVEY_ENTRY_DISTANCE   0

if 0 default to half sweep

Definition at line 47 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_setup().

#define POLYSURVEY_MAX_POLYGONSIZE   20

maximum number of polygon corners

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

Function Documentation

float EvaluateLineForX ( float  y,
struct Line  L 
)
static

Definition at line 524 of file nav_survey_poly_rotorcraft.c.

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

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

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

Referenced by nav_survey_poly_setup().

+ Here is the caller graph for this function:

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 124 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, MaxY, nav_set_heading_deg(), NavVerticalAltitudeMode, 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:

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

Definition at line 64 of file nav_survey_poly_rotorcraft.c.

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

+ Here is the call graph for this function:

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

References EnuCoor_f::x, and EnuCoor_f::y.

Referenced by nav_survey_poly_run().

+ Here is the caller graph for this function:

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

Definition at line 493 of file nav_survey_poly_rotorcraft.c.

References EnuCoor_f::x, and EnuCoor_f::y.

Referenced by nav_survey_poly_setup().

+ Here is the caller graph for this function:

Variable Documentation

enum SurveyStatus CSurveyStatus
static

Definition at line 102 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

float dSweep
static

Definition at line 108 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

float EdgeMaxY[MaxPolygonSize]
static

Definition at line 105 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

float EdgeMinY[MaxPolygonSize]
static

Definition at line 106 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

struct Line Edges[MaxPolygonSize]
static

Definition at line 104 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

float EntryRadius

Definition at line 120 of file nav_survey_poly_rotorcraft.c.

bool Half_Sweep_Enabled = POLY_OSAM_HALF_SWEEP_ENABLED

Definition at line 121 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run().

float MaxY
static

Definition at line 117 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

float Poly_Distance = POLYSURVEY_DEFAULT_DISTANCE

Definition at line 61 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_setup_towards().

Definition at line 60 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_setup_towards().

uint16_t PolySurveySweepBackNum

Definition at line 119 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

uint16_t PolySurveySweepNum

Definition at line 118 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

struct Point2D SmallestCorner
static

Definition at line 103 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

struct EnuCoor_i survey_from_i survey_to_i
static

Definition at line 113 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run().

struct EnuCoor_f SurveyEntry
static

Definition at line 111 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

uint8_t SurveyEntryWP
static

Definition at line 115 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

struct EnuCoor_f SurveyFromWP
static

Definition at line 110 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

uint8_t SurveySize
static

Definition at line 116 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

float SurveyTheta
static

Definition at line 107 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().

struct EnuCoor_f SurveyToWP
static

Definition at line 109 of file nav_survey_poly_rotorcraft.c.

Referenced by nav_survey_poly_run(), and nav_survey_poly_setup().