From 8fbaa333d553f9842ade2c78ccae0b176ab77f91 Mon Sep 17 00:00:00 2001 From: OSAM-UAV Team Date: Mon, 17 Aug 2009 21:34:56 +0000 Subject: [PATCH] --- sw/airborne/OSAMNav.c | 304 ++++++++++++++++++++++++++++++------------ 1 file changed, 219 insertions(+), 85 deletions(-) diff --git a/sw/airborne/OSAMNav.c b/sw/airborne/OSAMNav.c index 785f8a3e90..24a90377ed 100644 --- a/sw/airborne/OSAMNav.c +++ b/sw/airborne/OSAMNav.c @@ -294,7 +294,7 @@ bool_t BungeeTakeoff(void) enum SurveyStatus { Init, Entry, Sweep, SweepCircle }; static enum SurveyStatus CSurveyStatus; -static struct Point2D PolygonCenter; +static struct Point2D SmallestCorner; static struct Line Edges[PolygonSize]; static float EdgeMaxY[PolygonSize]; static float EdgeMinY[PolygonSize]; @@ -307,19 +307,16 @@ static struct Point2D SurveyCircle; static uint8_t SurveyEntryWP; static uint8_t SurveySize; static float SurveyCircleQdr; -static float MinY; static float MaxY; uint16_t PolySurveySweepNum; uint16_t PolySurveySweepBackNum; bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Orientation) { - PolygonCenter.x = 0; - PolygonCenter.y = 0; + SmallestCorner.x = 0; + SmallestCorner.y = 0; int i = 0; float ys = 0; - float Xpmin = MaxFloat; - float Xnmin = -MinFloat; static struct Point2D EntryPoint; float LeftYInt; float RightYInt; @@ -341,16 +338,6 @@ bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Or //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90 if(Size <= PolygonSize && Orientation >= 0 && Orientation <= 90) { - //Find Polygon Center - for(i = EntryWP; i < Size+EntryWP; i++) - { - PolygonCenter.x = PolygonCenter.x + waypoints[i].x; - PolygonCenter.y = PolygonCenter.y + waypoints[i].y; - } - - PolygonCenter.x = PolygonCenter.x/Size; - PolygonCenter.y = PolygonCenter.y/Size; - //Initialize Corners for(i = 0; i < Size; i++) { @@ -358,24 +345,36 @@ bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Or Corners[i].y = waypoints[i+EntryWP].y; } - //Rotate and Translate Corners + //Rotate Corners so sweeps are parellel with x axis for(i = 0; i < Size; i++) - TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, PolygonCenter.x, PolygonCenter.y); + 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 min and max y - MinY = 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; - - if(Corners[i].y < MinY) - MinY = Corners[i].y; } //Find polygon edges @@ -443,7 +442,7 @@ bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Or } //Find amount to increment by every sweep - if(EntryPoint.y >= 0) + if(EntryPoint.y >= MaxY/2) dSweep = -sw; else dSweep = sw; @@ -454,18 +453,6 @@ bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Or else SurveyCircleQdr = 180-DegOfRad(SurveyTheta); - //If the Sweep distance makes a circle smaller than the nav radius, the sweep distance will be multiplied by two - // and the plane will cover the sweeps inbetween on the way back - //if(Sweep/2 < nav_radius) - // dSweep = dSweep*2; - //**Didin't like it - - //Find out which way to circle - if(EntryPoint.x >= 0) - SurveyRadius = -dSweep/2; - else - SurveyRadius = dSweep/2; - //Find y value of the first sweep ys = EntryPoint.y+(dSweep/2); @@ -478,18 +465,6 @@ bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Or XIntercept1 = EvaluateLineForX(ys, Edges[i]); } } - - //Find out which intercept is smaller than the other - if(XIntercept1 > XIntercept2) - { - Xnmin = XIntercept2; - Xpmin = XIntercept1; - } - else - { - Xnmin = XIntercept1; - Xpmin = XIntercept2; - } //Find point to come from and point to go to if(fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1)) @@ -509,6 +484,14 @@ bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Or 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; @@ -528,11 +511,11 @@ bool_t PolygonSurvey(void) float ys; static struct Point2D LastPoint; int i; - float Xpmin = MaxFloat; - float Xnmin = -MinFloat; bool_t SweepingBack = FALSE; float XIntercept1 = 0; float XIntercept2 = 0; + float DInt1 = 0; + float DInt2 = 0; NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(waypoints[SurveyEntryWP].a, 0.); @@ -543,12 +526,13 @@ bool_t PolygonSurvey(void) //Rotate and translate circle point into real world C.x = SurveyCircle.x; C.y = SurveyCircle.y; - RotateAndTranslateToWorld(&C, SurveyTheta, PolygonCenter.x, PolygonCenter.y); + RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0); //follow the circle nav_circle_XY(C.x, C.y, SurveyRadius); - if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > 0 && estimator_z > waypoints[SurveyEntryWP].a-10) + if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > .1 && estimator_z > waypoints[SurveyEntryWP].a-10) { CSurveyStatus = Sweep; nav_init_stage(); @@ -560,9 +544,12 @@ bool_t PolygonSurvey(void) ToP.y = SurveyToWP.y; FromP.x = SurveyFromWP.x; FromP.y = SurveyFromWP.y; - - RotateAndTranslateToWorld(&ToP, SurveyTheta, PolygonCenter.x, PolygonCenter.y); - RotateAndTranslateToWorld(&FromP, SurveyTheta, PolygonCenter.x, PolygonCenter.y); + + RotateAndTranslateToWorld(&ToP, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&ToP, SurveyTheta, 0, 0); + + RotateAndTranslateToWorld(&FromP, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0); //follow the line nav_route_xy(FromP.x,FromP.y,ToP.x,ToP.y); @@ -571,7 +558,7 @@ bool_t PolygonSurvey(void) LastPoint.x = SurveyToWP.x; LastPoint.y = SurveyToWP.y; - if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= MinY) //Your out of the Polygon so Sweep Back + if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back { dSweep = -dSweep; ys = LastPoint.y+(dSweep/2); @@ -584,9 +571,7 @@ bool_t PolygonSurvey(void) PolySurveySweepBackNum++; } else - { - SurveyRadius = -SurveyRadius; - + { //Find y value of the first sweep ys = LastPoint.y+dSweep; } @@ -601,37 +586,60 @@ bool_t PolygonSurvey(void) } } - //Find out which intercept is small er than the other - if(XIntercept1 > XIntercept2) - { - Xnmin = XIntercept2; - Xpmin = XIntercept1; - } - else - { - Xnmin = XIntercept1; - Xpmin = XIntercept2; - } - //Find point to come from and point to go to - if(fabs(LastPoint.x - XIntercept2) <= fabs(LastPoint.x - XIntercept1)) - { - SurveyToWP.x = XIntercept1; - SurveyToWP.y = ys; + DInt1 = XIntercept1 - LastPoint.x; + DInt2 = XIntercept2 - LastPoint.x; - SurveyFromWP.x = XIntercept2; - SurveyFromWP.y = ys; + if(DInt1 * DInt2 >= 0) + { + if(fabs(DInt2) <= fabs(DInt1)) + { + 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; + } } else { - SurveyToWP.x = XIntercept2; - SurveyToWP.y = ys; + if((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) + { + SurveyToWP.x = XIntercept1; + SurveyToWP.y = ys; - SurveyFromWP.x = XIntercept1; - SurveyFromWP.y = ys; + SurveyFromWP.x = XIntercept2; + SurveyFromWP.y = ys; + } + else if((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) + { + 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; + } } - if(fabs(LastPoint.x) > fabs(SurveyFromWP.x)) + + + if(fabs(LastPoint.x-SurveyToWP.x) > fabs(SurveyFromWP.x-SurveyToWP.x)) SurveyCircle.x = LastPoint.x; else SurveyCircle.x = SurveyFromWP.x; @@ -640,8 +648,15 @@ bool_t PolygonSurvey(void) if(!SweepingBack) SurveyCircle.y = LastPoint.y+(dSweep/2); else - SurveyCircle.y = LastPoint.y; - + SurveyCircle.y = LastPoint.y; + + //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; //Go into circle state CSurveyStatus = SweepCircle; @@ -655,7 +670,8 @@ bool_t PolygonSurvey(void) //Rotate and translate circle point into real world C.x = SurveyCircle.x; C.y = SurveyCircle.y; - RotateAndTranslateToWorld(&C, SurveyTheta, PolygonCenter.x, PolygonCenter.y); + RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0); //follow the circle nav_circle_XY(C.x, C.y, SurveyRadius); @@ -674,6 +690,124 @@ bool_t PolygonSurvey(void) return TRUE; } + +/************** Vertical Raster **********************************************/ + +/** Copy of nav line. The only difference is it changes altitude every sweep, but doesn't come out of circle until +it reaches altitude. + */ +enum line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 }; +static enum line_status line_status; + +bool_t InitializeVerticalRaster( void ) { + line_status = LR12; + return FALSE; +} + +bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { + radius = fabs(radius); + float alt = waypoints[l1].a; + waypoints[l2].a = alt; + + float l2_l1_x = waypoints[l1].x - waypoints[l2].x; + float l2_l1_y = waypoints[l1].y - waypoints[l2].y; + float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y); + + /* Unit vector from l1 to l2 */ + float u_x = l2_l1_x / d; + float u_y = l2_l1_y / d; + + /* The half circle centers and the other leg */ + struct point l2_c1 = { waypoints[l1].x + radius * u_y, + waypoints[l1].y + radius * -u_x, + alt }; + struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x, + waypoints[l1].y + 1.732*radius * u_y, + alt }; + struct point l2_c3 = { waypoints[l1].x + radius * -u_y, + waypoints[l1].y + radius * u_x, + alt }; + + struct point l1_c1 = { waypoints[l2].x + radius * -u_y, + waypoints[l2].y + radius * u_x, + alt }; + struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x, + waypoints[l2].y + 1.732*radius * -u_y, + alt }; + struct point l1_c3 = { waypoints[l2].x + radius * u_y, + waypoints[l2].y + radius * -u_x, + alt }; + float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x); + + float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x); + float qdr_out_2_3 = M_PI - atan2(u_y, u_x); + + /* Vertical target */ + NavVerticalAutoThrottleMode(0); /* No pitch */ + NavVerticalAltitudeMode(WaypointAlt(l1), 0.); + + switch (line_status) { + case LR12: /* From wp l2 to wp l1 */ + NavSegment(l2, l1); + if (NavApproachingFrom(l1, l2, CARROT)) { + line_status = LQC21; + waypoints[l1].a = waypoints[l1].a+AltSweep; + nav_init_stage(); + } + break; + case LQC21: + nav_circle_XY(l2_c1.x, l2_c1.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) { + line_status = LTC2; + nav_init_stage(); + } + break; + case LTC2: + nav_circle_XY(l2_c2.x, l2_c2.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && estimator_z >= (waypoints[l1].a-5)) { + line_status = LQC22; + nav_init_stage(); + } + break; + case LQC22: + nav_circle_XY(l2_c3.x, l2_c3.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) { + line_status = LR21; + nav_init_stage(); + } + break; + case LR21: /* From wp l1 to wp l2 */ + NavSegment(l1, l2); + if (NavApproachingFrom(l2, l1, CARROT)) { + line_status = LQC12; + waypoints[l1].a = waypoints[l1].a+AltSweep; + nav_init_stage(); + } + break; + case LQC12: + nav_circle_XY(l1_c1.x, l1_c1.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) { + line_status = LTC1; + nav_init_stage(); + } + break; + case LTC1: + nav_circle_XY(l1_c2.x, l1_c2.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && estimator_z >= (waypoints[l1].a-5)) { + line_status = LQC11; + nav_init_stage(); + } + break; + case LQC11: + nav_circle_XY(l1_c3.x, l1_c3.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) { + line_status = LR12; + nav_init_stage(); + } + } + return TRUE; /* This pattern never ends */ +} + /* Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot */