コード例 #1
0
ファイル: mission_fw_nav.c プロジェクト: amtcvx/paparazzi
/** 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;
}
コード例 #2
0
ファイル: discsurvey.c プロジェクト: ERAUBB/paparazzi
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;
}
コード例 #3
0
ファイル: snav.c プロジェクト: AntoineBlais/paparazzi
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));
}
コード例 #4
0
/** 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;
}
コード例 #5
0
ファイル: mission_fw_nav.c プロジェクト: amtcvx/paparazzi
/** 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;
}
コード例 #6
0
ファイル: nav_survey_disc.c プロジェクト: 2seasuav/paparuzzi
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;
}
コード例 #7
0
ファイル: nav_line_osam.c プロジェクト: 2seasuav/paparuzzi
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;

}
コード例 #8
0
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 */
}
コード例 #9
0
ファイル: nav_flower.c プロジェクト: AE4317group07/paparazzi
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;
}
コード例 #10
0
ファイル: OSAMNav.c プロジェクト: AntoineBlais/paparazzi
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;
}
コード例 #11
0
ファイル: spiral.c プロジェクト: flywewill/paparazzi
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;
}
コード例 #12
0
static void nav_points(point2d start, point2d end)
{
  nav_route_xy(start.x, start.y, end.x, end.y);
}
コード例 #13
0
ファイル: nav_survey_polygon.c プロジェクト: HWal/paparazzi
static void nav_points(struct FloatVect2 start, struct FloatVect2 end)
{
    nav_route_xy(start.x, start.y, end.x, end.y);
}
コード例 #14
0
ファイル: nav.c プロジェクト: EricPoppe/paparazzi
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;
  }
}
コード例 #15
0
ファイル: spiral.c プロジェクト: ERAUBB/paparazzi
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;
}
コード例 #16
0
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;
}
コード例 #17
0
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;
}
コード例 #18
0
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;
}
コード例 #19
0
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;
}
コード例 #20
0
ファイル: nav.c プロジェクト: EricPoppe/paparazzi
/** 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 */
}
コード例 #21
0
ファイル: OSAMNav.c プロジェクト: BrandoJS/paparazzi
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;

}
コード例 #22
0
/**
 * 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;
  }
}
コード例 #23
0
ファイル: OSAMNav.c プロジェクト: AntoineBlais/paparazzi
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;
}
コード例 #24
0
ファイル: nav_spiral.c プロジェクト: 2seasuav/paparuzzi
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;
}
コード例 #25
0
ファイル: OSAMNav.c プロジェクト: AntoineBlais/paparazzi
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;
}
コード例 #26
0
ファイル: OSAMNav.c プロジェクト: AntoineBlais/paparazzi
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;
}
コード例 #27
0
ファイル: ZHAWNav_Landing.c プロジェクト: Bruzzlee/paparazzi
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;
}