/** Navigation function along a path */ static inline bool mission_nav_path(struct _mission_path *path) { if (path->nb == 0) { return false; // nothing to do } if (path->nb == 1) { // handle as a single waypoint struct _mission_wp wp; wp.wp.wp_f = path->path.path_f[0]; return mission_nav_wp(&wp); } if (path->path_idx == path->nb - 1) { last_wp_f = path->path.path_f[path->path_idx]; // store last wp return false; // end of path } // normal case struct EnuCoor_f from_f = path->path.path_f[path->path_idx]; struct EnuCoor_f to_f = path->path.path_f[path->path_idx + 1]; nav_route_xy(from_f.x, from_f.y, to_f.x, to_f.y); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(to_f.z, 0.); // both altitude should be the same anyway if (nav_approaching_xy(to_f.x, to_f.y, from_f.x, from_f.y, CARROT)) { path->path_idx++; // go to next segment } return true; }
bool_t disc_survey( uint8_t center, float radius) { float wind_dir = atan2(wind_north, wind_east) + 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: nav_circle_XY(c.x, c.y, grid*sign); if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { c1.x = estimator_x; c1.y = estimator_y; float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_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 snav_route(void) { /* Straight route from TD to TA */ NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(wp_cd.a, 0.); nav_route_xy(wp_td.x, wp_td.y, wp_ta.x, wp_ta.y); return (! nav_approaching_xy(wp_ta.x, wp_ta.y, wp_td.x, wp_td.y, CARROT)); }
/** Navigation function along a segment */ static inline bool_t mission_nav_segment(struct _mission_segment * segment) { if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)) { last_wp = segment->to; return FALSE; // end of mission element } nav_route_xy(segment->from.x, segment->from.y, segment->to.x, segment->to.y); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(segment->to.z, 0.); // both altitude should be the same anyway return TRUE; }
/** Navigation function along a segment */ static inline bool mission_nav_segment(struct _mission_segment *segment) { if (nav_approaching_xy(segment->to.to_f.x, segment->to.to_f.y, segment->from.from_f.x, segment->from.from_f.y, CARROT)) { last_wp_f = segment->to.to_f; return false; // end of mission element } nav_route_xy(segment->from.from_f.x, segment->from.from_f.y, segment->to.to_f.x, segment->to.to_f.y); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(segment->to.to_f.z, 0.); // both altitude should be the same anyway return true; }
bool nav_survey_disc_run(uint8_t center_wp, 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 */ struct FloatVect2 upwind; upwind.x = cos(wind_dir); upwind.y = sin(wind_dir); float grid = nav_survey_shift / 2; switch (disc_survey.status) { case UTURN: nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid * disc_survey.sign); if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) { disc_survey.c1.x = stateGetPositionEnu_f()->x; disc_survey.c1.y = stateGetPositionEnu_f()->y; struct FloatVect2 dist; VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]); float d = VECT2_DOT_PRODUCT(upwind, dist); if (d > radius) { disc_survey.status = DOWNWIND; } else { float w = sqrtf(radius * radius - d * d) - 1.5 * grid; struct FloatVect2 crosswind; crosswind.x = -upwind.y; crosswind.y = upwind.x; disc_survey.c2.x = waypoints[center_wp].x + d * upwind.x - w * disc_survey.sign * crosswind.x; disc_survey.c2.y = waypoints[center_wp].y + d * upwind.y - w * disc_survey.sign * crosswind.y; disc_survey.status = SEGMENT; } nav_init_stage(); } break; case DOWNWIND: disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius; disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius; disc_survey.status = SEGMENT; /* No break; */ case SEGMENT: nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y); if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) { disc_survey.c.x = disc_survey.c2.x + grid * upwind.x; disc_survey.c.y = disc_survey.c2.y + grid * upwind.y; disc_survey.sign = -disc_survey.sign; disc_survey.status = UTURN; nav_init_stage(); } break; default: break; } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */ return true; }
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; }
void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { static float survey_radius; nav_survey_active = TRUE; nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); /* Update the current segment from corners' coordinates*/ if (SurveyGoingNorth()) { survey_to.y = nav_survey_north; survey_from.y = nav_survey_south; } else if (SurveyGoingSouth()) { survey_to.y = nav_survey_south; survey_from.y = nav_survey_north; } else if (SurveyGoingEast()) { survey_to.x = nav_survey_east; survey_from.x = nav_survey_west; } else if (SurveyGoingWest()) { survey_to.x = nav_survey_west; survey_from.x = nav_survey_east; } if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */ if ((estimator_y < nav_survey_north && SurveyGoingNorth()) || (estimator_y > nav_survey_south && SurveyGoingSouth()) || (estimator_x < nav_survey_east && SurveyGoingEast()) || (estimator_x > nav_survey_west && SurveyGoingWest())) { /* Continue ... */ nav_route_xy(survey_from.x, survey_from.y, survey_to.x, survey_to.y); } else { if (survey_orientation == NS) { /* North or South limit reached, prepare U-turn and next leg */ float x0 = survey_from.x; /* Current longitude */ if (x0+nav_survey_shift < nav_survey_west || x0+nav_survey_shift > nav_survey_east) { x0 += nav_survey_shift / 2; nav_survey_shift = -nav_survey_shift; } x0 = x0 + nav_survey_shift; /* Longitude of next leg */ survey_from.x = survey_to.x = x0; /* Swap South and North extremities */ float tmp = survey_from.y; survey_from.y = survey_to.y; survey_to.y = tmp; /** Do half a circle around WP 0 */ waypoints[0].x = x0 - nav_survey_shift/2.; waypoints[0].y = survey_from.y; /* Computes the right direction for the circle */ survey_radius = nav_survey_shift / 2.; if (SurveyGoingNorth()) { survey_radius = -survey_radius; } } else { /* (survey_orientation == WE) */ /* East or West limit reached, prepare U-turn and next leg */ /* There is a y0 declared in math.h (for ARM) !!! */ float my_y0 = survey_from.y; /* Current latitude */ if (my_y0+nav_survey_shift < nav_survey_south || my_y0+nav_survey_shift > nav_survey_north) { my_y0 += nav_survey_shift / 2; nav_survey_shift = -nav_survey_shift; } my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */ survey_from.y = survey_to.y = my_y0; /* Swap West and East extremities */ float tmp = survey_from.x; survey_from.x = survey_to.x; survey_to.x = tmp; /** Do half a circle around WP 0 */ waypoints[0].x = survey_from.x; waypoints[0].y = my_y0 - nav_survey_shift/2.; /* Computes the right direction for the circle */ survey_radius = nav_survey_shift / 2.; if (SurveyGoingWest()) { survey_radius = -survey_radius; } } nav_in_segment = FALSE; survey_uturn = TRUE; LINE_STOP_FUNCTION; } } else { /* U-turn */ if ((SurveyGoingNorth() && NavCourseCloseTo(0)) || (SurveyGoingSouth() && NavCourseCloseTo(180)) || (SurveyGoingEast() && NavCourseCloseTo(90)) || (SurveyGoingWest() && NavCourseCloseTo(270))) { /* U-turn finished, back on a segment */ survey_uturn = FALSE; nav_in_circle = FALSE; LINE_START_FUNCTION; } else { NavCircleWaypoint(0, survey_radius); } } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */ }
bool_t nav_flower_run(void) { TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center); TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center); DistanceFromCenter = sqrtf(TransCurrentX * TransCurrentX + TransCurrentY * TransCurrentY); bool_t InCircle = TRUE; float CircleTheta; if (DistanceFromCenter > Flowerradius) { InCircle = FALSE; } NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(waypoints[Center].a, 0.); switch (CFlowerStatus) { case Outside: nav_route_xy(FlyFromX, FlyFromY, Fly2X, Fly2Y); if (InCircle) { CFlowerStatus = FlowerLine; FlowerTheta = atan2f(TransCurrentY, TransCurrentX); Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center); Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center); FlyFromX = stateGetPositionEnu_f()->x; FlyFromY = stateGetPositionEnu_f()->y; nav_init_stage(); } break; case FlowerLine: nav_route_xy(FlyFromX, FlyFromY, Fly2X, Fly2Y); if (!InCircle) { CFlowerStatus = Circle; CircleTheta = nav_radius / Flowerradius; CircleX = Flowerradius * cosf(FlowerTheta + 3.14 - CircleTheta) + WaypointX(Center); CircleY = Flowerradius * sinf(FlowerTheta + 3.14 - CircleTheta) + WaypointY(Center); nav_init_stage(); } break; case Circle: nav_circle_XY(CircleX, CircleY, nav_radius); if (InCircle) { EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); Flowerradius = sqrtf(EdgeCurrentX * EdgeCurrentX + EdgeCurrentY * EdgeCurrentY); if (DistanceFromCenter > Flowerradius) { CFlowerStatus = Outside; } else { CFlowerStatus = FlowerLine; } FlowerTheta = atan2f(TransCurrentY, TransCurrentX); Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center); Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center); FlyFromX = stateGetPositionEnu_f()->x; FlyFromY = stateGetPositionEnu_f()->y; nav_init_stage(); } break; default: break; } 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_t SpiralNav(void) { TransCurrentX = estimator_x - waypoints[Center].x; TransCurrentY = estimator_y - waypoints[Center].y; DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); bool_t InCircle = TRUE; if(DistanceFromCenter > Spiralradius) InCircle = FALSE; switch(CSpiralStatus) { case Outside: //flys until center of the helix is reached an start helix nav_route_xy(FlyFromX,FlyFromY,waypoints[Center].x, waypoints[Center].y); // center reached? if (nav_approaching_xy(waypoints[Center].x, waypoints[Center].y, FlyFromX, FlyFromY, 0)) { // nadir image //dc_shutter(); CSpiralStatus = StartCircle; } break; case StartCircle: // Starts helix // storage of current coordinates // calculation needed, State switch to Circle nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad); if(DistanceFromCenter >= SRad){ LastCircleX = estimator_x; LastCircleY = estimator_y; CSpiralStatus = Circle; // Start helix //dc_Circle(360/Segmente); } break; case Circle: nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| float DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x)) + ((LastCircleY-estimator_y)*(LastCircleY-estimator_y))); float CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad))); if (CircleAlpha >= Alphalimit) { LastCircleX = estimator_x; LastCircleY = estimator_y; CSpiralStatus = IncSpiral; } break; case IncSpiral: // increasing circle radius as long as it is smaller than max helix radius if(SRad + IRad < Spiralradius) { SRad = SRad + IRad; /*if (dc_cam_tracing) { // calculating Camwinkel for camera alignment TransCurrentZ = estimator_z - ZPoint; CamAngle = atan(SRad/TransCurrentZ) * 180 / 3.14; //dc_cam_angle = CamAngle; }*/ } else { SRad = Spiralradius; // Stopps DC //dc_stop(); } CSpiralStatus = Circle; break; } return TRUE; }
static void nav_points(point2d start, point2d end) { nav_route_xy(start.x, start.y, end.x, end.y); }
static void nav_points(struct FloatVect2 start, struct FloatVect2 end) { nav_route_xy(start.x, start.y, end.x, end.y); }
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 = sqrtf(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 - atan2f(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; } }
bool_t SpiralNav(void) { TransCurrentX = estimator_x - WaypointX(Center); TransCurrentY = estimator_y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); float DistanceStartEstim; float CircleAlpha; switch(CSpiralStatus) { case Outside: //flys until center of the helix is reached an start helix nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center)); // center reached? if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) { // nadir image #ifdef DIGITAL_CAM dc_send_command(DC_SHOOT); #endif CSpiralStatus = StartCircle; } break; case StartCircle: // Starts helix // storage of current coordinates // calculation needed, State switch to Circle nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); if(DistanceFromCenter >= SRad){ LastCircleX = estimator_x; LastCircleY = estimator_y; CSpiralStatus = Circle; // Start helix #ifdef DIGITAL_CAM dc_Circle(360/Segmente); #endif } break; case Circle: { nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x)) + ((LastCircleY-estimator_y)*(LastCircleY-estimator_y))); CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad))); if (CircleAlpha >= Alphalimit) { LastCircleX = estimator_x; LastCircleY = estimator_y; CSpiralStatus = IncSpiral; } break; } case IncSpiral: // increasing circle radius as long as it is smaller than max helix radius if(SRad + IRad < Spiralradius) { SRad = SRad + IRad; #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment TransCurrentZ = estimator_z - ZPoint; dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI; } #endif } else { SRad = Spiralradius; #ifdef DIGITAL_CAM // Stopps DC dc_stop(); #endif } CSpiralStatus = Circle; break; default: break; } return TRUE; }
bool nav_bungee_takeoff_run(void) { float cross = 0.; // Get current position struct FloatVect2 pos; VECT2_ASSIGN(pos, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y); switch (CTakeoffStatus) { case Launch: // Recalculate lines if below min speed if (stateGetHorizontalSpeedNorm_f() < BUNGEE_TAKEOFF_MIN_SPEED) { compute_points_from_bungee(); } // Follow Launch Line with takeoff pitch and no throttle NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH); NavVerticalThrottleMode(0); // FIXME previously using altitude mode, maybe not wise without motors //NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.); nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y); kill_throttle = 1; // Find out if UAV has crossed the line VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point cross = VECT2_DOT_PRODUCT(pos, takeoff_dir); if (cross > 0. && stateGetHorizontalSpeedNorm_f() > BUNGEE_TAKEOFF_MIN_SPEED) { CTakeoffStatus = Throttle; kill_throttle = 0; nav_init_stage(); } else { // If not crossed stay in this status break; } // Start throttle imidiatelly case Throttle: //Follow Launch Line NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH); NavVerticalThrottleMode(MAX_PPRZ * (BUNGEE_TAKEOFF_THROTTLE)); nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y); kill_throttle = 0; if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT) #if USE_AIRSPEED && (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED) #endif ) { CTakeoffStatus = Finished; return false; } else { return true; } break; default: // Invalid status or Finished, end function return false; } return true; }
bool nav_launcher_run(void) { //Find distance from laucher float dist_x = stateGetPositionEnu_f()->x - launch_x; float dist_y = stateGetPositionEnu_f()->y - launch_y; float launch_dist = sqrtf(dist_x * dist_x + dist_y * dist_y); if (launch_dist <= 0.01) { launch_dist = 0.01; } switch (CLaunch_Status) { case L_Pitch_Nav: //Follow Launch Line NavVerticalAltitudeMode(launch_alt, 0); NavVerticalAutoThrottleMode(LAUNCHER_TAKEOFF_PITCH); NavVerticalThrottleMode(MAX_PPRZ * (1)); NavAttitude(0); kill_throttle = 0; //If the plane has been launched and has traveled for more than a specified distance, switch to line nav if (stateGetHorizontalSpeedNorm_f() > LAUNCHER_TAKEOFF_MIN_SPEED_LINE) { if (launch_dist > LAUNCHER_TAKEOFF_DISTANCE) { launch_line_x = stateGetPositionEnu_f()->x; launch_line_y = stateGetPositionEnu_f()->y; CLaunch_Status = L_Line_Nav; } } break; case L_Line_Nav: //Follow Launch Line NavVerticalAltitudeMode(launch_alt, 0); NavVerticalAutoThrottleMode(LAUNCHER_TAKEOFF_PITCH); NavVerticalThrottleMode(MAX_PPRZ * (1)); nav_route_xy(launch_x, launch_y, launch_line_x, launch_line_y); kill_throttle = 0; //If the aircraft is above a specific alt, greater than a specific speed or too far away, circle up if (((stateGetPositionUtm_f()->alt > (launch_alt - LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD)) && (stateGetHorizontalSpeedNorm_f() > LAUNCHER_TAKEOFF_MIN_SPEED_CIRCLE)) || (launch_dist > LAUNCHER_TAKEOFF_MAX_CIRCLE_DISTANCE)) { CLaunch_Status = L_CircleUp; //Find position of circle float x_1 = dist_x / launch_dist; float y_1 = dist_y / launch_dist; launch_circle.x = stateGetPositionEnu_f()->x + y_1 * LAUNCHER_TAKEOFF_CIRCLE_RADIUS; launch_circle.y = stateGetPositionEnu_f()->y - x_1 * LAUNCHER_TAKEOFF_CIRCLE_RADIUS; } break; case L_CircleUp: NavVerticalAutoThrottleMode(0); NavVerticalAltitudeMode(launch_circle_alt, 0); nav_circle_XY(launch_circle.x, launch_circle.y, LAUNCHER_TAKEOFF_CIRCLE_RADIUS); if (stateGetPositionUtm_f()->alt > (launch_circle_alt - LAUNCHER_TAKEOFF_HEIGHT_THRESHOLD)) { CLaunch_Status = L_Finished; return FALSE; } break; default: break; } return TRUE; }
bool_t nav_photogrammetry(){ struct XYPoint temp1; struct XYPoint temp2; struct XYPoint position_photogrammetry; switch(block_status){ case ENTRY: temp1.y = photo_lines_completed * photogrammetry_sidestep + photogrammetry_radius; temp1.x = photogrammetry_bb_offset + temp1.y * from_step; temp2.y = (photo_lines_completed * photogrammetry_sidestep); temp2.x = photogrammetry_bb_offset + temp1.y * from_step; if(fabs(temp1.y) > fabs(ytop_low) && top_from_below_to == TRUE) { temp1.x = fabs(ytop_low) * from_step + fabs(temp1.y-ytop_low)*top_step + photogrammetry_bb_offset; temp2.x = fabs(ytop_low) * from_step + fabs(temp1.y-ytop_low)*top_step + photogrammetry_bb_offset; } TranslateAndRotateFromPhotogrammetrytoWorld(&temp1, TransX, TransY); TranslateAndRotateFromPhotogrammetrytoWorld(&temp2, TransX, TransY); nav_circle_XY(temp1.x, temp1.y, -photogrammetry_radius); if( fabs(estimator_x - temp2.x)<10 && fabs(estimator_y - temp2.y)<10) { block_status = LINE_HEAD; LINE_START_FUNCTION; } break; case LINE_HEAD: temp1.y = (photo_lines_completed * photogrammetry_sidestep); temp1.x = photogrammetry_bb_offset + temp1.y * from_step; if(fabs(temp1.y) > fabs(ytop_low) && top_from_below_to == TRUE) { temp1.x = fabs(ytop_low) * from_step + fabs(temp1.y-ytop_low)*top_step + photogrammetry_bb_offset; } temp2.y = (photo_lines_completed * photogrammetry_sidestep); temp2.x = xmax - photogrammetry_bb_offset + temp2.y * to_step; if(fabs(temp2.y) > fabs(ytop_low) && top_from_below_to == FALSE) { temp2.x = xmax + ytop_low * to_step - photogrammetry_bb_offset + fabs(temp2.y-ytop_low) * top_step; } position_photogrammetry.x = estimator_x; position_photogrammetry.y = estimator_y; TranslateAndRotateFromWorldtoPhotogrammetry(&position_photogrammetry, TransX, TransY); TranslateAndRotateFromPhotogrammetrytoWorld(&temp1, TransX, TransY); TranslateAndRotateFromPhotogrammetrytoWorld(&temp2, TransX, TransY); nav_route_xy(temp1.x, temp1.y, temp2.x, temp2.y); if(nav_approaching_xy(temp2.x, temp2.y, temp1.x, temp1.y, 0)) { TranslateAndRotateFromWorldtoPhotogrammetry(&temp2, TransX, TransY); LINE_STOP_FUNCTION; if((fabs(temp2.y) + 3 * fabs(photogrammetry_sidestep)) > fabs(ymax)) { photo_lines_completed = photo_lines_completed + 2; final_line = TRUE; block_status = RETURN; } else if(photo_lines_completed == 0) { photo_lines_completed = photo_lines_completed + 3; block_status = RETURN; } else if(photo_lines_completed == 1) { photo_lines_completed = photo_lines_completed + 4; block_status = RETURN; } else { photo_lines_completed = photo_lines_completed + 5; block_status = RETURN; } } break; case RETURN: temp1.y = (photo_lines_completed * photogrammetry_sidestep) - photogrammetry_radius; temp1.x = xmax - photogrammetry_bb_offset + temp1.y * to_step; temp2.y = (photo_lines_completed * photogrammetry_sidestep); temp2.x = xmax - photogrammetry_bb_offset + temp1.y * to_step; if((fabs(temp1.y) -fabs(photogrammetry_radius)) > fabs(ytop_low) && //7dec: bij fabs(temp1.y) - halve radius weggehaald top_from_below_to == FALSE) { temp1.x = xmax + ytop_low * to_step - photogrammetry_bb_offset + fabs(temp2.y-ytop_low) * top_step; temp2.x = temp1.x; } TranslateAndRotateFromPhotogrammetrytoWorld(&temp1, TransX, TransY); TranslateAndRotateFromPhotogrammetrytoWorld(&temp2, TransX, TransY); nav_circle_XY(temp1.x, temp1.y, -(photogrammetry_radius)); if( fabs(estimator_x - temp2.x) <10 && fabs(estimator_y - temp2.y) <10) { block_status = LINE_TAIL; LINE_START_FUNCTION; } break; case LINE_TAIL: temp1.y = (photo_lines_completed * photogrammetry_sidestep); temp2.y = temp1.y; temp1.x = xmax - photogrammetry_bb_offset + temp1.y * to_step; temp2.x = photogrammetry_bb_offset + (temp2.y - photogrammetry_radius) * from_step; if(fabs(temp1.y) > fabs(ytop_low) && top_from_below_to == FALSE) { temp1.x = xmax + ytop_low * to_step - photogrammetry_bb_offset + fabs(temp1.y-ytop_low) * top_step; } if(fabs(temp2.y) > fabs(ytop_low) && top_from_below_to == TRUE) { temp2.x = fabs(ytop_low) * from_step + fabs(temp1.y-ytop_low) * top_step + photogrammetry_bb_offset; } TranslateAndRotateFromPhotogrammetrytoWorld(&temp1, TransX, TransY); TranslateAndRotateFromPhotogrammetrytoWorld(&temp2, TransX, TransY); nav_route_xy(temp1.x, temp1.y, temp2.x, temp2.y); if(nav_approaching_xy(temp2.x, temp2.y, temp1.x, temp1.y, 0)) { LINE_STOP_FUNCTION; if(final_line == TRUE) { return FALSE; } else if(photo_lines_completed == 3) photo_lines_completed = photo_lines_completed -2; else photo_lines_completed = photo_lines_completed -3; block_status = ENTRY; } break; } NavVerticalAltitudeMode(photogrammetry_height, 0); NavVerticalAutoThrottleMode(0); return TRUE; }
bool_t nav_line(uint8_t l1, uint8_t l2, float 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); switch (line_status) { /* LR12, LQC21, LTC2, LQC22, LR21 LQC12, LTC1, LQC11 */ case LR12: nav_route_xy(waypoints[l2].x, waypoints[l2].y, waypoints[l1].x, waypoints[l1].y); if (NavApproaching(l1, CARROT)) { line_status = LQC21; 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)) { 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: nav_route_xy(waypoints[l1].x, waypoints[l1].y,waypoints[l2].x, waypoints[l2].y); if (NavApproaching(l2, CARROT)) { line_status = LQC12; 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)) { 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; }
/** 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 = sqrtf(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 - atan2f(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 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; }
/** * main navigation routine. * This is called periodically evaluates the current * Position and stage and navigates accordingly. * * @returns TRUE until the survey is finished */ bool_t nav_survey_zamboni_run(void) { // retain altitude NavVerticalAutoThrottleMode(0.0); NavVerticalAltitudeMode(zs.altitude, 0.0); //go from center of field to end of field - (before starting the syrvey) if (zs.stage == Z_ENTRY) { nav_route_xy(zs.wp_center.x, zs.wp_center.y, zs.seg_end.x, zs.seg_end.y); if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.wp_center.x, zs.wp_center.y, CARROT)) { zs.stage = Z_TURN1; NavVerticalAutoThrottleMode(0.0); nav_init_stage(); } } //Turn from stage to return else if (zs.stage == Z_TURN1) { nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1); if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)){ // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT //calculate SEG-points for the next flyover VECT2_ADD(zs.seg_start, zs.sweep_width); VECT2_ADD(zs.seg_end, zs.sweep_width); zs.stage = Z_RET; nav_init_stage(); #ifdef DIGITAL_CAM LINE_START_FUNCTION; #endif } } //fly the segment until seg_end is reached else if (zs.stage == Z_RET) { nav_route_xy(zs.ret_start.x, zs.ret_start.y, zs.ret_end.x, zs.ret_end.y); if (nav_approaching_xy(zs.ret_end.x, zs.ret_end.y, zs.ret_start.x, zs.ret_start.y, 0)) { zs.current_laps = zs.current_laps + 1; #ifdef DIGITAL_CAM //dc_stop(); LINE_STOP_FUNCTION; #endif zs.stage = Z_TURN2; } } //turn from stage to return else if (zs.stage == Z_TURN2) { nav_circle_XY(zs.turn_center2.x, zs.turn_center2.y, zs.turnradius2); if (NavCourseCloseTo(zs.flight_angle + zs.pre_leave_angle)) { //zs.current_laps = zs.current_laps + 1; zs.stage = Z_SEG; nav_init_stage(); #ifdef DIGITAL_CAM LINE_START_FUNCTION; #endif } //return } else if (zs.stage == Z_SEG) { nav_route_xy(zs.seg_start.x, zs.seg_start.y, zs.seg_end.x, zs.seg_end.y); if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, 0)) { // calculate the rest of the points for the next fly-over VECT2_ADD(zs.ret_start, zs.sweep_width); VECT2_ADD(zs.ret_end, zs.sweep_width); zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x)/2; zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y)/2; zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2; zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2; zs.stage = Z_TURN1; nav_init_stage(); #ifdef DIGITAL_CAM //dc_stop(); LINE_STOP_FUNCTION; #endif } } if (zs.current_laps >= zs.total_laps) { #ifdef DIGITAL_CAM LINE_STOP_FUNCTION; #endif return FALSE; } else { return TRUE; } }
bool_t BungeeTakeoff(void) { //Translate current position so Throttle point is (0,0) float Currentx = estimator_x-throttlePx; float Currenty = estimator_y-throttlePy; bool_t CurrentAboveLine; float ThrottleB; switch(CTakeoffStatus) { case Launch: //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 if(estimator_hspeed_mod < Takeoff_MinSpeed) { initialx = estimator_x; initialy = estimator_y; //Translate initial position so that the position of the bungee is (0,0) Currentx = initialx-(waypoints[BungeeWaypoint].x); Currenty = initialy-(waypoints[BungeeWaypoint].y); //Find Launch line slope float MLaunch = Currenty/Currentx; //Find Throttle Point (the point where the throttle line and launch line intersect) if(Currentx < 0) throttlePx = TDistance/sqrt(MLaunch*MLaunch+1); else throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1)); if(Currenty < 0) throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx)); else throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx)); //Find ThrottleLine 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); } //Find out if the UAV is currently above the line if(Currenty > (ThrottleSlope*Currentx)) CurrentAboveLine = TRUE; else CurrentAboveLine = FALSE; //Find out if UAV has crossed the line if(AboveLine != CurrentAboveLine && estimator_hspeed_mod > Takeoff_MinSpeed) { 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-10) && (estimator_hspeed_mod > Takeoff_Speed)) { CTakeoffStatus = Finished; return FALSE; } else { return TRUE; } break; default: break; } return TRUE; }
bool nav_spiral_run(void) { struct EnuCoor_f pos_enu = *stateGetPositionEnu_f(); VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center); nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current); float DistanceStartEstim; float CircleAlpha; switch (nav_spiral.status) { case SpiralOutside: //flys until center of the helix is reached an start helix nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y); // center reached? if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) { // nadir image #ifdef DIGITAL_CAM dc_send_command(DC_SHOOT); #endif nav_spiral.status = SpiralStartCircle; } break; case SpiralStartCircle: // Starts helix // storage of current coordinates // calculation needed, State switch to SpiralCircle nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start); if (nav_spiral.dist_from_center >= nav_spiral.radius_start) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralCircle; // Start helix #ifdef DIGITAL_CAM dc_Circle(360 / nav_spiral.segments); #endif } break; case SpiralCircle: { nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| struct FloatVect2 pos_diff; VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu); DistanceStartEstim = float_vect2_norm(&pos_diff); CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start))); if (CircleAlpha >= nav_spiral.alpha_limit) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralInc; } break; } case SpiralInc: // increasing circle radius as long as it is smaller than max helix radius if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) { nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment; #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z; dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180 / M_PI; } #endif } else { nav_spiral.radius_start = nav_spiral.radius; #ifdef DIGITAL_CAM // Stopps DC dc_stop(); #endif } nav_spiral.status = SpiralCircle; break; default: break; } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */ return true; }
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; }
bool_t FlowerNav(void) { TransCurrentX = estimator_x - waypoints[Center].x; TransCurrentY = estimator_y - waypoints[Center].y; DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); bool_t InCircle = TRUE; float CircleTheta; if(DistanceFromCenter > Flowerradius) InCircle = FALSE; NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(waypoints[Center].a, 0.); switch(CFlowerStatus) { case Outside: nav_route_xy(FlyFromX,FlyFromY,Fly2X,Fly2Y); if(InCircle) { CFlowerStatus = FlowerLine; FlowerTheta = atan2(TransCurrentY,TransCurrentX); Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x; Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y; FlyFromX = estimator_x; FlyFromY = estimator_y; nav_init_stage(); } break; case FlowerLine: nav_route_xy(FlyFromX,FlyFromY,Fly2X,Fly2Y); if(!InCircle) { CFlowerStatus = Circle; CircleTheta = nav_radius/Flowerradius; CircleX = Flowerradius*cos(FlowerTheta+3.14-CircleTheta)+waypoints[Center].x; CircleY = Flowerradius*sin(FlowerTheta+3.14-CircleTheta)+waypoints[Center].y; nav_init_stage(); } break; case Circle: nav_circle_XY(CircleX, CircleY, nav_radius); if(InCircle) { EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x; EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y; Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); if(DistanceFromCenter > Flowerradius) CFlowerStatus = Outside; else CFlowerStatus = FlowerLine; FlowerTheta = atan2(TransCurrentY,TransCurrentX); Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x; Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y; FlyFromX = estimator_x; FlyFromY = estimator_y; nav_init_stage(); } break; } return TRUE; }
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; }