diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c9716550e3..23c0c466f4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -465,7 +465,7 @@ FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt) return takeoff_alt; } -bool +void FixedwingPositionControl::update_desired_altitude(float dt) { /* @@ -481,9 +481,6 @@ FixedwingPositionControl::update_desired_altitude(float dt) */ const float factor = 1.0f - deadBand; - /* Climbout mode sets maximum throttle and pitch up */ - bool climbout_mode = false; - /* * Reset the hold altitude to the current altitude if the uncertainty * changes significantly. @@ -512,7 +509,6 @@ FixedwingPositionControl::update_desired_altitude(float dt) float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor; _hold_alt += (_param_fw_t_clmb_max.get() * dt) * pitch; _was_in_deadband = false; - climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); } else if (!_was_in_deadband) { /* store altitude at which manual.x was inside deadBand @@ -529,7 +525,6 @@ FixedwingPositionControl::update_desired_altitude(float dt) } } - return climbout_mode; } bool @@ -934,7 +929,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 float altctrl_airspeed = get_demanded_airspeed(); /* update desired altitude based on user pitch stick input */ - bool climbout_requested = update_desired_altitude(dt); + 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 @@ -956,8 +951,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _param_fw_thr_min.get(), throttle_max, _param_fw_thr_cruise.get(), - climbout_requested, - climbout_requested ? radians(10.0f) : pitch_limit_min, + false, + pitch_limit_min, tecs_status_s::TECS_MODE_NORMAL); /* heading control */ @@ -1036,7 +1031,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 float altctrl_airspeed = get_demanded_airspeed(); /* update desired altitude based on user pitch stick input */ - bool climbout_requested = update_desired_altitude(dt); + 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 @@ -1058,8 +1053,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _param_fw_thr_min.get(), throttle_max, _param_fw_thr_cruise.get(), - climbout_requested, - climbout_requested ? radians(10.0f) : pitch_limit_min, + false, + pitch_limit_min, tecs_status_s::TECS_MODE_NORMAL); _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index a0c46372b0..a7b21a8511 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -121,8 +121,6 @@ static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort l static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes -static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = - 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; class FixedwingPositionControl final : public ModuleBase, public ModuleParams, @@ -328,9 +326,8 @@ private: * Update desired altitude base on user pitch stick input * * @param dt Time step - * @return true if climbout mode was requested by user (climb with max rate and min airspeed) */ - bool update_desired_altitude(float dt); + void update_desired_altitude(float dt); bool control_position(const hrt_abstime &now, const Vector2f &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,