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