关于固定翼位置控制
固定翼的位置控制control_position()函数中采用了L1的航点制导算法，l1算法中有3个选点的策略，
在位置控制的函数中用到了当前的期望航点，上一个航点，地速)
bool
FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
{
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);//对dt时间做了约束
_control_position_last_called = now;
_l1_control.set_dt(dt); //L1控制算法中需要dt， 怎么用的需要研究
//在固定翼模式下运行位置控制器，而且不能是旋翼固定翼相互转换的时候
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
_control_mode_current = FW_POSCTRL_MODE_OTHER;
return false;
}
bool setpoint = true;
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder默认情况下， 我们不想让偏航由方向舵直接控制。
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // 默认情况下不用flap襟翼
Vector2f nav_speed_2d{ground_speed}; //地速是两个方向的矢量，但是究竟是哪个方向
if (_airspeed_valid) {
//当风速超过最大空速时，l1导航逻辑中止使用
//根据空速航向投影计算二维地速
const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
//air_speed_2d和地速之间的夹角
const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
//如果角度>90度或地面速度小于阈值，则用空速投影代替地面速度
if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
nav_speed_2d = air_speed_2d;
}
}
/* no throttle limit as default */默认无油门限制
float throttle_max = 1.0f;
/节省飞机在空中飞行的时间/
if (!_was_in_air && !_landed) {
_was_in_air = true;
_time_went_in_air = now;
_takeoff_ground_alt = _current_altitude;
}
/* 飞机已经着落了，将_was_in_air标志复位 */
if (_landed) {
_was_in_air = false;
}
/* 复位积分，如果刚从其它模式(并非位置控制)切换到这个模式 */
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
/* 复位积分 */
_tecs.reset_state();
}
if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr.valid) {
/* 自主飞行 */
_control_mode_current = FW_POSCTRL_MODE_AUTO;
/*复位hold的高度 */
_hold_alt = _current_altitude;
/* 复位hold的航向 */
_hdg_hold_yaw = _yaw;
/*获得是否为绕圈模式 */
bool was_circle_mode = _l1_control.circle_mode();
/*在间歇性变化的情况下（例如在着陆处理中），恢复TECS参数*/
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
Vector2d curr_wp{0, 0};
Vector2d prev_wp{0, 0};
//当飞机从旋翼转换到固定翼时, 也用到了l1制导算法，以便让飞机能直着出去， 当转换模式在、结束后飞机就开始绕圈了。
if (_vehicle_status.in_transition_to_fw) {
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
//转换过程中，在车辆前方创建一个虚拟航路点HDG_HOLD_DIST_NEXT(3000)米，L1控制器可以跟踪。
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition, &lon_transition);
_transition_waypoint(0) = lat_transition;
_transition_waypoint(1) = lon_transition;
}
curr_wp = prev_wp = _transition_waypoint;
} else {
//目前飞机正在前往的航点。
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
//如果上一个航点有效，那么我们就用上一个航点到L1制导算法中。
if (pos_sp_prev.valid) {
prev_wp(0) = pos_sp_prev.lat;
prev_wp(1) = pos_sp_prev.lon;
} else {
//没有有效的先前航路点，就直接使用当前wp作为上一个航路点，以便满足l1控制律要求。 这由L1库自动处理。
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
}
//在进入前过渡前，我们需要重置过渡航路点
_transition_waypoint(0) = static_cast(NAN);
_transition_waypoint(1) = static_cast(NAN);
}
/将姿态控制器积分器复位标志初始化为0/
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
//获取任务的默认空速值
float mission_airspeed = _param_fw_airspd_trim.get();
//当前航点的巡航速度有效，并且不为0. 看来浮点数和0比较，只要>0.1f就可以了。
if (PX4_ISFINITE(pos_sp_curr.cruising_speed) &&
pos_sp_curr.cruising_speed > 0.1f) {
mission_airspeed = pos_sp_curr.cruising_speed;
}
//任务油门，也是从默认参数里面获取
float mission_throttle = _param_fw_thr_cruise.get();
//如果当前航点的巡航油门有效，并且这个巡航油门值大于等于0
if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
pos_sp_curr.cruising_throttle >= 0.0f) {
mission_throttle = pos_sp_curr.cruising_throttle;
}
//tecs算法运行时，固定翼最小油门，最大油门和任务油门。
float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;
if (mission_throttle < _param_fw_thr_min.get()) {
/* 在该航路点开启滑行， 当固定翼滑行期间油门减少到最小油门一下了，就把相关的参数复位到0., 我们只需要看速度就可以了，不需要再管高度了。 滑行阶段已经不需要动力了。 */
_tecs.set_speed_weight(2.0f);
tecs_fw_thr_min = 0.0;
tecs_fw_thr_max = 0.0;
tecs_fw_mission_throttle = 0.0;
} else {//固定翼飞行时的最大油门、最小油门从默认参数里 获得。
tecs_fw_thr_min = _param_fw_thr_min.get();
tecs_fw_thr_max = _param_fw_thr_max.get();
tecs_fw_mission_throttle = mission_throttle;
}
//这里的switch_distance（）函数是什么意思，我需要研究一下。
const float acc_rad = _l1_control.switch_distance(500.0f);
uint8_t position_sp_type = pos_sp_curr.type;
//这里需要处理航点的类型了， 有起飞点，航路点，悬停航路点
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// TAKEOFF: handle like a regular POSITION setpoint if already flying
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
//这里处理普通航路点和悬停航路点。
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
float dist_xy = -1.f;
float dist_z = -1.f;
const float dist = get_distance_to_point_global_wgs84(
(double)curr_wp(0), (double)curr_wp(1), pos_sp_curr.alt,
_current_latitude, _current_longitude, _current_altitude,
&dist_xy, &dist_z);
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// 通过悬停打到位置设定点高度
// 离航路点很近，但是高度误差几乎是两倍的验收半径
if ((dist >= 0.f)
&& (dist_z > 2.f * _param_fw_clmbout_diff.get())
&& (dist_xy < 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
}
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER
if ((dist >= 0.f)
&& (dist_z > 2.f * _param_fw_clmbout_diff.get())
&& (dist_xy > 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
}
}
_type = position_sp_type;
//这里处理空闲航路点。空闲意味着飞机休息，横滚油门俯仰都是0.
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust_body[0] = 0.0f;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = radians(_param_fw_psp_off.get());
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// 这个就是普通的导航航路点
float position_sp_alt = pos_sp_curr.alt;
// 高度一阶保持，优先保持高度吧(FOH)
if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF))
) {
const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
pos_sp_prev.lat, pos_sp_prev.lon);
//如果最后一个航路点在当前航路点的可接受半径内，那么这个航点就到了。
// Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one
if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
// Calculate distance to current waypoint
const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
_current_latitude, _current_longitude);
// Save distance to waypoint if it is the smallest ever achieved, however make sure that
// _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp
_min_current_sp_distance_xy = math::min(math::min(d_curr, _min_current_sp_distance_xy), d_curr_prev);
// if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt
// navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude
if (_min_current_sp_distance_xy > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
// The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
// radius around the current waypoint
const float delta_alt = (pos_sp_curr.alt - pos_sp_prev.alt);
const float grad = -delta_alt / (d_curr_prev - math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)));
const float a = pos_sp_prev.alt - grad * d_curr_prev;
position_sp_alt = a + grad * _min_current_sp_distance_xy;
}
}
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
tecs_update_pitch_throttle(now, position_sp_alt,
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
float loiter_radius = pos_sp_curr.loiter_radius;
uint8_t loiter_direction = pos_sp_curr.loiter_direction;
if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) {
loiter_radius = _param_nav_loiter_rad.get();
loiter_direction = (loiter_radius > 0) ? 1 : -1;
}
_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, nav_speed_2d);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
float alt_sp = pos_sp_curr.alt;
if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
&& _l1_control.circle_mode() && _param_fw_lnd_earlycfg.get()) {
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter altitude control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
mission_airspeed = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
_att_sp.apply_flaps = true;
}
if (in_takeoff_situation()) {
alt_sp = max(alt_sp, _takeoff_ground_alt + _param_fw_clmbout_diff.get());
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
}
if (_land_abort) {
if (pos_sp_curr.alt - _current_altitude < _param_fw_clmbout_diff.get()) {
// aborted landing complete, normal loiter over landing point
abort_landing(false);
} else {
// continue straight until vehicle has sufficient altitude
_att_sp.roll_body = 0.0f;
}
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
}
tecs_update_pitch_throttle(now, alt_sp,
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND) {
control_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
control_takeoff(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
}
/* reset landing state */
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state();
}
/* reset takeoff/launch state */
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
}
} else if (_control_mode.flag_control_velocity_enabled &&
_control_mode.flag_control_altitude_enabled) {
/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed,
heading is set to a distant waypoint */
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
_hdg_hold_yaw = _yaw;
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
/* reset setpoints from other modes (auto) otherwise we won't
* level out without new manual input */
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
float altctrl_airspeed = get_demanded_airspeed();
/* update desired altitude based on user pitch stick input */
update_desired_altitude(dt);
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
// this will only affect planes and not VTOL
float pitch_limit_min = _param_fw_p_lim_min.get();
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* throttle limiting */
throttle_max = _param_fw_thr_max.get();
if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(now, _hold_alt,
altctrl_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
_param_fw_thr_cruise.get(),
false,
pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL,
_manual_height_rate_setpoint_m_s);
/* heading control */
if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
/* heading / roll is zero, lock onto current heading */
if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;
}
/* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration
to make sure the plane does not start rolling
*/
if (in_takeoff_situation()) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = true;
}
if (_yaw_lock_engaged) {
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
/* we have a valid heading hold position, are we too close? */
float dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _hdg_hold_curr_wp.lat,
_hdg_hold_curr_wp.lon);
if (dist < HDG_HOLD_REACHED_DIST) {
get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
}
Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};
/* populate l1 control setpoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (in_takeoff_situation()) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
}
}
}
if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
}
} else if (_control_mode.flag_control_altitude_enabled) {
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
/* Get demanded airspeed */
float altctrl_airspeed = get_demanded_airspeed();
/* update desired altitude based on user pitch stick input */
update_desired_altitude(dt);
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
// this will only affect planes and not VTOL
float pitch_limit_min = _param_fw_p_lim_min.get();
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* throttle limiting */
throttle_max = _param_fw_thr_max.get();
if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(now, _hold_alt,
altctrl_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
_param_fw_thr_cruise.get(),
false,
pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL,
_manual_height_rate_setpoint_m_s);
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0;
} else {
_control_mode_current = FW_POSCTRL_MODE_OTHER;
/* do not publish the setpoint */
setpoint = false;
// reset hold altitude
_hold_alt = _current_altitude;
/* reset landing and takeoff state */
if (!_last_manual) {
reset_landing_state();
reset_takeoff_state();
}
}
/* Copy thrust output for publication */
if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
!_runway_takeoff.runwayTakeoffEnabled()) {
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons.
the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) {
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, min(get_tecs_thrust(), throttle_max));
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust_body[0] = 0.0f;
} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
} else {
/* Copy thrust and pitch values from tecs */
if (_landed) {
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max);
} else {
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
}
}
// decide when to use pitch setpoint from TECS because in some cases pitch
// setpoint is generated by other means
bool use_tecs_pitch = true;
// auto runway takeoff
use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
// flaring during landing
use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical);
// manual attitude control
use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER);
if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch();
}
if (_control_mode.flag_control_position_enabled) {
_last_manual = false;
} else {
_last_manual = true;
}
return setpoint;
}
扫一扫
4241
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
