C++ math::min方法代码示例

void
RTL::set_rtl_item()
{
	_navigator->set_can_loiter_at_sp(false);

	const home_position_s &home = *_navigator->get_home_position();
	const vehicle_global_position_s &gpos = *_navigator->get_global_position();

	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {

			// check if we are pretty close to home already
			const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);

			// if we are close to home we do not climb as high, otherwise we climb to return alt
			float climb_alt = home.alt + _param_return_alt.get();

			// we are close to home, limit climb to min
			if (home_dist < _param_rtl_min_dist.get()) {
				climb_alt = home.alt + _param_descend_alt.get();
			}

			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.lat = gpos.lat;
			_mission_item.lon = gpos.lon;
			_mission_item.altitude = climb_alt;
			_mission_item.altitude_is_relative = false;
			_mission_item.yaw = NAN;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
						     (int)(climb_alt), (int)(climb_alt - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_RETURN: {

			// don't change altitude
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.lat = home.lat;
			_mission_item.lon = home.lon;
			_mission_item.altitude = gpos.alt;
			_mission_item.altitude_is_relative = false;

			// use home yaw if close to home
			/* check if we are pretty close to home already */
			const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);

			if (home_dist < _param_rtl_min_dist.get()) {
				_mission_item.yaw = home.yaw;

			} else {
				// use current heading to home
				_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, home.lat, home.lon);
			}

			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
						     (int)(_mission_item.altitude), (int)(_mission_item.altitude - home.alt));

			break;
		}

	case RTL_STATE_TRANSITION_TO_MC: {
			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
			break;
		}

	case RTL_STATE_DESCEND: {
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.lat = home.lat;
			_mission_item.lon = home.lon;
			_mission_item.altitude = min(home.alt + _param_descend_alt.get(), gpos.alt);
			_mission_item.altitude_is_relative = false;

			// except for vtol which might be still off here and should point towards this location
			const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);

			if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
				_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);

			} else {
				_mission_item.yaw = home.yaw;
			}

			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

//.........这里部分代码省略.........

void TECS::_update_pitch_setpoint()
{
	/*
	 * The SKE_weighting variable controls how speed and height control are prioritised by the pitch demand calculation.
	 * A weighting of 1 givea equal speed and height priority
	 * A weighting of 0 gives 100% priority to height control and must be used when no airspeed measurement is available.
	 * A weighting of 2 provides 100% priority to speed control and is used when:
	 * a) an underspeed condition is detected.
	 * b) during climbout where a minimum pitch angle has been set to ensure height is gained. If the airspeed
	 * rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
	 * The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements.
	*/

	// Calculate the weighting applied to control of specific kinetic energy error
	float SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);

	if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
		SKE_weighting = 2.0f;

	} else if (!airspeed_sensor_enabled()) {
		SKE_weighting = 0.0f;
	}

	// Calculate the weighting applied to control of specific potential energy error
	float SPE_weighting = 2.0f - SKE_weighting;

	// Calculate the specific energy balance demand which specifies how the available total
	// energy should be allocated to speed (kinetic energy) and height (potential energy)
	float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting;

	// Calculate the specific energy balance rate demand
	float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting;

	// Calculate the specific energy balance and balance rate error
	_SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting);
	_SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * SPE_weighting - _SKE_rate * SKE_weighting);

	// Calculate derivative from change in climb angle to rate of change of specific energy balance
	float climb_angle_to_SEB_rate = _tas_state * _pitch_time_constant * CONSTANTS_ONE_G;

	// Calculate pitch integrator input term
	float pitch_integ_input = _SEB_error * _integrator_gain;

	// Prevent the integrator changing in a direction that will increase pitch demand saturation
	// Decay the integrator at the control loop time constant if the pitch demand from the previous time step is saturated
	if (_pitch_setpoint_unc > _pitch_setpoint_max) {
		pitch_integ_input = min(pitch_integ_input,
					min((_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));

	} else if (_pitch_setpoint_unc < _pitch_setpoint_min) {
		pitch_integ_input = max(pitch_integ_input,
					max((_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f));
	}

	// Update the pitch integrator state
	_pitch_integ_state = _pitch_integ_state + pitch_integ_input * _dt;

	// Calculate a specific energy correction that doesn't include the integrator contribution
	float SEB_correction = _SEB_error + _SEB_rate_error * _pitch_damping_gain + SEB_rate_setpoint * _pitch_time_constant;

	// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
	// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
	// having to catch up before the nose can be raised to reduce excess speed during climbout.
	if (_climbout_mode_active) {
		SEB_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate;
	}

	// Sum the correction terms and convert to a pitch angle demand. This calculation assumes:
	// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
	// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
	// pitch transients due to control action or turbulence.
	_pitch_setpoint_unc = (SEB_correction + _pitch_integ_state) / climb_angle_to_SEB_rate;
	_pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);

	// Comply with the specified vertical acceleration limit by applying a pitch rate limit
	float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;

	if ((_pitch_setpoint - _last_pitch_setpoint) > ptchRateIncr) {
		_pitch_setpoint = _last_pitch_setpoint + ptchRateIncr;

	} else if ((_pitch_setpoint - _last_pitch_setpoint) < -ptchRateIncr) {
		_pitch_setpoint = _last_pitch_setpoint - ptchRateIncr;
	}

	_last_pitch_setpoint = _pitch_setpoint;
}