Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
OSAMNav.c File Reference
#include "subsystems/navigation/OSAMNav.h"
#include "subsystems/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
+ Include dependency graph for OSAMNav.c:

Go to the source code of this file.

Macros

#define LINE_START_FUNCTION   {}
 
#define LINE_STOP_FUNCTION   {}
 
#define Takeoff_Distance   10
 Takeoff functions for bungee takeoff. More...
 
#define Takeoff_Height   30
 
#define Takeoff_Speed   15
 
#define Takeoff_MinSpeed   5
 
#define Landing_AFHeight   50
 Landing Routine. More...
 
#define Landing_FinalHeight   5
 
#define Landing_FinalStageTime   5
 

Enumerations

enum  FlowerStatus { Outside, FlowerLine, Circle }
 Makes a flower pattern. More...
 
enum  TakeoffStatus { Launch, Throttle, Finished }
 
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...
 
enum  line_status {
  LR12, LQC21, LTC2, LQC22,
  LR21, LQC12, LTC1, LQC11,
  LR12, LQC21, LTC2, LQC22,
  LR21, LQC12, LTC1, LQC11
}
 Copy of nav line. More...
 
enum  LandingStatus { CircleDown, LandingWait, Final, Approach }
 
enum  FLStatus { FLInitialize, FLCircleS, FLLine, FLFinished }
 

Functions

bool_t InitializeFlower (uint8_t CenterWP, uint8_t EdgeWP)
 
bool_t FlowerNav (void)
 
bool_t InitializeBungeeTakeoff (uint8_t BungeeWP)
 
bool_t BungeeTakeoff (void)
 
bool_t InitializePolygonSurvey (uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
 
bool_t PolygonSurvey (void)
 
bool_t InitializeVerticalRaster (void)
 
bool_t VerticalRaster (uint8_t l1, uint8_t l2, float radius, float AltSweep)
 
bool_t InitializeSkidLanding (uint8_t AFWP, uint8_t TDWP, float radius)
 
bool_t SkidLanding (void)
 
bool_t FlightLine (uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
 
bool_t FlightLineBlock (uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After)
 
void TranslateAndRotateFromWorld (struct Point2D *p, float Zrot, float transX, float transY)
 
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...
 
void FindInterceptOfTwoLines (float *x, float *y, struct Line L1, struct Line L2)
 
float EvaluateLineForY (float x, struct Line L)
 
float EvaluateLineForX (float y, struct Line L)
 
float DistanceEquation (struct Point2D p1, struct Point2D p2)
 

Variables

static enum FlowerStatus CFlowerStatus
 
static float CircleX
 
static float CircleY
 
static float Fly2X
 
static float Fly2Y
 
static float FlyFromX
 
static float FlyFromY
 
static float TransCurrentX
 
static float TransCurrentY
 
static float EdgeCurrentX
 
static float EdgeCurrentY
 
static float DistanceFromCenter
 
static float FlowerTheta
 
static float Flowerradius
 
static uint8_t Center
 
static uint8_t Edge
 
static enum TakeoffStatus CTakeoffStatus
 
static float throttlePx
 
static float throttlePy
 
static float initialx
 
static float initialy
 
static float ThrottleSlope
 
static bool_t AboveLine
 
static float BungeeAlt
 
static float TDistance
 
static uint8_t BungeeWaypoint
 
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
 
static enum line_status line_status
 
static enum LandingStatus CLandingStatus
 
static uint8_t AFWaypoint
 
static uint8_t TDWaypoint
 
static float LandRadius
 
static struct Point2D LandCircle
 
static float LandAppAlt
 
static float LandCircleQDR
 
static float ApproachQDR
 
static float FinalLandAltitude
 
static uint8_t FinalLandCount
 
static enum FLStatus CFLStatus = FLInitialize
 
static struct Point2D FLCircle
 
static struct Point2D FLFROMWP
 
static struct Point2D FLTOWP
 
static float FLQDR
 
static float FLRadius
 
static uint8_t FLBlockCount = 0
 

Macro Definition Documentation

#define Landing_AFHeight   50

Landing Routine.

<section name="Landing" prefix="Landing_">
<define name="AFHeight" value="50" unit="m"/>
<define name="FinalHeight" value="5" unit="m"/>
<define name="FinalStageTime" value="5" unit="s"/>
</section>

Definition at line 896 of file OSAMNav.c.

Referenced by InitializeSkidLanding().

#define Landing_FinalHeight   5

Definition at line 899 of file OSAMNav.c.

Referenced by InitializeSkidLanding().

#define Landing_FinalStageTime   5

Definition at line 902 of file OSAMNav.c.

Referenced by SkidLanding().

#define LINE_START_FUNCTION   {}

Definition at line 60 of file OSAMNav.c.

Referenced by FlightLine(), and PolygonSurvey().

#define LINE_STOP_FUNCTION   {}

Definition at line 63 of file OSAMNav.c.

Referenced by FlightLine(), InitializePolygonSurvey(), and PolygonSurvey().

#define Takeoff_Distance   10

Takeoff functions for bungee takeoff.

Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to launch the plane. After initialized, the plane will follow a line drawn by the position of the plane on initialization and the position of the bungee (given in the arguments). Once the plane crosses the throttle line, which is perpendicular to the line the plane is following, and intersects the position of the bungee (plus or minus a fixed distance (TakeOff_Distance in airframe file) from the bungee just in case the bungee doesn't release directly above the bungee) the prop will come on. The plane will then continue to follow the line until it has reached a specific height (defined in as Takeoff_Height in airframe file) above the bungee waypoint and speed (defined as Takeoff_Speed in the airframe file).

<section name="Takeoff" prefix="Takeoff_">
<define name="Height" value="30" unit="m"/>
<define name="Speed" value="15" unit="m/s"/>
<define name="Distance" value="10" unit="m"/>
<define name="MinSpeed" value="5" unit="m/s"/>
</section>

Definition at line 182 of file OSAMNav.c.

Referenced by InitializeBungeeTakeoff().

#define Takeoff_Height   30

Definition at line 185 of file OSAMNav.c.

Referenced by BungeeTakeoff().

#define Takeoff_MinSpeed   5

Definition at line 191 of file OSAMNav.c.

Referenced by BungeeTakeoff().

#define Takeoff_Speed   15

Definition at line 188 of file OSAMNav.c.

Referenced by BungeeTakeoff().

Enumeration Type Documentation

Makes a flower pattern.

CenterWP is the center of the flower. The Navigation Height is taken from this waypoint. EdgeWP defines the radius of the flower (distance from CenterWP to EdgeWP)

Enumerator
Outside 
FlowerLine 
Circle 

Definition at line 41 of file OSAMNav.c.

enum FLStatus
Enumerator
FLInitialize 
FLCircleS 
FLLine 
FLFinished 

Definition at line 1024 of file OSAMNav.c.

Enumerator
CircleDown 
LandingWait 
Final 
Approach 

Definition at line 905 of file OSAMNav.c.

Copy of nav line.

The only difference is it changes altitude every sweep, but doesn't come out of circle until it reaches altitude.

Enumerator
LR12 
LQC21 
LTC2 
LQC22 
LR21 
LQC12 
LTC1 
LQC11 
LR12 
LQC21 
LTC2 
LQC22 
LR21 
LQC12 
LTC1 
LQC11 

Definition at line 768 of file OSAMNav.c.

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 359 of file OSAMNav.c.

Enumerator
Launch 
Throttle 
Finished 

Definition at line 194 of file OSAMNav.c.

Function Documentation

float DistanceEquation ( struct Point2D  p1,
struct Point2D  p2 
)

Definition at line 1217 of file OSAMNav.c.

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

float EvaluateLineForX ( float  y,
struct Line  L 
)

Definition at line 1212 of file OSAMNav.c.

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

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

+ Here is the caller graph for this function:

float EvaluateLineForY ( float  x,
struct Line  L 
)

Definition at line 1207 of file OSAMNav.c.

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

void FindInterceptOfTwoLines ( float *  x,
float *  y,
struct Line  L1,
struct Line  L2 
)

Definition at line 1201 of file OSAMNav.c.

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

Referenced by InitializePolygonSurvey().

+ Here is the caller graph for this function:

bool_t FlightLine ( uint8_t  From_WP,
uint8_t  To_WP,
float  radius,
float  Space_Before,
float  Space_After 
)
bool_t FlightLineBlock ( uint8_t  First_WP,
uint8_t  Last_WP,
float  radius,
float  Space_Before,
float  Space_After 
)

Definition at line 1139 of file OSAMNav.c.

References CFLStatus, FALSE, FLBlockCount, FlightLine(), FLInitialize, and TRUE.

+ Here is the call graph for this function:

bool_t InitializeBungeeTakeoff ( uint8_t  BungeeWP)
bool_t InitializeFlower ( uint8_t  CenterWP,
uint8_t  EdgeWP 
)
bool_t InitializeSkidLanding ( uint8_t  AFWP,
uint8_t  TDWP,
float  radius 
)
bool_t InitializeVerticalRaster ( void  )

Definition at line 771 of file OSAMNav.c.

References FALSE, and LR12.

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)

Definition at line 1190 of file OSAMNav.c.

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

Referenced by PolygonSurvey().

+ Here is the caller graph for this function:

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

Definition at line 1177 of file OSAMNav.c.

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

Referenced by FlightLine(), and InitializePolygonSurvey().

+ Here is the caller graph for this function:

bool_t VerticalRaster ( uint8_t  l1,
uint8_t  l2,
float  radius,
float  AltSweep 
)

Variable Documentation

bool_t AboveLine
static

Definition at line 201 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

uint8_t AFWaypoint
static

Definition at line 907 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float ApproachQDR
static

Definition at line 913 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float BungeeAlt
static

Definition at line 202 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

uint8_t BungeeWaypoint
static

Definition at line 204 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

uint8_t Center
static

Definition at line 56 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

enum FlowerStatus CFlowerStatus
static

Definition at line 42 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

enum FLStatus CFLStatus = FLInitialize
static

Definition at line 1025 of file OSAMNav.c.

Referenced by FlightLine(), and FlightLineBlock().

float CircleX
static

Definition at line 43 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float CircleY
static

Definition at line 44 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

enum LandingStatus CLandingStatus
static

Definition at line 906 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

enum SurveyStatus CSurveyStatus
static

Definition at line 360 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

enum TakeoffStatus CTakeoffStatus
static

Definition at line 195 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float DistanceFromCenter
static

Definition at line 53 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float dSweep
static

Definition at line 366 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint8_t Edge
static

Definition at line 57 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float EdgeCurrentX
static

Definition at line 51 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float EdgeCurrentY
static

Definition at line 52 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float EdgeMaxY[PolygonSize]
static

Definition at line 363 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float EdgeMinY[PolygonSize]
static

Definition at line 364 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Line Edges[PolygonSize]
static

Definition at line 362 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float FinalLandAltitude
static

Definition at line 914 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

uint8_t FinalLandCount
static

Definition at line 915 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

uint8_t FLBlockCount = 0
static

Definition at line 1137 of file OSAMNav.c.

Referenced by FlightLineBlock().

struct Point2D FLCircle
static

Definition at line 1026 of file OSAMNav.c.

Referenced by FlightLine().

struct Point2D FLFROMWP
static

Definition at line 1027 of file OSAMNav.c.

Referenced by FlightLine().

float Flowerradius
static

Definition at line 55 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float FlowerTheta
static

Definition at line 54 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float FLQDR
static

Definition at line 1029 of file OSAMNav.c.

Referenced by FlightLine().

float FLRadius
static

Definition at line 1030 of file OSAMNav.c.

Referenced by FlightLine().

struct Point2D FLTOWP
static

Definition at line 1028 of file OSAMNav.c.

Referenced by FlightLine().

float Fly2X
static

Definition at line 45 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float Fly2Y
static

Definition at line 46 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float FlyFromX
static

Definition at line 47 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float FlyFromY
static

Definition at line 48 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float initialx
static

Definition at line 198 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float initialy
static

Definition at line 199 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float LandAppAlt
static

Definition at line 911 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

struct Point2D LandCircle
static

Definition at line 910 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float LandCircleQDR
static

Definition at line 912 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float LandRadius
static

Definition at line 909 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

enum line_status line_status
static

Definition at line 769 of file OSAMNav.c.

float MaxY
static

Definition at line 374 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint16_t PolySurveySweepBackNum

Definition at line 376 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint16_t PolySurveySweepNum

Definition at line 375 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Point2D SmallestCorner
static

Definition at line 361 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Point2D SurveyCircle
static

Definition at line 370 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float SurveyCircleQdr
static

Definition at line 373 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint8_t SurveyEntryWP
static

Definition at line 371 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Point2D SurveyFromWP
static

Definition at line 369 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float SurveyRadius
static

Definition at line 367 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint8_t SurveySize
static

Definition at line 372 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float SurveyTheta
static

Definition at line 365 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Point2D SurveyToWP
static

Definition at line 368 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float TDistance
static

Definition at line 203 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

uint8_t TDWaypoint
static

Definition at line 908 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float throttlePx
static

Definition at line 196 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float throttlePy
static

Definition at line 197 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float ThrottleSlope
static

Definition at line 200 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float TransCurrentX
static

Definition at line 49 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float TransCurrentY
static

Definition at line 50 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().