Example #1
0
bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
  float alt = WaypointAlt(_climb);

  nav_catapult_armed = 1;

  // No Roll, Climb Pitch, No motor Phase
  if (nav_catapult_launch <= nav_catapult_motor_delay)
  {
    NavAttitude(RadOfDeg(0));
    NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
    NavVerticalThrottleMode(9600*(0));



    // Store take-off waypoint
    WaypointX(_to) = GetPosX();
    WaypointY(_to) = GetPosY();
    WaypointAlt(_to) = GetPosAlt();

    nav_catapult_x = stateGetPositionEnu_f()->x;
    nav_catapult_y = stateGetPositionEnu_f()->y;

  }
  // No Roll, Climb Pitch, Full Power
  else if (nav_catapult_launch < nav_catapult_heading_delay)
  {
    NavAttitude(RadOfDeg(0));
    NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
    NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle));
  }
  // Normal Climb: Heading Locked by Waypoint Target
  else if (nav_catapult_launch == 0xffff)
  {
    NavVerticalAltitudeMode(alt, 0);	// vertical mode (folow glideslope)
    NavVerticalAutoThrottleMode(0);		// throttle mode
    NavGotoWaypoint(_climb);				// horizontal mode (stay on localiser)
  }
  else
  {
    // Store Heading, move Climb
    nav_catapult_launch = 0xffff;

    float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
    float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;

    float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);

    WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
    WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;

    DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
  }


return TRUE;

}	// end of gls()
Example #2
0
bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
  float alt = WaypointAlt(_climb);  

  nav_catapult_armed = 1;

  // No Roll, Climb Pitch, No motor Phase   零滚转 府仰爬行 没有电机阶段
  if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
  {
    NavAttitude(RadOfDeg(0));  //高度设置
    NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);   //自动油门模式
    NavVerticalThrottleMode(9600*(0));   //设定油门


    // Store take-off waypoint   存储起飞点
    WaypointX(_to) = GetPosX();   //获得x坐标
    WaypointY(_to) = GetPosY();   //获得y坐标
    WaypointAlt(_to) = GetPosAlt();   //获得高度

    nav_catapult_x = stateGetPositionEnu_f()->x;   //起飞点x坐标
    nav_catapult_y = stateGetPositionEnu_f()->y;   //起飞点y坐标

  }
  // No Roll, Climb Pitch, Full Power   零滚转  府仰爬行  满油门
  else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
  {
    NavAttitude(RadOfDeg(0));   //高度设置
    NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);   //自动油门模式
    NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle));   //设定油门
  }
  // Normal Climb: Heading Locked by Waypoint Target    
  // 正常爬行:锁定给定航点
  else if (nav_catapult_launch == 0xffff)
  {
    NavVerticalAltitudeMode(alt, 0);	// vertical mode (folow glideslope)  水平模式(跟随滑坡)
    NavVerticalAutoThrottleMode(0);		// throttle mode  油门模式
    NavGotoWaypoint(_climb);				// horizontal mode (stay on localiser)   垂直模式(保持定位)
  }
  else
  {
    // Store Heading, move Climb   
    nav_catapult_launch = 0xffff;

    float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
    float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;

    float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);

    WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
    WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;

    DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
  }


return TRUE;

}
Example #3
0
bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {


  if (init){

#if USE_AIRSPEED
    v_ctl_auto_airspeed_setpoint = target_speed;			// set target speed for approach
#endif
    init = FALSE;
  }


  float final_x = WaypointX(_td) - WaypointX(_tod);
  float final_y = WaypointY(_td) - WaypointY(_tod);
  float final2 = Max(final_x * final_x + final_y * final_y, 1.);

  float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
  Bound(nav_final_progress,-1,1);
  float nav_final_length = sqrt(final2);

  float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
  Bound(pre_climb, -5, 0.);

  float start_alt = WaypointAlt(_tod);
  float diff_alt = WaypointAlt(_td) - start_alt;
  float alt = start_alt + nav_final_progress * diff_alt;
  Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(v_ctl_altitude_pgain))) // to prevent climbing before intercept




  if(nav_final_progress < -0.5) {			// for smooth intercept

    NavVerticalAltitudeMode(WaypointAlt(_tod), 0);	// vertical mode (fly straigt and intercept glideslope)

    NavVerticalAutoThrottleMode(0);		// throttle mode

    NavSegment(_af, _td);				// horizontal mode (stay on localiser)
  }

  else {

    NavVerticalAltitudeMode(alt, pre_climb);	// vertical mode (folow glideslope)

    NavVerticalAutoThrottleMode(0);		// throttle mode

    NavSegment(_af, _td);				// horizontal mode (stay on localiser)
  }


