bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {

    if (chemo_sensor > last_plume_value) {
        /* Move the circle in this direction */
        float x = estimator_x - waypoints[plume].x;
        float y = estimator_y - waypoints[plume].y;
        waypoints[c].x = waypoints[plume].x + ALPHA * x;
        waypoints[c].y = waypoints[plume].y + ALPHA * y;
        //    DownlinkSendWp(c);
        /* Turn in the right direction */
        float dir_x = cos(M_PI_2 - estimator_hspeed_dir);
        float dir_y = sin(M_PI_2 - estimator_hspeed_dir);
        float pvect = dir_x*y - dir_y*x;
        sign = (pvect > 0 ? -1 : 1);
        /* Reduce the radius */
        radius = sign * (DEFAULT_CIRCLE_RADIUS+(MAX_CHEMO-chemo_sensor)/(float)MAX_CHEMO*(MAX_RADIUS-DEFAULT_CIRCLE_RADIUS));


        /* Store this plume */
        waypoints[plume].x = estimator_x;
        waypoints[plume].y = estimator_y;
        // DownlinkSendWp(plume);
        last_plume_value = chemo_sensor;
    }

    NavCircleWaypoint(c, radius);
    return TRUE;
}
Exemple #2
0
/** \brief Home mode navigation (circle around HOME) */
void nav_home(void) {
  NavCircleWaypoint(WP_HOME, FAILSAFE_HOME_RADIUS);
  /** Nominal speed */
  nav_pitch = 0.;
  v_ctl_mode = V_CTL_MODE_AUTO_ALT;
  nav_altitude = ground_alt+HOME_MODE_HEIGHT;
  compute_dist2_to_home();
  dist2_to_wp = dist2_to_home;
  nav_set_altitude();
}
Exemple #3
0
/** 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 */
}
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 */
}
Exemple #5
0
bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
  if (chemo_sensor) {
    last_plume_was_here();
    waypoints[plume].x = stateGetPositionEnu_f()->x;
    waypoints[plume].y = stateGetPositionEnu_f()->y;
    //    DownlinkSendWp(plume);
  }

  struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
  float wind_dir = atan2(wind->x, wind->y) + M_PI;

  /** Not null even if wind_east=wind_north=0 */
  float upwind_x = cos(wind_dir);
  float upwind_y = sin(wind_dir);

  switch (status) {
  case UTURN:
    NavCircleWaypoint(c, DEFAULT_CIRCLE_RADIUS*sign);
    if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
      float crosswind_x = - upwind_y;
      float crosswind_y = upwind_x;
      waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
      waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS*upwind_y;

      float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS);

      waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
      waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;

      //      DownlinkSendWp(c1);
      //      DownlinkSendWp(c2);

      status = CROSSWIND;
      nav_init_stage();
    }
    break;

  case CROSSWIND:
    NavSegment(c1, c2);
    if (NavApproaching(c2, CARROT)) {
      waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
      waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS*upwind_y;

      // DownlinkSendWp(c);

      sign = -sign;
      status = UTURN;
      nav_init_stage();
    }

    if (chemo_sensor) {
      waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*upwind_x;
      waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*upwind_y;

      // DownlinkSendWp(c);

      sign = -sign;
      status = UTURN;
      nav_init_stage();
    }
    break;
  }
  chemo_sensor = 0;
  return TRUE;
}