fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 18:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position Control: remove manual climbout mode trigger via pitch stick

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2020-11-16 14:41:01 +01:00
parent ad4068f472
commit b58e270fd4
2 changed files with 8 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+7 -12
View File
@@ -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());
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+1 -4
View File
@@ -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<FixedwingPositionControl>, 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,