return TRUE;

}	// end of gls()
Example #4
0
bool_t snav_circle1(void) {
  /* circle around CD until QDR_TD */
  NavVerticalAutoThrottleMode(0); /* No pitch */
  NavVerticalAltitudeMode(wp_cd.a, 0.);
  nav_circle_XY(wp_cd.x, wp_cd.y, d_radius);
  return(! NavQdrCloseTo(DegOfRad(qdr_td)));
}
Example #5
0
/** Navigation function on a circle
 */
static inline bool mission_nav_circle(struct _mission_circle *circle)
{
  nav_circle_XY(circle->center.center_f.x, circle->center.center_f.y, circle->radius);
  NavVerticalAutoThrottleMode(0.);
  NavVerticalAltitudeMode(circle->center.center_f.z, 0.);
  return true;
}
/** Navigation function along a path
*/
static inline bool_t mission_nav_path(struct _mission_element *el)
{
  if (el->element.mission_path.nb == 0) {
    return FALSE; // nothing to do
  }

  if (el->element.mission_path.path_idx == 0) { //first wp of path
    el->element.mission_wp.wp.wp_i = el->element.mission_path.path.path_i[0];
    if (!mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
  }

  else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path

    struct EnuCoor_i *from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]);
    struct EnuCoor_i *to_wp   = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]);

    //Check proximity and wait for t seconds in proximity circle if desired
    if (nav_approaching_from(to_wp, from_wp, CARROT)) {
      last_mission_wp = *to_wp;

      if (el->duration > 0.) {
        if (nav_check_wp_time(to_wp, el->duration)) {
          el->element.mission_path.path_idx++;
        }
      } else { el->element.mission_path.path_idx++; }
    }
    //Route Between from-to
    horizontal_mode = HORIZONTAL_MODE_ROUTE;
    nav_route(from_wp, to_wp);
    NavVerticalAutoThrottleMode(RadOfDeg(0.0));
    NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(from_wp->z), 0.);
  } else { return FALSE; } //end of path

  return TRUE;
}
Example #7
0
/** 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;
}
Example #8
0
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;
}
Example #9
0
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));
}
Example #10
0
bool snav_circle2(void)
{
  /* circle around CA until QDR_A */
  NavVerticalAutoThrottleMode(0); /* No pitch */
  NavVerticalAltitudeMode(wp_cd.a, 0.);
  nav_circle_XY(wp_ca.x, wp_ca.y, a_radius);

  return (! NavQdrCloseTo(DegOfRad(qdr_a)));
}
Example #11
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;
}
Example #12
0
/*�Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */
bool snav_on_time(float nominal_radius)
{
  nominal_radius = fabs(nominal_radius);

  float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y - wp_ca.y, stateGetPositionEnu_f()->x - wp_ca.x);
  float remaining_angle = Norm2Pi(Sign(a_radius) * (qdr_a - current_qdr));
  float remaining_time = snav_desired_tow - gps.tow / 1000.;

  /* Use the nominal airspeed if the estimated one is not realistic */
  float airspeed = stateGetAirspeed_f();
  if (airspeed < NOMINAL_AIRSPEED / 2. ||
      airspeed > 2.* NOMINAL_AIRSPEED) {
    airspeed = NOMINAL_AIRSPEED;
  }

  /* Recompute ground speeds every 10 s */
  if (ground_speed_timer == 0) {
    ground_speed_timer = 40; /* every 10s, called at 40Hz */
    compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y,
                         stateGetHorizontalWindspeed_f()->x); // Wind in NED frame
  }
  ground_speed_timer--;

  /* Time to complete the circle at nominal_radius */
  float nominal_time = 0.;

  float a;
  float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */
  /* Going one step too far */
  for (a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) {
    float qdr = current_qdr + Sign(a_radius) * a;
    ground_speed = ground_speed_of_course(qdr + Sign(a_radius) * M_PI_2);
    nominal_time += ANGLE_STEP * nominal_radius / ground_speed;
  }
  /* Removing what exceeds remaining_angle */
  nominal_time -= (a - remaining_angle) * nominal_radius / ground_speed;

  /* Radius size to finish in one single circle */
  float radius = remaining_time / nominal_time * nominal_radius;
  if (radius > 2. * nominal_radius) {
    radius = nominal_radius;
  }

  NavVerticalAutoThrottleMode(0); /* No pitch */
  NavVerticalAltitudeMode(wp_cd.a, 0.);

  radius *= Sign(a_radius);
  wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x;
  wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y;
  nav_circle_XY(wp_ca.x, wp_ca.y, radius);

  /* Stay in this mode until the end of time */
  return (remaining_time > 0);
}
Example #13
0
/** Navigation function to a single waypoint
 */
