bool_t 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 disc_survey( uint8_t center, float radius) { struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ float upwind_x = cos(wind_dir); float upwind_y = sin(wind_dir); float grid = nav_survey_shift / 2; switch (status) { case UTURN: U形弯模式 nav_circle_XY(c.x, c.y, grid*sign); if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { c1.x = stateGetPositionEnu_f()->x; c1.y = stateGetPositionEnu_f()->y; float d = ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-WaypointX(center), stateGetPositionEnu_f()->y-WaypointY(center)); if (d > radius) { status = DOWNWIND; } else { float w = sqrt(radius*radius - d*d) - 1.5*grid; float crosswind_x = - upwind_y; float crosswind_y = upwind_x; c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x; c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y; status = SEGMENT; } nav_init_stage(); } break; case DOWNWIND: //这是什么模式? c2.x = WaypointX(center) - upwind_x * radius; c2.y = WaypointY(center) - upwind_y * radius; status = SEGMENT; /* No break; */ case SEGMENT: //线段模式 nav_route_xy(c1.x, c1.y, c2.x, c2.y); if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) { c.x = c2.x + grid*upwind_x; c.y = c2.y + grid*upwind_y; sign = -sign; status = UTURN; nav_init_stage(); } break; default: break; } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */ return TRUE; }
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; }
bool nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume) { if (chemo_sensor) { last_plume_was_here(); waypoints[plume].x = stateGetPositionEnu_f()->x; waypoints[plume].y = stateGetPositionEnu_f()->y; // DownlinkSendWp(plume); } struct FloatVect2 *wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ float upwind_x = cos(wind_dir); float upwind_y = sin(wind_dir); switch (status) { case UTURN: NavCircleWaypoint(c, DEFAULT_CIRCLE_RADIUS * sign); if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) { float crosswind_x = - upwind_y; float crosswind_y = upwind_x; waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS * upwind_x; waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS * upwind_y; float width = Max(2 * ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x - last_plume.x, stateGetPositionEnu_f()->y - last_plume.y), DEFAULT_CIRCLE_RADIUS); waypoints[c2].x = waypoints[c1].x - width * crosswind_x * sign; waypoints[c2].y = waypoints[c1].y - width * crosswind_y * sign; // DownlinkSendWp(c1); // DownlinkSendWp(c2); status = CROSSWIND; nav_init_stage(); } break; case CROSSWIND: NavSegment(c1, c2); if (NavApproaching(c2, CARROT)) { waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS * upwind_x; waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS * upwind_y; // DownlinkSendWp(c); sign = -sign; status = UTURN; nav_init_stage(); } if (chemo_sensor) { waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS * upwind_x; waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS * upwind_y; // DownlinkSendWp(c); sign = -sign; status = UTURN; nav_init_stage(); } break; } chemo_sensor = 0; return true; }
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-10)) { 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 */ }
bool_t PolygonSurvey(void) { struct Point2D C; struct Point2D ToP; struct Point2D FromP; float ys; static struct Point2D LastPoint; int i; 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.); switch(CSurveyStatus) { case Entry: //Rotate and translate circle point into real world C.x = SurveyCircle.x; C.y = SurveyCircle.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() > .1 && estimator_z > waypoints[SurveyEntryWP].a-10) { CSurveyStatus = Sweep; nav_init_stage(); LINE_START_FUNCTION; } break; case Sweep: //Rotate and Translate Line points into real world ToP.x = SurveyToWP.x; ToP.y = SurveyToWP.y; FromP.x = SurveyFromWP.x; FromP.y = SurveyFromWP.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); if(nav_approaching_xy(ToP.x,ToP.y,FromP.x,FromP.y, 0)) { LastPoint.x = SurveyToWP.x; LastPoint.y = SurveyToWP.y; if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back { dSweep = -dSweep; ys = LastPoint.y+(dSweep/2); if(dSweep >= 0) SurveyCircleQdr = -DegOfRad(SurveyTheta); else SurveyCircleQdr = 180-DegOfRad(SurveyTheta); SweepingBack = TRUE; PolySurveySweepBackNum++; } else { //Find y value of the first sweep ys = LastPoint.y+dSweep; } //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 DInt1 = XIntercept1 - LastPoint.x; DInt2 = XIntercept2 - LastPoint.x; 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 { if((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) { SurveyToWP.x = XIntercept1; SurveyToWP.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-SurveyToWP.x) > fabs(SurveyFromWP.x-SurveyToWP.x)) SurveyCircle.x = LastPoint.x; else SurveyCircle.x = SurveyFromWP.x; if(!SweepingBack) SurveyCircle.y = LastPoint.y+(dSweep/2); else 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; nav_init_stage(); LINE_STOP_FUNCTION; PolySurveySweepNum++; } break; case SweepCircle: //Rotate and translate circle point into real world C.x = SurveyCircle.x; C.y = SurveyCircle.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) { CSurveyStatus = Sweep; nav_init_stage(); LINE_START_FUNCTION; } break; case Init: return FALSE; default: return FALSE; } return TRUE; }
void nav_oval(uint8_t p1, uint8_t p2, float radius) { radius = - radius; /* Historical error ? */ float alt = waypoints[p1].a; waypoints[p2].a = alt; float p2_p1_x = waypoints[p1].x - waypoints[p2].x; float p2_p1_y = waypoints[p1].y - waypoints[p2].y; float d = sqrt(p2_p1_x*p2_p1_x+p2_p1_y*p2_p1_y); /* Unit vector from p1 to p2 */ float u_x = p2_p1_x / d; float u_y = p2_p1_y / d; /* The half circle centers and the other leg */ struct point p1_center = { waypoints[p1].x + radius * -u_y, waypoints[p1].y + radius * u_x, alt }; struct point p1_out = { waypoints[p1].x + 2*radius * -u_y, waypoints[p1].y + 2*radius * u_x, alt }; struct point p2_in = { waypoints[p2].x + 2*radius * -u_y, waypoints[p2].y + 2*radius * u_x, alt }; struct point p2_center = { waypoints[p2].x + radius * -u_y, waypoints[p2].y + radius * u_x, alt }; float qdr_out_2 = M_PI - atan2(u_y, u_x); float qdr_out_1 = qdr_out_2 + M_PI; if (radius < 0) { qdr_out_2 += M_PI; qdr_out_1 += M_PI; } float qdr_anticipation = (radius > 0 ? -15 : 15); switch (oval_status) { case OC1 : nav_circle_XY(p1_center.x,p1_center.y, -radius); if (NavQdrCloseTo(DegOfRad(qdr_out_1)-qdr_anticipation)) { oval_status = OR12; InitStage(); LINE_START_FUNCTION; } return; case OR12: nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y); if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) { oval_status = OC2; nav_oval_count++; InitStage(); LINE_STOP_FUNCTION; } return; case OC2 : nav_circle_XY(p2_center.x, p2_center.y, -radius); if (NavQdrCloseTo(DegOfRad(qdr_out_2)-qdr_anticipation)) { oval_status = OR21; InitStage(); LINE_START_FUNCTION; } return; case OR21: nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y); if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) { oval_status = OC1; InitStage(); LINE_STOP_FUNCTION; } return; default: /* Should not occur !!! Doing nothing */ return; } }
/** Navigation along a figure 8. The cross center is defined by the waypoint [target], the center of one of the circles is defined by [c1]. Altitude is given by [target]. The navigation goes through 6 states: C1 (circle around [c1]), R1T, RT2 (route from circle 1 to circle 2 over [target]), C2 and R2T, RT1. If necessary, the [c1] waypoint is moved in the direction of [target] to be not far than [2*radius]. */ void nav_eight(uint8_t target, uint8_t c1, float radius) { float aradius = fabs(radius); float alt = waypoints[target].a; waypoints[c1].a = alt; float target_c1_x = waypoints[c1].x - waypoints[target].x; float target_c1_y = waypoints[c1].y - waypoints[target].y; float d = sqrt(target_c1_x*target_c1_x+target_c1_y*target_c1_y); d = Max(d, 1.); /* To prevent a division by zero */ /* Unit vector from target to c1 */ float u_x = target_c1_x / d; float u_y = target_c1_y / d; /* Move [c1] closer if needed */ if (d > 2 * aradius) { d = 2*aradius; waypoints[c1].x = waypoints[target].x + d*u_x; waypoints[c1].y = waypoints[target].y + d*u_y; } /* The other center */ struct point c2 = { waypoints[target].x - d*u_x, waypoints[target].y - d*u_y, alt }; struct point c1_in = { waypoints[c1].x + radius * -u_y, waypoints[c1].y + radius * u_x, alt }; struct point c1_out = { waypoints[c1].x - radius * -u_y, waypoints[c1].y - radius * u_x, alt }; struct point c2_in = { c2.x + radius * -u_y, c2.y + radius * u_x, alt }; struct point c2_out = { c2.x - radius * -u_y, c2.y - radius * u_x, alt }; float qdr_out = M_PI - atan2(u_y, u_x); if (radius < 0) qdr_out += M_PI; switch (eight_status) { case C1 : NavCircleWaypoint(c1, radius); if (NavQdrCloseTo(DegOfRad(qdr_out)-10)) { eight_status = R1T; InitStage(); } return; case R1T: nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y); if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) { eight_status = RT2; InitStage(); } return; case RT2: nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y); if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) { eight_status = C2; InitStage(); } return; case C2 : nav_circle_XY(c2.x, c2.y, -radius); if (NavQdrCloseTo(DegOfRad(qdr_out)+10)) { eight_status = R2T; InitStage(); } return; case R2T: nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y); if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) { eight_status = RT1; InitStage(); } return; case RT1: nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y); if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) { eight_status = C1; InitStage(); } return; default:/* Should not occur !!! Doing nothing */ return; } /* switch */ }
bool_t ZHAWSkidLanding(void) { switch(CLandingStatus) { case CircleDown: // Kreisen bis die Höhe, die im AFWaypoint vorgegeben ist erreicht ist (um den Wegpunkt der in InitializeSkidLanding berechnet wurde) 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) //Drohne hat die Höhe des AF_Waypoints erreicht. { CLandingStatus = LandingWait; nav_init_stage(); } msgLandStatus=1; break; case LandingWait: // Höhe halten und weiter um CircleCircle kreisen NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0); nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius); if(NavCircleCount() > 0.5 && NavQdrCloseTo(DegOfRad(ApproachQDR))) // Drohne nähert sich dem Winkel (bzw. Kurs) auf dem Sie fliegen muss um zu landen (45° fehlen) { CLandingStatus = ApproachHeading; nav_init_stage(); } msgLandStatus=2; break; case ApproachHeading: //Drohne fliegt den Kreis fertig, bis sie auf Landekurs ist NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0); //Sollhöhe geben nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius); if(NavQdrCloseTo(DegOfRad(LandCircleQDR))) //Drohne ist auf Landekurs und auf Höhe AF { CLandingStatus = DeclineToSonar; nav_init_stage(); set_land_params(); } msgLandStatus=3; break; case DeclineToSonar: // Sinken, bis Sonar sich meldet NavVerticalAltitudeMode(waypoints[TDWaypoint].a+Landing_SonarHeight, 0); nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y); if(sonar_dist < (Landing_SonarHeight + 0.5)) { CLandingStatus = Approach; estimator_z_mode = SONAR_HEIGHT; nav_init_stage(); AboveCheckPoint = CalculateCheckPoint(); } msgLandStatus=4; break; case Approach: //Sonar Höhe (ca. 6m) halten, bis zum FlarePoint, wo die Landung begonnen werden kann. NavVerticalAltitudeMode(Landing_SonarHeight, 0); //Sonarhöhe ÜBER BODEN!!!! nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y); //find ouf if the UAV has crossed the CheckPoint first time if (UAVcrossedCheckPoint() == 1) { CLandingStatus = Flare; nav_init_stage(); // SonarHeight = SonarHeight * Landing_FlareFactor; kill_throttle = 1; set_fixed_pitch_pitch(0.2); } msgLandStatus=5; break; case Flare: // NavVerticalAltitudeMode(Landing_SonarHeight, 0); nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y); if (estimator_z < 0.6) { float flare_pitch = 1/estimator_z*Landing_FLARE_FACTOR; set_fixed_pitch_pitch(flare_pitch); } msgLandStatus=6; break; case Stall: kill_throttle = 1; NavVerticalAltitudeMode(Landing_SonarHeight, 0); nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y); msgLandStatus=7; break; default: break; } RunOnceEvery(5, DOWNLINK_SEND_ZHAWLAND(DefaultChannel, &msgLandStatus, &estimator_z_mode, &AboveCheckPoint, &CurrentAboveCheckPoint, &SonarHeight, &estimator_z_sonar, &estimator_z)); //Failsave Height if ((estimator_z < Landing_SaveHeight) && (CLandingStatus != Flare) && (CLandingStatus != Stall)) { estimator_z_mode=GPS_HEIGHT; set_max_pitch(99); set_min_pitch(99); set_max_roll(99); return FALSE; } //Failsave CheckPoint if ((CLandingStatus == DeclineToSonar) && ( UAVcrossedCheckPoint() == 1 )) { estimator_z_mode=GPS_HEIGHT; set_max_pitch(99); set_min_pitch(99); set_max_roll(99); return FALSE; } return TRUE; }