关于固定翼位置控制

关于固定翼位置控制

固定翼的位置控制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;

}

  • 0
    点赞
  • 8
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值