C++ (Cpp) nav_ground_speed_loop示例

示例#1

文件： nav.c 项目： EricPoppe/paparazzi

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
}

示例#2

文件： nav.c 项目： bartremes/paparazzi

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
}