/**
   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 poly_survey_adv(void)
{
  NavVerticalAutoThrottleMode(0.0);
  NavVerticalAltitudeMode(psa_altitude, 0.0);

  //entry circle around entry-center until the desired altitude is reached
  if (psa_stage == ENTRY) {
    nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
    if (NavCourseCloseTo(segment_angle)
        && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
        && fabs(estimator_z - psa_altitude) <= 20) {
      psa_stage = SEG;
      nav_init_stage();
#ifdef DIGITAL_CAM
      dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
#endif
    }
  }
  //fly the segment until seg_end is reached
  if (psa_stage == SEG) {
    nav_points(seg_start, seg_end);
    //calculate all needed points for the next flyover
    if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) {
#ifdef DIGITAL_CAM
      dc_stop();
#endif
      VEC_CALC(seg_center1, seg_end, rad_vec, -);
      ret_start.x = seg_end.x - 2*rad_vec.x;
      ret_start.y = seg_end.y - 2*rad_vec.y;

      //if we get no intersection the survey is finished
      if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec)))
        return FALSE;

      ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x;
      ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y;

      seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x);
      seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y);

      psa_stage = TURN1;
      nav_init_stage();
    }
Exemple #2
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 nav_survey_polygon_run(void)
{
    NavVerticalAutoThrottleMode(0.0);
    NavVerticalAltitudeMode(survey.psa_altitude, 0.0);

    //entry circle around entry-center until the desired altitude is reached
    if (survey.stage == ENTRY) {
        nav_circle_XY(survey.entry_center.x, survey.entry_center.y, -survey.psa_min_rad);
        if (NavCourseCloseTo(survey.segment_angle)
                && nav_approaching_xy(survey.seg_start.x, survey.seg_start.y, last_x, last_y, CARROT)
                && fabs(stateGetPositionUtm_f()->alt - survey.psa_altitude) <= 20) {
            survey.stage = SEG;
            nav_init_stage();
#ifdef DIGITAL_CAM
            dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5,
                      survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5);
#endif
        }
    }
    //fly the segment until seg_end is reached
    if (survey.stage == SEG) {
        nav_points(survey.seg_start, survey.seg_end);
        //calculate all needed points for the next flyover
        if (nav_approaching_xy(survey.seg_end.x, survey.seg_end.y, survey.seg_start.x, survey.seg_start.y, 0)) {
#ifdef DIGITAL_CAM
            dc_stop();
#endif
            VECT2_DIFF(survey.seg_center1, survey.seg_end, survey.rad_vec);
            survey.ret_start.x = survey.seg_end.x - 2 * survey.rad_vec.x;
            survey.ret_start.y = survey.seg_end.y - 2 * survey.rad_vec.y;

            //if we get no intersection the survey is finished
            static struct FloatVect2 sum_start_sweep;
            static struct FloatVect2 sum_end_sweep;
            VECT2_SUM(sum_start_sweep, survey.seg_start, survey.sweep_vec);
            VECT2_SUM(sum_end_sweep, survey.seg_end, survey.sweep_vec);
            if (!get_two_intersects(&survey.seg_start, &survey.seg_end, sum_start_sweep, sum_end_sweep)) {
                return false;
            }

            survey.ret_end.x = survey.seg_start.x - survey.sweep_vec.x - 2 * survey.rad_vec.x;
            survey.ret_end.y = survey.seg_start.y - survey.sweep_vec.y - 2 * survey.rad_vec.y;

            survey.seg_center2.x = survey.seg_start.x - 0.5 * (2.0 * survey.rad_vec.x + survey.sweep_vec.x);
            survey.seg_center2.y = survey.seg_start.y - 0.5 * (2.0 * survey.rad_vec.y + survey.sweep_vec.y);

            survey.stage = TURN1;
            nav_init_stage();
        }
    }
    //turn from stage to return
    else if (survey.stage == TURN1) {
        nav_circle_XY(survey.seg_center1.x, survey.seg_center1.y, -survey.psa_min_rad);
        if (NavCourseCloseTo(survey.return_angle)) {
            survey.stage = RET;
            nav_init_stage();
        }
        //return
    } else if (survey.stage == RET) {
        nav_points(survey.ret_start, survey.ret_end);
        if (nav_approaching_xy(survey.ret_end.x, survey.ret_end.y, survey.ret_start.x, survey.ret_start.y, 0)) {
            survey.stage = TURN2;
            nav_init_stage();
        }
        //turn from return to stage
    } else if (survey.stage == TURN2) {
        nav_circle_XY(survey.seg_center2.x, survey.seg_center2.y, -(2 * survey.psa_min_rad + survey.psa_sweep_width) * 0.5);
        if (NavCourseCloseTo(survey.segment_angle)) {
            survey.stage = SEG;
            nav_init_stage();
#ifdef DIGITAL_CAM
            dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5,
                      survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5);
#endif
        }
    }

    return true;
}
/**
   initializes the variables needed for the survey to start
   first_wp    :  the first Waypoint of the polygon
   size        :  the number of points that make up the polygon
   angle       :  angle in which to do the flyovers
   sweep_width :  distance between the sweeps
   shot_dist   :  distance between the shots
   min_rad     :  minimal radius when navigating
   altitude    :  the altitude that must be reached before the flyover starts
**/
bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude)
{
  int i;
  point2d small, sweep;
  float divider, len, angle_rad = angle/180.0*M_PI;

  if (angle < 0.0) angle += 360.0;
  if (angle >= 360.0) angle -= 360.0;

  poly_first = first_wp;
  poly_count = size;

  psa_sweep_width = sweep_width;
  psa_min_rad = min_rad;
  psa_shot_dist = shot_dist;
  psa_altitude = altitude;

  segment_angle = angle;
  return_angle = angle+180;
  if (return_angle > 359) return_angle -= 360;

  if (angle <= 45.0 || angle >= 315.0) {
    //north
    dir_vec.y = 1.0;
    dir_vec.x = 1.0*tanf(angle_rad);
    sweep.x = 1.0;
    sweep.y = - dir_vec.x / dir_vec.y;
  }
  else if (angle <= 135.0) {
    //east
    dir_vec.x = 1.0;
    dir_vec.y = 1.0/tanf(angle_rad);
    sweep.y = - 1.0;
    sweep.x = dir_vec.y / dir_vec.x;
  }
  else if (angle <= 225.0) {
    //south
    dir_vec.y = -1.0;
    dir_vec.x = -1.0*tanf(angle_rad);
    sweep.x = -1.0;
    sweep.y = dir_vec.x / dir_vec.y;
  }
  else {
    //west
    dir_vec.x = -1.0;
    dir_vec.y = -1.0/tanf(angle_rad);
    sweep.y = 1.0;
    sweep.x = - dir_vec.y / dir_vec.x;
  }

  //normalize
  len = sqrt(sweep.x*sweep.x+sweep.y*sweep.y);
  sweep.x = sweep.x / len;
  sweep.y = sweep.y / len;

  rad_vec.x = sweep.x * psa_min_rad;
  rad_vec.y = sweep.y * psa_min_rad;
  sweep_vec.x = sweep.x * psa_sweep_width;
  sweep_vec.y = sweep.y * psa_sweep_width;

  //begin at leftmost position (relative to dir_vec)
  small.x = waypoints[poly_first].x;
  small.y = waypoints[poly_first].y;

  divider = (sweep_vec.y*dir_vec.x) - (sweep_vec.x*dir_vec.y);

  //cacluate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
  if (divider < 0.0) {
    for(i=1;i<poly_count;i++)
      if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
        small.x = waypoints[poly_first+i].x;
        small.y = waypoints[poly_first+i].y;
      }
  }
  else
    for(i=1;i<poly_count;i++)
      if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
        small.x = waypoints[poly_first+i].x;
        small.y = waypoints[poly_first+i].y;
      }

  //calculate the line the defines the first flyover
  seg_start.x = small.x + 0.5*sweep_vec.x;
  seg_start.y = small.y + 0.5*sweep_vec.y;
  VEC_CALC(seg_end, seg_start, dir_vec, +);

  if (!get_two_intersects(&seg_start, &seg_end, seg_start, seg_end)) {
    psa_stage = ERR;
    return FALSE;
  }

  //center of the entry circle
  entry_center.x = seg_start.x - rad_vec.x;
  entry_center.y = seg_start.y - rad_vec.y;

  //fast climbing to desired altitude
  NavVerticalAutoThrottleMode(0.0);
  NavVerticalAltitudeMode(psa_altitude, 0.0);

  psa_stage = ENTRY;

  return FALSE;
}
Exemple #4
0
/**
 *  initializes the variables needed for the survey to start
 *  @param first_wp      the first Waypoint of the polygon
 *  @param size          the number of points that make up the polygon
 *  @param angle         angle in which to do the flyovers
 *  @param sweep_width   distance between the sweeps
 *  @param shot_dist     distance between the shots
 *  @param min_rad       minimal radius when navigating
 *  @param altitude      the altitude that must be reached before the flyover starts
 **/
