C++ NavVerticalAltitudeMode函数代码示例

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()

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)));
}

/** 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;
}

/** Navigation function along a path
 */
static inline bool mission_nav_path(struct _mission_path *path)
{
  if (path->nb == 0) {
    return false; // nothing to do
  }
  if (path->nb == 1) {
    // handle as a single waypoint
    struct _mission_wp wp;
    wp.wp.wp_f = path->path.path_f[0];
    return mission_nav_wp(&wp);
  }
  if (path->path_idx == path->nb - 1) {
    last_wp_f = path->path.path_f[path->path_idx]; // store last wp
    return false; // end of path
  }
  // normal case
  struct EnuCoor_f from_f = path->path.path_f[path->path_idx];
  struct EnuCoor_f to_f = path->path.path_f[path->path_idx + 1];
  nav_route_xy(from_f.x, from_f.y, to_f.x, to_f.y);
  NavVerticalAutoThrottleMode(0.);
  NavVerticalAltitudeMode(to_f.z, 0.); // both altitude should be the same anyway
  if (nav_approaching_xy(to_f.x, to_f.y, from_f.x, from_f.y, CARROT)) {
    path->path_idx++; // go to next segment
  }
  return true;
}

bool_t disc_survey( uint8_t center, float radius) {
  float wind_dir = atan2(wind_north, wind_east) + M_PI;

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

  float grid = nav_survey_shift / 2;

  switch (status) {
  case UTURN:
    nav_circle_XY(c.x, c.y, grid*sign);
    if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
      c1.x = estimator_x;
      c1.y = estimator_y;

      float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center));
      if (d > radius) {
	status = DOWNWIND;
      } else {
	float w = sqrt(radius*radius - d*d) - 1.5*grid;

	float crosswind_x = - upwind_y;
	float crosswind_y = upwind_x;

	c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
	c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;

	status = SEGMENT;
      }
      nav_init_stage();
    }
    break;

  case DOWNWIND:
    c2.x = WaypointX(center) - upwind_x * radius;
    c2.y = WaypointY(center) - upwind_y * radius;
    status = SEGMENT;
    /* No break; */

  case SEGMENT:
    nav_route_xy(c1.x, c1.y, c2.x, c2.y);
    if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
      c.x = c2.x + grid*upwind_x;
      c.y = c2.y + grid*upwind_y;

      sign = -sign;
      status = UTURN;
      nav_init_stage();
    }
    break;
  default:
    break;
  }

  NavVerticalAutoThrottleMode(0.); /* No pitch */
  NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */

  return TRUE;
}

void nav_glide(uint8_t start_wp, uint8_t wp)
{
  float start_alt = waypoints[start_wp].a;
  float diff_alt = waypoints[wp].a - start_alt;
  float alt = start_alt + nav_leg_progress * diff_alt;
  float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length;
  NavVerticalAltitudeMode(alt, pre_climb);
}

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));
}

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();   //获得ｘ坐标
    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 * 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;

}

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()

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)));
}

bool_t nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so)
{
  rectangle_survey_sweep_num = 0;
  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));
  survey_orientation = so;

  if (survey_orientation == NS) {
    if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) < fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
      survey_from.x = survey_to.x = nav_survey_west + grid / 4.;
    } else {
      survey_from.x = survey_to.x = nav_survey_east - grid / 4.;
      grid = -grid;
    }

    if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) > fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
      survey_to.y = nav_survey_south;
      survey_from.y = nav_survey_north;
    } else {
      survey_from.y = nav_survey_south;
      survey_to.y = nav_survey_north;
    }
  } else { /* survey_orientation == WE */
    if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) < fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
      survey_from.y = survey_to.y = nav_survey_south + grid / 4.;
    } else {
      survey_from.y = survey_to.y = nav_survey_north - grid / 4.;
      grid = -grid;
    }

    if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) > fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
      survey_to.x = nav_survey_west;
      survey_from.x = nav_survey_east;
    } else {
      survey_from.x = nav_survey_west;
      survey_to.x = nav_survey_east;
    }
  }
  nav_survey_shift = grid;
  survey_uturn = FALSE;
  nav_survey_rectangle_active = FALSE;

  //go to start position
  ENU_BFP_OF_REAL(survey_from_i, survey_from);
  horizontal_mode = HORIZONTAL_MODE_ROUTE;
  VECT3_COPY(navigation_target, survey_from_i);
  LINE_STOP_FUNCTION;
  NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.);
  if (survey_orientation == NS) {
    nav_set_heading_deg(0);
  } else {
    nav_set_heading_deg(90);
  }
  return FALSE;
}

/** Navigation function along a segment
 */
static inline bool_t mission_nav_segment(struct _mission_segment * segment) {
  if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)) {
    last_wp = segment->to;
    return FALSE; // end of mission element
  }
  nav_route_xy(segment->from.x, segment->from.y, segment->to.x, segment->to.y);
  NavVerticalAutoThrottleMode(0.);
  NavVerticalAltitudeMode(segment->to.z, 0.); // both altitude should be the same anyway
  return TRUE;
}

/** Navigation function 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;
}

/*�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);
}