fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 11:14:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_pos_control_l1 using math constrain, max, min, radians

Browse Source
This commit is contained in:
Daniel Agar 2017-02-07 01:28:27 -05:00 committed by Lorenz Meier
parent 8b4877a6eb
commit 474ce2851e
1 changed files with 61 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -114,6 +114,11 @@ static int _control_task = -1; /**< task handle for sensor task */
#define MANUAL_THROTTLE_CLIMBOUT_THRESH 0.85f ///< a throttle / pitch input above this value leads to the system switching to climbout mode
#define ALTHOLD_EPV_RESET_THRESH 5.0f
using math::constrain;
using math::max;
using math::min;
using math::radians;
using matrix::Eulerf;
using matrix::Quatf;
@ -735,13 +740,13 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad);
_parameters.man_roll_max_rad = math::radians(_parameters.man_roll_max_rad);
_parameters.man_roll_max_rad = radians(_parameters.man_roll_max_rad);
param_get(_parameter_handles.man_pitch_max_deg, &_parameters.man_pitch_max_rad);
_parameters.man_pitch_max_rad = math::radians(_parameters.man_pitch_max_rad);
_parameters.man_pitch_max_rad = radians(_parameters.man_pitch_max_rad);
param_get(_parameter_handles.rollsp_offset_deg, &_parameters.rollsp_offset_rad);
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_rad);
_parameters.rollsp_offset_rad = radians(_parameters.rollsp_offset_rad);
param_get(_parameter_handles.pitchsp_offset_deg, &_parameters.pitchsp_offset_rad);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_rad);
_parameters.pitchsp_offset_rad = radians(_parameters.pitchsp_offset_rad);
param_get(_parameter_handles.time_const, &(_parameters.time_const));
@ -783,7 +788,7 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_l1_control.set_l1_roll_limit(radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_time_const_throt(_parameters.time_const_throt);
@ -816,7 +821,7 @@ FixedwingPositionControl::parameters_update()
}
/* Update the landing slope */
_landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative,
_landingslope.update(radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative,
_parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
/* Update and publish the navigation capabilities */
@ -1021,12 +1026,8 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
/* add minimum ground speed undershoot (only non-zero in presence of sufficient wind) */
target_airspeed += _groundspeed_undershoot;
if (0/* throttle nudging enabled */) {
//target_airspeed += nudge term.
}
/* sanity check: limit to range */
target_airspeed = math::constrain(target_airspeed, _parameters.airspeed_min, _parameters.airspeed_max);
target_airspeed = constrain(target_airspeed, _parameters.airspeed_min, _parameters.airspeed_max);
/* plain airspeed error */
_airspeed_error = target_airspeed - airspeed;
@ -1063,7 +1064,6 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
/*
* Ground speed undershoot is the amount of ground velocity not reached
* by the plane. Consequently it is zero if airspeed is >= min ground speed
@ -1073,7 +1073,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
* not exceeded) travels towards a waypoint (and is not pushed more and more away
* by wind). Not countering this would lead to a fly-away.
*/
_groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f);
_groundspeed_undershoot = max(ground_speed_desired - ground_speed_body, 0.0f);
} else {
_groundspeed_undershoot = 0;
@ -1230,7 +1230,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc
/* demand "climbout_diff" m above ground if user switched into this mode during takeoff */
if (in_takeoff_situation()) {
*hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff;
*pitch_limit_min = math::radians(10.0f);
*pitch_limit_min = radians(10.0f);
} else {
*pitch_limit_min = _parameters.pitch_limit_min;
@ -1259,6 +1259,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
_att_sp.apply_flaps = false; // by default we don't use flaps
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
/* filter speed and altitude for controller */
@ -1399,9 +1400,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.yaw_body = _l1_control.nav_bearing();
tecs_update_pitch_throttle(pos_sp_triplet.current.alt, calculate_target_airspeed(mission_airspeed), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
radians(_parameters.pitch_limit_min), radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, mission_throttle,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
false, radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
@ -1414,8 +1415,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float alt_sp = pos_sp_triplet.current.alt;
if (in_takeoff_situation()) {
alt_sp = math::max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff);
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-5.0f), math::radians(5.0f));
alt_sp = max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff);
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
}
if (_fw_pos_ctrl_status.abort_landing) {
@ -1432,13 +1433,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(alt_sp,
calculate_target_airspeed(mission_airspeed),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
@ -1466,7 +1467,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
float wp_distance_save = wp_distance;
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) {
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= radians(90.0f)) {
wp_distance_save = 0.0f;
}
@ -1513,7 +1514,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (_land_noreturn_horizontal) {
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
}
/* Vertical landing control */
@ -1598,13 +1599,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
throttle_max = min(throttle_max, _parameters.throttle_land_max);
if (!_land_motor_lim) {
_land_motor_lim = true;
mavlink_log_info(&_mavlink_log_pub, "#Landing, limiting throttle");
}
}
float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp,
@ -1619,14 +1619,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land),
eas2tas,
math::radians(_parameters.land_flare_pitch_min_deg),
math::radians(_parameters.land_flare_pitch_max_deg),
radians(_parameters.land_flare_pitch_min_deg),
radians(_parameters.land_flare_pitch_max_deg),
0.0f,
throttle_max,
throttle_land,
false,
_land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg)
: math::radians(_parameters.pitch_limit_min),
_land_motor_lim ? radians(_parameters.land_flare_pitch_min_deg) : radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed,
_land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
@ -1640,11 +1639,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
if (_global_pos.vel_d > 0.1f) {
_att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) *
math::constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f);
} else {
_att_sp.pitch_body = _att_sp.pitch_body;
_att_sp.pitch_body = radians(_parameters.land_flare_pitch_min_deg) *
constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f);
}
}
@ -1674,19 +1670,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
/* continue horizontally */
altitude_desired_rel = pos_sp_triplet.previous.valid ? L_altitude_rel :
_global_pos.alt - terrain_alt;
altitude_desired_rel = pos_sp_triplet.previous.valid ? L_altitude_rel :
_global_pos.alt - terrain_alt;
}
tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach), eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
@ -1724,22 +1720,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// update tecs
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 = radians(takeoff_pitch_max_deg);
tecs_update_pitch_throttle(pos_sp_triplet.current.alt,
calculate_target_airspeed(
_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min),
calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_min),
takeoff_pitch_max_rad,
_parameters.throttle_min,
_parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here?
_parameters.throttle_cruise,
_runway_takeoff.climbout(),
math::radians(_runway_takeoff.getMinPitch(
pos_sp_triplet.current.pitch_min,
10.0f,
_parameters.pitch_limit_min)),
radians(_runway_takeoff.getMinPitch(pos_sp_triplet.current.pitch_min, 10.0f, _parameters.pitch_limit_min)),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_TAKEOFF);
@ -1754,9 +1746,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators();
_att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators();
/*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body,
(double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/
} else {
/* Perform launch detection */
if (_launchDetector.launchDetectionEnabled() &&
@ -1797,7 +1786,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* select maximum pitch: the launchdetector may impose another limit for the pitch
* depending on the state of the launch */
float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_parameters.pitch_limit_max);
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
float takeoff_pitch_max_rad = radians(takeoff_pitch_max_deg);
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
* meters */
@ -1806,32 +1795,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(pos_sp_triplet.current.alt,
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_min),
takeoff_pitch_max_rad,
_parameters.throttle_min, takeoff_throttle,
_parameters.throttle_cruise,
true,
math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f)),
max(radians(pos_sp_triplet.current.pitch_min), radians(10.0f)),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_TAKEOFF);
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
math::radians(15.0f));
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
} else {
tecs_update_pitch_throttle(pos_sp_triplet.current.alt,
calculate_target_airspeed(mission_airspeed),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
takeoff_throttle,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
@ -1845,8 +1832,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Set default roll and pitch setpoints during detection phase */
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f));
_att_sp.pitch_body = max(radians(pos_sp_triplet.current.pitch_min), radians(10.0f));
}
}
@ -1904,7 +1890,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float pitch_limit_min;
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* throttle limiting */
throttle_max = _parameters.throttle_max;
@ -1915,13 +1900,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(_hold_alt,
altctrl_airspeed,
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
throttle_max,
_parameters.throttle_cruise,
climbout_requested,
((climbout_requested) ? math::radians(10.0f) : pitch_limit_min),
((climbout_requested) ? radians(10.0f) : pitch_limit_min),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
@ -1977,11 +1962,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (in_takeoff_situation()) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
math::radians(15.0f));
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
}
}
}
if (!_yaw_lock_engaged || fabsf(_manual.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
@ -2030,13 +2013,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
tecs_update_pitch_throttle(_hold_alt,
altctrl_airspeed,
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
throttle_max,
_parameters.throttle_cruise,
climbout_requested,
((climbout_requested) ? math::radians(10.0f) : pitch_limit_min),
((climbout_requested) ? radians(10.0f) : pitch_limit_min),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
@ -2078,26 +2061,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) {
_att_sp.thrust = _runway_takeoff.getThrottle(math::min(get_tecs_thrust(), throttle_max));
_att_sp.thrust = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max));
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust = 0.0f;
} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
_att_sp.thrust = math::min(_att_sp.thrust, _parameters.throttle_max);
_att_sp.thrust = min(_att_sp.thrust, _parameters.throttle_max);
} else {
/* Copy thrust and pitch values from tecs */
if (_vehicle_land_detected.landed) {
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust = math::min(_parameters.throttle_idle, throttle_max);
_att_sp.thrust = min(_parameters.throttle_idle, throttle_max);
} else {
_att_sp.thrust = math::min(get_tecs_thrust(), throttle_max);
_att_sp.thrust = min(get_tecs_thrust(), throttle_max);
}
}
// decide when to use pitch setpoint from TECS because in some cases pitch
@ -2128,7 +2109,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_last_manual = true;
}
return setpoint;
}
@ -2178,7 +2158,6 @@ FixedwingPositionControl::handle_command()
void
FixedwingPositionControl::task_main()
{
/*
* do subscriptions
*/
@ -2310,8 +2289,8 @@ FixedwingPositionControl::task_main()
_att_sp.pitch_body += _parameters.pitchsp_offset_rad;
if (_control_mode.flag_control_manual_enabled) {
_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad);
_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad);
_att_sp.roll_body = constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad);
_att_sp.pitch_body = constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad);
}
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
@ -2358,12 +2337,10 @@ FixedwingPositionControl::task_main()
fw_pos_ctrl_status_publish();
}
}
perf_end(_loop_perf);
}
}
_task_running = false;
@ -2399,7 +2376,6 @@ void FixedwingPositionControl::reset_landing_state()
_fw_pos_ctrl_status.abort_landing = false;
}
}
void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
@ -2440,7 +2416,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_asp_after_transition = _ctrl_state.airspeed;
}
_asp_after_transition = math::constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max);
_asp_after_transition = constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max);
} else if (_was_in_transition) {
// after transition we ramp up desired airspeed from the speed we had coming out of the transition