bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
{
SmallestCorner.x = 0;
SmallestCorner.y = 0;
int i = 0;
float ys = 0;
static struct Point2D EntryPoint;
float LeftYInt;
float RightYInt;
float temp;
float XIntercept1 = 0;
float XIntercept2 = 0;
SurveyTheta = RadOfDeg(Orientation);
PolySurveySweepNum = 0;
PolySurveySweepBackNum = 0;
SurveyEntryWP = EntryWP;
SurveySize = Size;
struct Point2D Corners[PolygonSize];
CSurveyStatus = Init;
//Don't initialize if Polygon is too big or if the orientation is not between 0 and 90
if(Size <= PolygonSize && Orientation >= -90 && Orientation <= 90)
{
//Initialize Corners
for(i = 0; i < Size; i++)
{
Corners[i].x = waypoints[i+EntryWP].x;
Corners[i].y = waypoints[i+EntryWP].y;
}
//Rotate Corners so sweeps are parellel with x axis
for(i = 0; i < Size; i++)
TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0);
//Find min x and min y
SmallestCorner.y = Corners[0].y;
SmallestCorner.x = Corners[0].x;
for(i = 1; i < Size; i++)
{
if(Corners[i].y < SmallestCorner.y)
SmallestCorner.y = Corners[i].y;
if(Corners[i].x < SmallestCorner.x)
SmallestCorner.x = Corners[i].x;
}
//Translate Corners all exist in quad #1
for(i = 0; i < Size; i++)
TranslateAndRotateFromWorld(&Corners[i], 0, SmallestCorner.x, SmallestCorner.y);
//Rotate and Translate Entry Point
EntryPoint.x = Corners[0].x;
EntryPoint.y = Corners[0].y;
//Find max y
MaxY = Corners[0].y;
for(i = 1; i < Size; i++)
{
if(Corners[i].y > MaxY)
MaxY = Corners[i].y;
}
//Find polygon edges
for(i = 0; i < Size; i++)
{
if(i == 0)
if(Corners[Size-1].x == Corners[i].x) //Don't divide by zero!
Edges[i].m = MaxFloat;
else
Edges[i].m = ((Corners[Size-1].y-Corners[i].y)/(Corners[Size-1].x-Corners[i].x));
else
if(Corners[i].x == Corners[i-1].x)
Edges[i].m = MaxFloat;
else
Edges[i].m = ((Corners[i].y-Corners[i-1].y)/(Corners[i].x-Corners[i-1].x));
//Edges[i].m = MaxFloat;
Edges[i].b = (Corners[i].y - (Corners[i].x*Edges[i].m));
}
//Find Min and Max y for each line
FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]);
FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size-1]);
if(LeftYInt > RightYInt)
{
EdgeMaxY[0] = LeftYInt;
EdgeMinY[0] = RightYInt;
}
else
{
EdgeMaxY[0] = RightYInt;
EdgeMinY[0] = LeftYInt;
}
for(i = 1; i < Size-1; i++)
{
FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i+1]);
FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i-1]);
if(LeftYInt > RightYInt)
{
EdgeMaxY[i] = LeftYInt;
EdgeMinY[i] = RightYInt;
}
else
{
EdgeMaxY[i] = RightYInt;
EdgeMinY[i] = LeftYInt;
}
}
FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size-1], Edges[0]);
FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size-1], Edges[Size-2]);
if(LeftYInt > RightYInt)
{
EdgeMaxY[Size-1] = LeftYInt;
EdgeMinY[Size-1] = RightYInt;
}
else
{
EdgeMaxY[Size-1] = RightYInt;
EdgeMinY[Size-1] = LeftYInt;
}
//Find amount to increment by every sweep
if(EntryPoint.y >= MaxY/2)
dSweep = -sw;
else
dSweep = sw;
//CircleQdr tells the plane when to exit the circle
if(dSweep >= 0)
SurveyCircleQdr = -DegOfRad(SurveyTheta);
else
SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
//Find y value of the first sweep
ys = EntryPoint.y+(dSweep/2);
//Find the edges which intercet the sweep line first
for(i = 0; i < SurveySize; i++)
{
if(EdgeMinY[i] <= ys && EdgeMaxY[i] > ys)
{
XIntercept2 = XIntercept1;
XIntercept1 = EvaluateLineForX(ys, Edges[i]);
}
}
//Find point to come from and point to go to
if(fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1))
{
SurveyToWP.x = XIntercept1;
SurveyToWP.y = ys;
SurveyFromWP.x = XIntercept2;
SurveyFromWP.y = ys;
}
else
{
SurveyToWP.x = XIntercept2;
SurveyToWP.y = ys;
SurveyFromWP.x = XIntercept1;
SurveyFromWP.y = ys;
}
//Find the direction to circle
if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
SurveyRadius = dSweep/2;
else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
SurveyRadius = dSweep/2;
else
SurveyRadius = -dSweep/2;
//Find the entry circle
SurveyCircle.x = SurveyFromWP.x;
SurveyCircle.y = EntryPoint.y;
//Go into entry circle state
CSurveyStatus = Entry;
LINE_STOP_FUNCTION;
}
return FALSE;
}
bool nav_line_osam_run(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
{
struct Point2D V;
struct Point2D P;
float dv;
switch (CFLStatus) {
case FLInitialize:
//Translate WPs so From_WP is origin
V.x = WaypointX(To_WP) - WaypointX(From_WP);
V.y = WaypointY(To_WP) - WaypointY(From_WP);
//Record Aircraft Position
P.x = stateGetPositionEnu_f()->x;
P.y = stateGetPositionEnu_f()->y;
//Rotate Aircraft Position so V is aligned with x axis
TranslateAndRotateFromWorld(&P, atan2f(V.y, V.x), WaypointX(From_WP), WaypointY(From_WP));
//Find which side of the flight line the aircraft is on
if (P.y > 0) {
FLRadius = -radius;
} else {
FLRadius = radius;
}
//Find unit vector of V
dv = sqrtf(V.x * V.x + V.y * V.y);
V.x = V.x / dv;
V.y = V.y / dv;
//Find begin and end points of flight line
FLFROMWP.x = -V.x * Space_Before;
FLFROMWP.y = -V.y * Space_Before;
FLTOWP.x = V.x * (dv + Space_After);
FLTOWP.y = V.y * (dv + Space_After);
//Find center of circle
FLCircle.x = FLFROMWP.x + V.y * FLRadius;
FLCircle.y = FLFROMWP.y - V.x * FLRadius;
//Find the angle to exit the circle
FLQDR = atan2f(FLFROMWP.x - FLCircle.x, FLFROMWP.y - FLCircle.y);
//Translate back
FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP);
FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP);
FLTOWP.x = FLTOWP.x + WaypointX(From_WP);
FLTOWP.y = FLTOWP.y + WaypointY(From_WP);
FLCircle.x = FLCircle.x + WaypointX(From_WP);
FLCircle.y = FLCircle.y + WaypointY(From_WP);
CFLStatus = FLCircleS;
nav_init_stage();
break;
case FLCircleS:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius);
if (NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR))) {
CFLStatus = FLLine;
LINE_START_FUNCTION;
nav_init_stage();
}
break;
case FLLine:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
nav_route_xy(FLFROMWP.x, FLFROMWP.y, FLTOWP.x, FLTOWP.y);
if (nav_approaching_xy(FLTOWP.x, FLTOWP.y, FLFROMWP.x, FLFROMWP.y, 0)) {
CFLStatus = FLFinished;
LINE_STOP_FUNCTION;
nav_init_stage();
}
break;
case FLFinished:
CFLStatus = FLInitialize;
nav_init_stage();
return false;
break;
default:
break;
}
return true;
}
bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
{
struct Point2D V;
struct Point2D P;
float dv;
switch(CFLStatus)
{
case FLInitialize:
//Translate WPs so From_WP is origin
V.x = waypoints[To_WP].x - waypoints[From_WP].x;
V.y = waypoints[To_WP].y - waypoints[From_WP].y;
//Record Aircraft Position
P.x = estimator_x;
P.y = estimator_y;
//Rotate Aircraft Position so V is aligned with x axis
TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), waypoints[From_WP].x, waypoints[From_WP].y);
//Find which side of the flight line the aircraft is on
if(P.y > 0)
FLRadius = -radius;
else
FLRadius = radius;
//Find unit vector of V
dv = sqrt(V.x*V.x+V.y*V.y);
V.x = V.x / dv;
V.y = V.y / dv;
//Find begin and end points of flight line
FLFROMWP.x = -V.x*Space_Before;
FLFROMWP.y = -V.y*Space_Before;
FLTOWP.x = V.x*(dv+Space_After);
FLTOWP.y = V.y*(dv+Space_After);
//Find center of circle
FLCircle.x = FLFROMWP.x + V.y * FLRadius;
FLCircle.y = FLFROMWP.y - V.x * FLRadius;
//Find the angle to exit the circle
FLQDR = atan2(FLFROMWP.x-FLCircle.x, FLFROMWP.y-FLCircle.y);
//Translate back
FLFROMWP.x = FLFROMWP.x + waypoints[From_WP].x;
FLFROMWP.y = FLFROMWP.y + waypoints[From_WP].y;
FLTOWP.x = FLTOWP.x + waypoints[From_WP].x;
FLTOWP.y = FLTOWP.y + waypoints[From_WP].y;
FLCircle.x = FLCircle.x + waypoints[From_WP].x;
FLCircle.y = FLCircle.y + waypoints[From_WP].y;
CFLStatus = FLCircleS;
nav_init_stage();
break;
case FLCircleS:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius);
if(NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR)))
{
CFLStatus = FLLine;
nav_init_stage();
}
break;
case FLLine:
NavVerticalAutoThrottleMode(0); /* No pitch */
//if(waypoints[From_WP].a == waypoints[To_WP].a)
// NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
//else
OSAMNavGlide(From_WP, To_WP);
nav_route_xy(FLFROMWP.x,FLFROMWP.y,FLTOWP.x,FLTOWP.y);
if(nav_approaching_xy(FLTOWP.x,FLTOWP.y,FLFROMWP.x,FLFROMWP.y, 0))
{
CFLStatus = FLFinished;
nav_init_stage();
}
break;
case FLFinished:
CFLStatus = FLInitialize;
nav_init_stage();
return FALSE;
break;
default:
break;
}
return TRUE;
}