void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
                              float min_rad, float altitude)
{
    int i;
    struct FloatVect2 small, sweep;
    float divider, angle_rad = angle / 180.0 * M_PI;

    if (angle < 0.0) {
        angle += 360.0;
    }
    if (angle >= 360.0) {
        angle -= 360.0;
    }

    survey.poly_first = first_wp;
    survey.poly_count = size;

    survey.psa_sweep_width = sweep_width;
    survey.psa_min_rad = min_rad;
    survey.psa_shot_dist = shot_dist;
    survey.psa_altitude = altitude;

    survey.segment_angle = angle;
    survey.return_angle = angle + 180;
    if (survey.return_angle > 359) {
        survey.return_angle -= 360;
    }

    if (angle <= 45.0 || angle >= 315.0) {
        //north
        survey.dir_vec.y = 1.0;
        survey.dir_vec.x = 1.0 * tanf(angle_rad);
        sweep.x = 1.0;
        sweep.y = - survey.dir_vec.x / survey.dir_vec.y;
    } else if (angle <= 135.0) {
        //east
        survey.dir_vec.x = 1.0;
        survey.dir_vec.y = 1.0 / tanf(angle_rad);
        sweep.y = - 1.0;
        sweep.x = survey.dir_vec.y / survey.dir_vec.x;
    } else if (angle <= 225.0) {
        //south
        survey.dir_vec.y = -1.0;
        survey.dir_vec.x = -1.0 * tanf(angle_rad);
        sweep.x = -1.0;
        sweep.y = survey.dir_vec.x / survey.dir_vec.y;
    } else {
        //west
        survey.dir_vec.x = -1.0;
        survey.dir_vec.y = -1.0 / tanf(angle_rad);
        sweep.y = 1.0;
        sweep.x = - survey.dir_vec.y / survey.dir_vec.x;
    }

    //normalize
    FLOAT_VECT2_NORMALIZE(sweep);

    VECT2_SMUL(survey.rad_vec, sweep, survey.psa_min_rad);
    VECT2_SMUL(survey.sweep_vec, sweep, survey.psa_sweep_width);

    //begin at leftmost position (relative to survey.dir_vec)
    VECT2_COPY(small, waypoints[survey.poly_first]);

    divider = (survey.sweep_vec.y * survey.dir_vec.x) - (survey.sweep_vec.x * survey.dir_vec.y);

    //calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
    if (divider < 0.0) {
        for (i = 1; i < survey.poly_count; i++)
            if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
                    (small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
                VECT2_COPY(small, waypoints[survey.poly_first + i]);
            }
    } else
        for (i = 1; i < survey.poly_count; i++)
            if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
                    (small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
                VECT2_COPY(small, waypoints[survey.poly_first + i]);
            }

    //calculate the line the defines the first flyover
    survey.seg_start.x = small.x + 0.5 * survey.sweep_vec.x;
    survey.seg_start.y = small.y + 0.5 * survey.sweep_vec.y;
    VECT2_SUM(survey.seg_end, survey.seg_start, survey.dir_vec);

    if (!get_two_intersects(&survey.seg_start, &survey.seg_end, survey.seg_start, survey.seg_end)) {
        survey.stage = ERR;
        return;
    }

    //center of the entry circle
    VECT2_DIFF(survey.entry_center, survey.seg_start, survey.rad_vec);

    //fast climbing to desired altitude
    NavVerticalAutoThrottleMode(0.0);
    NavVerticalAltitudeMode(survey.psa_altitude, 0.0);

    survey.stage = ENTRY;
}