static inline bool_t mission_nav_wp(struct _mission_wp * wp) {
  if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp.x, last_wp.y, CARROT)) {
    last_wp = wp->wp; // store last wp
    return FALSE; // end of mission element
  }
  // set navigation command
  fly_to_xy(wp->wp.x, wp->wp.y);
  NavVerticalAutoThrottleMode(0.);
  NavVerticalAltitudeMode(wp->wp.z, 0.);
  return TRUE;
}
Example #14
0
/** Navigation function to a single waypoint
 */
static inline bool mission_nav_wp(struct _mission_wp *wp)
{
  if (nav_approaching_xy(wp->wp.wp_f.x, wp->wp.wp_f.y, last_wp_f.x, last_wp_f.y, CARROT)) {
    last_wp_f = wp->wp.wp_f; // store last wp
    return false; // end of mission element
  }
  // set navigation command
  fly_to_xy(wp->wp.wp_f.x, wp->wp.wp_f.y);
  NavVerticalAutoThrottleMode(0.);
  NavVerticalAltitudeMode(wp->wp.wp_f.z, 0.);
  return true;
}
Example #15
0
/** 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;
}
Example #16
0
static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
  struct ac_info_ * ac = get_ac_info(_ac_id);
  NavVerticalAutoThrottleMode(0.);
  NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.);
  float alpha = M_PI/2 - ac->course;
  float ca = cosf(alpha), sa = sinf(alpha);
  float x = ac->east - _distance*ca;
  float y = ac->north - _distance*sa;
  fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN
  float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa;
  nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s;
  nav_ground_speed_loop();
#endif
}
/** Navigation function on a circle
*/
static inline bool_t mission_nav_circle(struct _mission_element * el) {
  struct EnuCoor_i* center_wp = &(el->element.mission_circle.center.center_i);
  int32_t radius = el->element.mission_circle.radius;

  //Draw the desired circle for a 'duration' time
  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
  nav_circle(center_wp, POS_BFP_OF_REAL(radius));
  NavVerticalAutoThrottleMode(RadOfDeg(0.0));
  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(center_wp->z), 0.);

  if (el->duration > 0. && mission.element_time >= el->duration) {
    return FALSE;
  }

  return TRUE;
}
Example #18
0
void nav_follow(uint8_t ac_id, float distance, float height)
{
  struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ac_id);
  NavVerticalAutoThrottleMode(0.);
  NavVerticalAltitudeMode(Max(ac->z + height, ground_alt + SECURITY_HEIGHT), 0.);
  float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
  float ca = cosf(alpha), sa = sinf(alpha);
  float x = ac->x - distance * ca;
  float y = ac->y - distance * sa;
  fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN
  float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa;
  nav_ground_speed_setpoint = acInfoGetGspeed(ac_id) + NAV_FOLLOW_PGAIN * s;
  nav_ground_speed_loop();
#endif
}
Example #19
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 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();
    }
