fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 04:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_pos_control only use const position_setpoint_triplet within control_position()

Browse Source
This commit is contained in:
Daniel Agar
2016-03-03 10:15:40 +01:00
committed by Lorenz Meier
parent 2eea0af843
commit 6bb3d5f47b
1 changed files with 17 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+17 -17
View File
@@ -586,7 +586,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_mTecs(), _mTecs(),
_control_mode_current(FW_POSCTRL_MODE_OTHER) _control_mode_current(FW_POSCTRL_MODE_OTHER)
{ {
_nav_capabilities.turn_distance = 0.0f; _nav_capabilities = {};
_parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_period = param_find("FW_L1_PERIOD");
_parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
@@ -1219,7 +1219,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
/* define altitude error */ /* define altitude error */
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; float altitude_error = pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */ /* no throttle limit as default */
float throttle_max = 1.0f; float throttle_max = 1.0f;
@@ -1304,7 +1304,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll(); _att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing(); _att_sp.yaw_body = _l1_control.nav_bearing();
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, tecs_update_pitch_throttle(pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
@@ -1322,15 +1322,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_nav_capabilities.abort_landing == true) { if (_nav_capabilities.abort_landing == true) {
// if we entered loiter due to an aborted landing, demand // if we entered loiter due to an aborted landing, demand
// altitude setpoint well above landing waypoint // altitude setpoint well above landing waypoint
alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; alt_sp = pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff;
} else { } else {
// use altitude given by wapoint // use altitude given by wapoint
alt_sp = _pos_sp_triplet.current.alt; alt_sp = pos_sp_triplet.current.alt;
} }
if (in_takeoff_situation() || if (in_takeoff_situation() ||
((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff) ((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff)
&& _nav_capabilities.abort_landing == true)) { && _nav_capabilities.abort_landing == true)) {
/* limit roll motion to ensure enough lift */ /* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
@@ -1442,11 +1442,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// wait for some time, maybe we will soon get a valid estimate // wait for some time, maybe we will soon get a valid estimate
// until then just use the altitude of the landing waypoint // until then just use the altitude of the landing waypoint
if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) {
terrain_alt = _pos_sp_triplet.current.alt; terrain_alt = pos_sp_triplet.current.alt;
} else { } else {
// still no valid terrain, abort landing // still no valid terrain, abort landing
terrain_alt = _pos_sp_triplet.current.alt; terrain_alt = pos_sp_triplet.current.alt;
_nav_capabilities.abort_landing = true; _nav_capabilities.abort_landing = true;
} }
@@ -1465,13 +1465,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else { } else {
// no terrain estimation, just use landing waypoint altitude // no terrain estimation, just use landing waypoint altitude
terrain_alt = _pos_sp_triplet.current.alt; terrain_alt = pos_sp_triplet.current.alt;
} }
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = _pos_sp_triplet.previous.valid ? float L_altitude_rel = pos_sp_triplet.previous.valid ?
_pos_sp_triplet.previous.alt - terrain_alt : 0.0f; pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
bearing_lastwp_currwp, bearing_airplane_currwp); bearing_lastwp_currwp, bearing_airplane_currwp);
@@ -1567,7 +1567,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else { } else {
/* continue horizontally */ /* continue horizontally */
altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : altitude_desired_rel = pos_sp_triplet.previous.valid ? L_altitude_rel :
_global_pos.alt - terrain_alt; _global_pos.alt - terrain_alt;
} }
@@ -1619,7 +1619,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max);
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, tecs_update_pitch_throttle(pos_sp_triplet.current.alt,
calculate_target_airspeed( calculate_target_airspeed(
_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min),
eas2tas, eas2tas,
@@ -1630,7 +1630,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_cruise, _parameters.throttle_cruise,
_runway_takeoff.climbout(), _runway_takeoff.climbout(),
math::radians(_runway_takeoff.getMinPitch( math::radians(_runway_takeoff.getMinPitch(
_pos_sp_triplet.current.pitch_min, pos_sp_triplet.current.pitch_min,
10.0f, 10.0f,
_parameters.pitch_limit_min)), _parameters.pitch_limit_min)),
_global_pos.alt, _global_pos.alt,
@@ -1697,7 +1697,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* meters */ * meters */
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, tecs_update_pitch_throttle(pos_sp_triplet.current.alt,
calculate_target_airspeed(1.3f * _parameters.airspeed_min), calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_min),
@@ -1705,7 +1705,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_min, takeoff_throttle, _parameters.throttle_min, takeoff_throttle,
_parameters.throttle_cruise, _parameters.throttle_cruise,
true, true,
math::max(math::radians(_pos_sp_triplet.current.pitch_min), math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f)), math::radians(10.0f)),
_global_pos.alt, _global_pos.alt,
ground_speed, ground_speed,
@@ -1717,7 +1717,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::radians(15.0f)); math::radians(15.0f));
} else { } else {
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, tecs_update_pitch_throttle(pos_sp_triplet.current.alt,
calculate_target_airspeed(_parameters.airspeed_trim), calculate_target_airspeed(_parameters.airspeed_trim),
eas2tas, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_min),