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; }