/** Navigation function to a single waypoint
*/
static inline bool_t mission_nav_wp(struct _mission_element *el)
{
  struct EnuCoor_i *target_wp = &(el->element.mission_wp.wp.wp_i);

  //Check proximity and wait for 'duration' seconds in proximity circle if desired
  if (nav_approaching_from(target_wp, NULL, CARROT)) {
    last_mission_wp = *target_wp;

    if (el->duration > 0.) {
      if (nav_check_wp_time(target_wp, el->duration)) { return FALSE; }
    } else { return FALSE; }

  }
  //Go to Mission Waypoint
  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
  VECT3_COPY(navigation_target, *target_wp);
  NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(target_wp->z), 0.);

  return TRUE;
}
/** Navigation function along a segment
*/
static inline bool_t mission_nav_segment(struct _mission_element *el)
{
  struct EnuCoor_i *from_wp = &(el->element.mission_segment.from.from_i);
  struct EnuCoor_i *to_wp   = &(el->element.mission_segment.to.to_i);

  //Check proximity and wait for 'duration' seconds in proximity circle if desired
  if (nav_approaching_from(to_wp, from_wp, CARROT)) {
    last_mission_wp = *to_wp;

    if (el->duration > 0.) {
      if (nav_check_wp_time(to_wp, el->duration)) { return FALSE; }
    } else { return FALSE; }
  }

  //Route Between from-to
  horizontal_mode = HORIZONTAL_MODE_ROUTE;
  nav_route(from_wp, to_wp);
  NavVerticalAutoThrottleMode(RadOfDeg(0.0));
  NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(to_wp->z), 0.);

  return TRUE;
}
Example #22
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;
}
Example #23
0
bool_t gls_run(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
{


  // set target speed for approach on final
  if (init) {
#if USE_AIRSPEED
    v_ctl_auto_airspeed_setpoint = target_speed;
#endif
    init = FALSE;
  }

  // calculate distance tod_td
  float final_x = WaypointX(_td) - WaypointX(_tod);
  float final_y = WaypointY(_td) - WaypointY(_tod);
  float final2 = Max(final_x * final_x + final_y * final_y, 1.);

  struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
  float hspeed = *stateGetHorizontalSpeedNorm_f();

  float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x +
                              (pos_enu->y - WaypointY(_tod)) * final_y) / final2;
  Bound(nav_final_progress, -1, 1);
  //  float nav_final_length = sqrt(final2);

  // calculate requiered decent rate on glideslope
  float pre_climb_glideslope = hspeed * (-tanf(app_angle));

  // calculate glideslope
  float start_alt = WaypointAlt(_tod);
  float diff_alt = WaypointAlt(_td) - start_alt;
  float alt_glideslope = start_alt + nav_final_progress * diff_alt;

  // calculate intercept
  float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2 * sd_tod_x +
                                  (pos_enu->y - WaypointY(_sd)) * 2 * sd_tod_y) /
                                 Max((sd_intercept * sd_intercept), 1.);
  Bound(nav_intercept_progress, -1, 1);
  float tmp = nav_intercept_progress * sd_intercept / gs_on_final;
  float alt_intercept = WaypointAlt(_tod) - (0.5 * app_intercept_rate * tmp * tmp);
  float pre_climb_intercept = -nav_intercept_progress * hspeed * (tanf(app_angle));

  //########################################################

  // handle the different vertical approach segments

  float pre_climb = 0.;
  float alt = 0.;

  // distance
  float f_af = sqrt((pos_enu->x - WaypointX(_af)) * (pos_enu->x - WaypointX(_af)) +
                    (pos_enu->y - WaypointY(_af)) * (pos_enu->y - WaypointY(_af)));

  if (f_af < app_distance_af_sd) { // approach fix (AF) to start descent (SD)
    alt = WaypointAlt(_af);
    pre_climb = 0.;
  } else if ((f_af > app_distance_af_sd) && (f_af < (app_distance_af_sd + sd_intercept))) {
    // start descent (SD) to intercept
    alt = alt_intercept;
    pre_climb = pre_climb_intercept;
  } else { //glideslope (intercept to touch down)
    alt = alt_glideslope;
    pre_climb = pre_climb_glideslope;
  }
  // Bound(pre_climb, -5, 0.);


  //######################### autopilot modes

  NavVerticalAltitudeMode(alt, pre_climb);  // vertical   mode (folow glideslope)
  NavVerticalAutoThrottleMode(0);   // throttle   mode
  NavSegment(_af, _td);     // horizontal mode (stay on localiser)

  return TRUE;
} // end of gls()
Example #24
0
/**
   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;
}
Example #25
0
int formation_flight(void)
{

  static uint8_t _1Hz   = 0;
  uint8_t nb = 0, i;
  float hspeed_dir = stateGetHorizontalSpeedDir_f();
  float ch = cosf(hspeed_dir);
  float sh = sinf(hspeed_dir);
  form_n = 0.;
  form_e = 0.;
  form_a = 0.;
  form_speed = stateGetHorizontalSpeedNorm_f();
  form_speed_n = form_speed * ch;
  form_speed_e = form_speed * sh;

  if (AC_ID == leader_id) {
    stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east;
    stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north;
  }
  // set info for this AC
  set_ac_info(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir,
            stateGetPositionUtm_f()->alt, form_speed, stateGetSpeedEnu_f()->z, gps.tow);

  // broadcast info
  uint8_t ac_id = AC_ID;
  uint8_t status = formation[the_acs_id[AC_ID]].status;
  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status);
  if (++_1Hz >= 4) {
    _1Hz = 0;
    DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode,
                                    &formation[the_acs_id[AC_ID]].east,
                                    &formation[the_acs_id[AC_ID]].north,
                                    &formation[the_acs_id[AC_ID]].alt);
  }
  if (formation[the_acs_id[AC_ID]].status != ACTIVE) { return FALSE; } // AC not ready

  // get leader info
  struct ac_info_ * leader = get_ac_info(leader_id);
  if (formation[the_acs_id[leader_id]].status == UNSET ||
      formation[the_acs_id[leader_id]].status == IDLE) {
    // leader not ready or not in formation
    return FALSE;
  }

  // compute slots in the right reference frame
  struct slot_ form[NB_ACS];
  float cr = 0., sr = 1.;
  if (form_mode == FORM_MODE_COURSE) {
    cr = cosf(leader->course);
    sr = sinf(leader->course);
  }
  for (i = 0; i < NB_ACS; ++i) {
    if (formation[i].status == UNSET) { continue; }
    form[i].east  = formation[i].east * sr - formation[i].north * cr;
    form[i].north = formation[i].east * cr + formation[i].north * sr;
    form[i].alt = formation[i].alt;
  }

  // compute control forces
  for (i = 0; i < NB_ACS; ++i) {
    if (the_acs[i].ac_id == AC_ID) { continue; }
    struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
    float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
    if (delta_t > FORM_CARROT) {
      // if AC not responding for too long
      formation[i].status = LOST;
      continue;
    } else {
      // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull)
      formation[i].status = ACTIVE;
      if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) {
        form_e += (ac->east  + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x)
          - (form[i].east - form[the_acs_id[AC_ID]].east);
        form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y)
          - (form[i].north - form[the_acs_id[AC_ID]].north);
        form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
        form_speed += ac->gspeed;
        //form_speed_e += ac->gspeed * sinf(ac->course);
        //form_speed_n += ac->gspeed * cosf(ac->course);
        ++nb;
      }
    }
  }
  uint8_t _nb = Max(1, nb);
  form_n /= _nb;
  form_e /= _nb;
  form_a /= _nb;
  form_speed = form_speed / (nb + 1) - stateGetHorizontalSpeedNorm_f();
  //form_speed_e = form_speed_e / (nb+1) - stateGetHorizontalSpeedNorm_f() * sh;
  //form_speed_n = form_speed_n / (nb+1) - stateGetHorizontalSpeedNorm_f() * ch;

  // set commands
  NavVerticalAutoThrottleMode(0.);

  // altitude loop
  float alt = 0.;
  if (AC_ID == leader_id) {
    alt = nav_altitude;
  } else {
    alt = leader->alt - form[the_acs_id[leader_id]].alt;
  }
  alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a;
  flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT);

  // carrot
  if (AC_ID != leader_id) {
    float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east;
    float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north;
    desired_x = leader->east  + NOMINAL_AIRSPEED * form_carrot * sinf(leader->course) + dx;
    desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy;
    // fly to desired
    fly_to_xy(desired_x, desired_y);
    desired_x = leader->east  + dx;
    desired_y = leader->north + dy;
    // lateral correction
    //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
    //float diff_course = leader->course - hspeed_dir;
    //NormRadAngle(diff_course);
    //h_ctl_roll_setpoint += coef_form_course * diff_course;
    //h_ctl_roll_setpoint += coef_form_course * diff_heading;
  }
  //BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);

  // speed loop
  if (nb > 0) {
    float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
    cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed;
    Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
    v_ctl_auto_throttle_cruise_throttle = cruise;
  }
  return TRUE;
}
Example #26
0
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;
}
Example #27
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;
}
Example #28
0
bool_t nav_line_run(uint8_t l1, uint8_t l2, float radius) {
  radius = fabs(radius);
  float alt = waypoints[l1].a;
  waypoints[l2].a = alt;

  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
  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 = { WaypointX(l1) + radius * u_y,
                         WaypointY(l1) + radius * -u_x,
                         alt  };
  struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x,
                         WaypointY(l1) + 1.732*radius * u_y,
                         alt  };
  struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
                         WaypointY(l1) + radius * u_x,
                         alt  };

  struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
                         WaypointY(l2) + radius * u_x,
                         alt  };
  struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x,
                         WaypointY(l2) + 1.732*radius * -u_y,
                         alt  };
  struct point l1_c3 = { WaypointX(l2) + radius * u_y,
                         WaypointY(l2) + radius * -u_x,
                         alt  };
  float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);

  float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);

  /* Vertical target */
  NavVerticalAutoThrottleMode(0); /* No pitch */
  NavVerticalAltitudeMode(WaypointAlt(l1), 0.);

  switch (line_status) {
  case LR12: /* From wp l2 to wp l1 */
    NavSegment(l2, l1);
    if (NavApproachingFrom(l1, l2, CARROT)) {
      line_status = LQC21;
      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: /* From wp l1 to wp l2 */
    NavSegment(l1, l2);
    if (NavApproachingFrom(l2, l1, 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();
    }
    break;
  default: /* Should not occur !!! End the pattern */
    return FALSE;
  }
  return TRUE; /* This pattern never ends */
}
/**
 * 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;
  }
}
/**
 * initializes the variables needed for the survey to start.
 *
 * @param center_wp     the waypoint defining the center of the survey-rectangle
 * @param dir_wp        the waypoint defining the orientation of the survey-rectangle
 * @param sweep_length  the length of the survey-rectangle
 * @param sweep_spacing distance between the sweeps
 * @param sweep_lines   number of sweep_lines to fly
 * @param altitude      the altitude that must be reached before the flyover starts
 */
bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
{
  zs.current_laps = 0;
  zs.pre_leave_angle = 2;

  // copy variables from the flight plan
  VECT2_COPY(zs.wp_center, waypoints[center_wp]);
  VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
  zs.altitude = altitude;

  // if turning right leave circle before angle is reached, if turning left - leave after
  if (sweep_spacing > 0) {
    zs.pre_leave_angle -= zs.pre_leave_angle;
  }

  struct FloatVect2 flight_vec;
  VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center);
  FLOAT_VECT2_NORMALIZE(flight_vec);

  // calculate the flight_angle
  zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
  zs.return_angle = zs.flight_angle + 180;
  if (zs.return_angle > 359) {
    zs.return_angle -= 360;
  }

  // calculate the vector from one flightline perpendicular to the next flightline left,
  // seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
  // (used later to move the flight pattern one flightline up for each round)
  zs.sweep_width.x = -flight_vec.y * sweep_spacing;
  zs.sweep_width.y = +flight_vec.x * sweep_spacing;

  // calculate number of laps to fly and turning radius for each end
  zs.total_laps = (sweep_lines+1)/2;
  zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5;
  zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5;

  struct FloatVect2 flight_line;
  VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);

  //CALCULATE THE NAVIGATION POINTS
  //start and end of flight-line in flight-direction
  VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
  VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);

  struct FloatVect2 sweep_span;
  VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1);
  //start and end of flight-line in return-direction
  VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
  VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);

  //turn-centers at both ends
  zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
  zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
  zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0;
  zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0;

  //fast climbing to desired altitude
  NavVerticalAutoThrottleMode(100.0);
  NavVerticalAltitudeMode(zs.altitude, 0.0);

  zs.stage = Z_ENTRY;

  return FALSE;
}