diff --git a/sw/airborne/OSAMNav.c b/sw/airborne/OSAMNav.c index 24a90377ed..992224dd3d 100644 --- a/sw/airborne/OSAMNav.c +++ b/sw/airborne/OSAMNav.c @@ -182,7 +182,7 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP) throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx)); //Find ThrottleLine - ThrottleSlope = -MLaunch; + ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2)); ThrottleB = (throttlePy - (ThrottleSlope*throttlePx)); //Determine whether the UAV is below or above the throttle line @@ -209,15 +209,15 @@ bool_t BungeeTakeoff(void) float Currenty = estimator_y-throttlePy; bool_t CurrentAboveLine; float ThrottleB; - - NavVerticalAutoThrottleMode(0); - NavVerticalAltitudeMode(BungeeAlt+Takeoff_Height, 0.); switch(CTakeoffStatus) { case Launch: - //Follow Launch Line + //Follow Launch Line + NavVerticalAutoThrottleMode(0); + NavVerticalAltitudeMode(BungeeAlt+Takeoff_Height, 0.); nav_route_xy(initialx,initialy,throttlePx,throttlePy); + kill_throttle = 1; //recalculate lines if below min speed @@ -230,7 +230,7 @@ bool_t BungeeTakeoff(void) Currentx = initialx-(waypoints[BungeeWaypoint].x); Currenty = initialy-(waypoints[BungeeWaypoint].y); - //Find Launch line slope and Throttle line slope + //Find Launch line slope float MLaunch = Currenty/Currentx; //Find Throttle Point (the point where the throttle line and launch line intersect) @@ -245,9 +245,15 @@ bool_t BungeeTakeoff(void) throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx)); //Find ThrottleLine - ThrottleSlope = -MLaunch; + ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2)); ThrottleB = (throttlePy - (ThrottleSlope*throttlePx)); + //Determine whether the UAV is below or above the throttle line + if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB)) + AboveLine = TRUE; + else + AboveLine = FALSE; + //Translate the throttle point back throttlePx = throttlePx+(waypoints[BungeeWaypoint].x); throttlePy = throttlePy+(waypoints[BungeeWaypoint].y); @@ -264,14 +270,17 @@ bool_t BungeeTakeoff(void) { CTakeoffStatus = Throttle; kill_throttle = 0; + nav_init_stage(); } break; case Throttle: //Follow Launch Line + NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH); + NavVerticalThrottleMode(9600*(1)); nav_route_xy(initialx,initialy,throttlePx,throttlePy); kill_throttle = 0; - if((estimator_z > BungeeAlt+Takeoff_Height) && (estimator_hspeed_mod > Takeoff_Speed)) + if((estimator_z > BungeeAlt+Takeoff_Height-10) && (estimator_hspeed_mod > Takeoff_Speed)) { CTakeoffStatus = Finished; return FALSE; @@ -336,7 +345,7 @@ bool_t InitializePolygonSurvey(uint8_t EntryWP, uint8_t Size, float sw, float Or CSurveyStatus = Init; //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) + if(Size <= PolygonSize && Orientation >= -90 && Orientation <= 90) { //Initialize Corners for(i = 0; i < Size; i++) @@ -690,7 +699,6 @@ 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 @@ -764,7 +772,7 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { 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)) { + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && estimator_z >= (waypoints[l1].a-10)) { line_status = LQC22; nav_init_stage(); } @@ -808,6 +816,276 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { return TRUE; /* This pattern never ends */ } +/************** SkidLanding **********************************************/ +/* +Landing Routine + */ + +enum LandingStatus { CircleDown, LandingWait, Final, Approach }; +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; + +bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius) +{ + AFWaypoint = AFWP; + TDWaypoint = TDWP; + CLandingStatus = CircleDown; + LandRadius = radius; + LandAppAlt = estimator_z; + FinalLandAltitude = Landing_FinalHeight; + FinalLandCount = 1; + waypoints[AFWaypoint].a = waypoints[TDWaypoint].a + Landing_AFHeight; + + float x_0 = waypoints[TDWaypoint].x - waypoints[AFWaypoint].x; + float y_0 = waypoints[TDWaypoint].y - waypoints[AFWaypoint].y; + + /* Unit vector from AF to TD */ + float d = sqrt(x_0*x_0+y_0*y_0); + float x_1 = x_0 / d; + float y_1 = y_0 / d; + + LandCircle.x = waypoints[AFWaypoint].x + y_1 * LandRadius; + LandCircle.y = waypoints[AFWaypoint].y - x_1 * LandRadius; + + LandCircleQDR = atan2(waypoints[AFWaypoint].x-LandCircle.x, waypoints[AFWaypoint].y-LandCircle.y); + + if(LandRadius > 0) + { + ApproachQDR = LandCircleQDR-RadOfDeg(90); + LandCircleQDR = LandCircleQDR-RadOfDeg(45); + } + else + { + ApproachQDR = LandCircleQDR+RadOfDeg(90); + LandCircleQDR = LandCircleQDR+RadOfDeg(45); + } + + + return FALSE; +} + +bool_t SkidLanding(void) +{ + switch(CLandingStatus) + { + case CircleDown: + NavVerticalAutoThrottleMode(0); /* No pitch */ + + if(NavCircleCount() < .1) + { + NavVerticalAltitudeMode(LandAppAlt, 0); + } + else + NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0); + + nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius); + + if(estimator_z < waypoints[AFWaypoint].a + 5) + { + CLandingStatus = LandingWait; + nav_init_stage(); + } + + break; + + case LandingWait: + NavVerticalAutoThrottleMode(0); /* No pitch */ + NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0); + nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius); + + if(NavCircleCount() > 0.5 && NavQdrCloseTo(DegOfRad(ApproachQDR))) + { + CLandingStatus = Approach; + nav_init_stage(); + } + break; + + case Approach: + kill_throttle = 1; + NavVerticalAutoThrottleMode(0); /* No pitch */ + NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0); + nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius); + + if(NavQdrCloseTo(DegOfRad(LandCircleQDR))) + { + CLandingStatus = Final; + nav_init_stage(); + } + break; + + case Final: + kill_throttle = 1; + NavVerticalAutoThrottleMode(0); + NavVerticalAltitudeMode(waypoints[TDWaypoint].a+FinalLandAltitude, 0); + nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y); + if(stage_time >= Landing_FinalStageTime*FinalLandCount) + { + FinalLandAltitude = FinalLandAltitude/2; + FinalLandCount++; + } + break; + + default: + + break; + } + return TRUE; +} + +enum FLStatus { FLInitialize, FLCircleS, FLLine, FLFinished }; +static enum FLStatus CFLStatus = FLInitialize; +static struct Point2D FLCircle; +static struct Point2D FLFROMWP; +static struct Point2D FLTOWP; +static float FLQDR; +static float FLRadius; + +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 */ + 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; + nav_init_stage(); + } + break; + + case FLFinished: + CFLStatus = FLInitialize; + nav_init_stage(); + return FALSE; + break; + + default: + break; + } + return TRUE; + +} + +static uint8_t FLBlockCount = 0; + +bool_t FlightLineBlock(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After) +{ + if(First_WP < Last_WP) + { + FlightLine(First_WP+FLBlockCount, First_WP+FLBlockCount+1, radius, Space_Before, Space_After); + + if(CFLStatus == FLInitialize) + { + FLBlockCount++; + if(First_WP+FLBlockCount >= Last_WP) + { + FLBlockCount = 0; + return FALSE; + } + } + } + else + { + FlightLine(First_WP-FLBlockCount, First_WP-FLBlockCount-1, radius, Space_Before, Space_After); + + if(CFLStatus == FLInitialize) + { + FLBlockCount++; + if(First_WP-FLBlockCount <= Last_WP) + { + FLBlockCount = 0; + return FALSE; + } + } + } + + return TRUE; +} + + /* Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot */ diff --git a/sw/airborne/OSAMNav.h b/sw/airborne/OSAMNav.h index 049f37aaea..ee2c61efdb 100644 --- a/sw/airborne/OSAMNav.h +++ b/sw/airborne/OSAMNav.h @@ -16,6 +16,9 @@ extern bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP); extern bool_t InitializeBungeeTakeoff(uint8_t BungeeWP); extern bool_t BungeeTakeoff(void); +extern bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius); +extern bool_t SkidLanding(void); + #define PolygonSize 10 #define MaxFloat 1000000000 #define MinFloat -1000000000 @@ -25,6 +28,12 @@ extern bool_t PolygonSurvey(void); extern uint16_t PolySurveySweepNum; extern uint16_t PolySurveySweepBackNum; +extern bool_t InitializeVerticalRaster( void ); +extern bool_t VerticalRaster(uint8_t wp1, uint8_t wp2, float radius, float AltSweep); + +extern bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After); +extern 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);