Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
OSAMNav.c File Reference
#include "subsystems/navigation/OSAMNav.h"
#include "subsystems/nav.h"
#include "estimator.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
 
#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)
 
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

Definition at line 869 of file OSAMNav.c.

Referenced by InitializeSkidLanding().

#define Landing_FinalHeight   5

Definition at line 872 of file OSAMNav.c.

Referenced by InitializeSkidLanding().

#define Landing_FinalStageTime   5

Definition at line 875 of file OSAMNav.c.

Referenced by SkidLanding().

#define LINE_START_FUNCTION   {}

Definition at line 34 of file OSAMNav.c.

Referenced by FlightLine(), and PolygonSurvey().

#define LINE_STOP_FUNCTION   {}

Definition at line 37 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 155 of file OSAMNav.c.

Referenced by InitializeBungeeTakeoff().

#define Takeoff_Height   30

Definition at line 158 of file OSAMNav.c.

Referenced by BungeeTakeoff().

#define Takeoff_MinSpeed   5

Definition at line 164 of file OSAMNav.c.

Referenced by BungeeTakeoff().

#define Takeoff_Speed   15

Definition at line 161 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 15 of file OSAMNav.c.

enum FLStatus
Enumerator
FLInitialize 
FLCircleS 
FLLine 
FLFinished 

Definition at line 997 of file OSAMNav.c.

Enumerator
CircleDown 
LandingWait 
Final 
Approach 

Definition at line 878 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 741 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 332 of file OSAMNav.c.

Enumerator
Launch 
Throttle 
Finished 

Definition at line 167 of file OSAMNav.c.

Function Documentation

float DistanceEquation ( struct Point2D  p1,
struct Point2D  p2 
)

Definition at line 1192 of file OSAMNav.c.

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

float EvaluateLineForX ( float  y,
struct Line  L 
)

Definition at line 1187 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 1182 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 1176 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 1112 of file OSAMNav.c.

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

+ Here is the call graph for this function:

bool_t InitializeVerticalRaster ( void  )

Definition at line 744 of file OSAMNav.c.

References FALSE, and LR12.

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

Definition at line 1165 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 1150 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 174 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

uint8_t AFWaypoint
static

Definition at line 880 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float ApproachQDR
static

Definition at line 886 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float BungeeAlt
static

Definition at line 175 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

uint8_t BungeeWaypoint
static

Definition at line 177 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

uint8_t Center
static

Definition at line 30 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

enum FlowerStatus CFlowerStatus
static

Definition at line 16 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

enum FLStatus CFLStatus = FLInitialize
static

Definition at line 998 of file OSAMNav.c.

Referenced by FlightLine(), and FlightLineBlock().

float CircleX
static

Definition at line 17 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float CircleY
static

Definition at line 18 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

enum LandingStatus CLandingStatus
static

Definition at line 879 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

enum SurveyStatus CSurveyStatus
static

Definition at line 333 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

enum TakeoffStatus CTakeoffStatus
static

Definition at line 168 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float DistanceFromCenter
static

Definition at line 27 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float dSweep
static

Definition at line 339 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint8_t Edge
static

Definition at line 31 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float EdgeCurrentX
static

Definition at line 25 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float EdgeCurrentY
static

Definition at line 26 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float EdgeMaxY[PolygonSize]
static

Definition at line 336 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float EdgeMinY[PolygonSize]
static

Definition at line 337 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Line Edges[PolygonSize]
static

Definition at line 335 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float FinalLandAltitude
static

Definition at line 887 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

uint8_t FinalLandCount
static

Definition at line 888 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

uint8_t FLBlockCount = 0
static

Definition at line 1110 of file OSAMNav.c.

Referenced by FlightLineBlock().

struct Point2D FLCircle
static

Definition at line 999 of file OSAMNav.c.

Referenced by FlightLine().

struct Point2D FLFROMWP
static

Definition at line 1000 of file OSAMNav.c.

Referenced by FlightLine().

float Flowerradius
static

Definition at line 29 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float FlowerTheta
static

Definition at line 28 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float FLQDR
static

Definition at line 1002 of file OSAMNav.c.

Referenced by FlightLine().

float FLRadius
static

Definition at line 1003 of file OSAMNav.c.

Referenced by FlightLine().

struct Point2D FLTOWP
static

Definition at line 1001 of file OSAMNav.c.

Referenced by FlightLine().

float Fly2X
static

Definition at line 19 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float Fly2Y
static

Definition at line 20 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float FlyFromX
static

Definition at line 21 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float FlyFromY
static

Definition at line 22 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float initialx
static

Definition at line 171 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float initialy
static

Definition at line 172 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float LandAppAlt
static

Definition at line 884 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

struct Point2D LandCircle
static

Definition at line 883 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float LandCircleQDR
static

Definition at line 885 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float LandRadius
static

Definition at line 882 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

enum line_status line_status
static

Definition at line 742 of file OSAMNav.c.

float MaxY
static

Definition at line 347 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint16_t PolySurveySweepBackNum

Definition at line 349 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint16_t PolySurveySweepNum

Definition at line 348 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Point2D SmallestCorner
static

Definition at line 334 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Point2D SurveyCircle
static

Definition at line 343 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float SurveyCircleQdr
static

Definition at line 346 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint8_t SurveyEntryWP
static

Definition at line 344 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Point2D SurveyFromWP
static

Definition at line 342 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float SurveyRadius
static

Definition at line 340 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

uint8_t SurveySize
static

Definition at line 345 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float SurveyTheta
static

Definition at line 338 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

struct Point2D SurveyToWP
static

Definition at line 341 of file OSAMNav.c.

Referenced by InitializePolygonSurvey(), and PolygonSurvey().

float TDistance
static

Definition at line 176 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

uint8_t TDWaypoint
static

Definition at line 881 of file OSAMNav.c.

Referenced by InitializeSkidLanding(), and SkidLanding().

float throttlePx
static

Definition at line 169 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float throttlePy
static

Definition at line 170 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float ThrottleSlope
static

Definition at line 173 of file OSAMNav.c.

Referenced by BungeeTakeoff(), and InitializeBungeeTakeoff().

float TransCurrentX
static

Definition at line 23 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().

float TransCurrentY
static

Definition at line 24 of file OSAMNav.c.

Referenced by FlowerNav(), and InitializeFlower().