自学教程：C++ stateGetPositionEnu_f函数代码示例

bool_t nav_flower_setup(uint8_t CenterWP, uint8_t EdgeWP){  Center = CenterWP;  Edge = EdgeWP;  EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);  EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);  Flowerradius = sqrtf(EdgeCurrentX * EdgeCurrentX + EdgeCurrentY * EdgeCurrentY);  TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);  TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);  DistanceFromCenter = sqrtf(TransCurrentX * TransCurrentX + TransCurrentY * TransCurrentY);  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;  if (DistanceFromCenter > Flowerradius) {    CFlowerStatus = Outside;  } else {    CFlowerStatus = FlowerLine;  }  CircleX = 0;  CircleY = 0;  return FALSE;}

void formation_pre_call(void){  if (leader_id == AC_ID) {    stateGetPositionEnu_f()->x -= formation[the_acs_id[AC_ID]].east;    stateGetPositionEnu_f()->y -= formation[the_acs_id[AC_ID]].north;  }}

bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments){  VECT2_COPY(nav_spiral.center, waypoints[center_wp]);    // center of the helix  nav_spiral.center.z = waypoints[center_wp].a;  nav_spiral.radius_start = radius_start;   // start radius of the helix  nav_spiral.segments = segments;  nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS;  if (nav_spiral.radius_start < nav_spiral.radius_min)    nav_spiral.radius_start = nav_spiral.radius_min;  nav_spiral.radius_increment = radius_inc;     // multiplier for increasing the spiral  struct FloatVect2 edge;  VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);  FLOAT_VECT2_NORM(nav_spiral.radius, edge);  // get a copy of the current position  struct EnuCoor_f pos_enu;  memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));  VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);  nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;  nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);  // nav_spiral.alpha_limit denotes angle, where the radius will be increased  nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments;  //current position  nav_spiral.fly_from.x = stateGetPositionEnu_f()->x;  nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;  if(nav_spiral.dist_from_center > nav_spiral.radius)    nav_spiral.status = SpiralOutside;  return FALSE;}

bool_t nav_select_touch_down(uint8_t _td){  WaypointX(_td) = stateGetPositionEnu_f()->x;  WaypointY(_td) = stateGetPositionEnu_f()->y;  WaypointAlt(_td) = stateGetPositionUtm_f()->alt;  return FALSE;}

/** /brief Decide if the UAV is approaching the current waypoint. *  Computes /a dist2_to_wp and compare it to square /a carrot. *  Return true if it is smaller. Else computes by scalar products if *  uav has not gone past waypoint. *  /a approaching_time can be negative and in this case, the UAV will *  fly after the waypoint for the given number of seconds. * *  @return true if the position (x, y) is reached */bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) {  /** distance to waypoint in x */  float pw_x = x - stateGetPositionEnu_f()->x;  /** distance to waypoint in y */  float pw_y = y - stateGetPositionEnu_f()->y;  if (approaching_time < 0.) {    // fly after the destination waypoint    float leg_x = x - from_x;    float leg_y = y - from_y;    float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));    float exceed_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); // negative value    float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;    return (scal_prod < exceed_dist);  }  else {    // fly close enough of the waypoint or cross it    dist2_to_wp = pw_x*pw_x + pw_y *pw_y;    float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());    if (dist2_to_wp < min_dist*min_dist) {      return TRUE;    }    float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;    return (scal_prod < 0.);  }}

bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {  if (chemo_sensor > last_plume_value) {    /* Move the circle in this direction */    float x = stateGetPositionEnu_f()->x - waypoints[plume].x;    float y = stateGetPositionEnu_f()->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 - (*stateGetHorizontalSpeedDir_f()));    float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));    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 = stateGetPositionEnu_f()->x;    waypoints[plume].y = stateGetPositionEnu_f()->y;    // DownlinkSendWp(plume);    last_plume_value = chemo_sensor;  }  NavCircleWaypoint(c, radius);  return TRUE;}

bool_t disc_survey_init( float grid ) {  //测绘圆形航线的初始化  nav_survey_shift = grid;  status = DOWNWIND;  sign = 1;  c1.x = stateGetPositionEnu_f()->x;  c1.y = stateGetPositionEnu_f()->y;  return FALSE;}

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_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 nav_survey_disc_setup(float grid){  nav_survey_shift = grid;  disc_survey.status = DOWNWIND;  disc_survey.sign = 1;  disc_survey.c1.x = stateGetPositionEnu_f()->x;  disc_survey.c1.y = stateGetPositionEnu_f()->y;  return false;}

bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP){  float ThrottleB;  initialx = stateGetPositionEnu_f()->x;  initialy = stateGetPositionEnu_f()->y;  BungeeWaypoint = BungeeWP;  //Takeoff_Distance can only be positive  TDistance = fabs(Takeoff_Distance);  //Translate initial position so that the position of the bungee is (0,0)  float Currentx = initialx - (WaypointX(BungeeWaypoint));  float Currenty = initialy - (WaypointY(BungeeWaypoint));  //Record bungee alt (which should be the ground alt at that point)  BungeeAlt = waypoints[BungeeWaypoint].a;  //Find Launch line slope and Throttle 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;  }  //Enable Launch Status and turn kill throttle on  CTakeoffStatus = Launch;  kill_throttle = 1;  //Translate the throttle point back  throttlePx = throttlePx + (WaypointX(BungeeWP));  throttlePy = throttlePy + (WaypointY(BungeeWP));  return FALSE;}

bool_t nav_anemotaxis_init( uint8_t c ) {  status = UTURN;  sign = 1;  struct FloatVect2* wind = stateGetHorizontalWindspeed_f();  float wind_dir = atan2(wind->x, wind->y);  waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI);  waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI);  last_plume_was_here();  return FALSE;}

void nav_init_stage( void ) {  last_x = stateGetPositionEnu_f()->x;  last_y = stateGetPositionEnu_f()->y;  stage_time = 0;  nav_circle_radians = 0;  nav_circle_radians_no_rewind = 0;  nav_in_circle = FALSE;  nav_in_segment = FALSE;  nav_shift = 0;}

/*
C++ state_add函数代码示例
C++ stateChanged函数代码示例