yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

FixedWingPositionControl: control only height rate when using pitch stick

Browse Source 
in manual altitude controlled modes

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-05-11 15:02:40 +03:00
committed by Silvan Fuhrer
parent 3d87bfcc0a
commit f6de99d42e
2 changed files with 18 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@@ -567,14 +567,22 @@ FixedwingPositionControl::update_desired_altitude(float dt)
if (_manual_control_setpoint_altitude > deadBand) { if (_manual_control_setpoint_altitude > deadBand) {
/* pitching down */ /* pitching down */
float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor; float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
_hold_alt += (_param_sinkrate_target.get() * dt) * pitch;
_was_in_deadband = false; if (pitch * _param_sinkrate_target.get() < _tecs.hgt_rate_setpoint()) {
_hold_alt += (_param_sinkrate_target.get() * dt) * pitch;
_manual_height_rate_setpoint_m_s = pitch * _param_sinkrate_target.get();
_was_in_deadband = false;
}
} else if (_manual_control_setpoint_altitude < - deadBand) { } else if (_manual_control_setpoint_altitude < - deadBand) {
/* pitching up */ /* pitching up */
float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor; float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
_hold_alt += (_param_climbrate_target.get() * dt) * pitch;
_was_in_deadband = false; if (pitch * _param_climbrate_target.get() > _tecs.hgt_rate_setpoint()) {
_hold_alt += (_param_climbrate_target.get() * dt) * pitch;
_manual_height_rate_setpoint_m_s = pitch * _param_climbrate_target.get();
_was_in_deadband = false;
}
} else if (!_was_in_deadband) { } else if (!_was_in_deadband) {
/* store altitude at which manual.x was inside deadBand /* store altitude at which manual.x was inside deadBand
@@ -583,6 +591,7 @@ FixedwingPositionControl::update_desired_altitude(float dt)
_hold_alt = _current_altitude; _hold_alt = _current_altitude;
_althold_epv = _local_pos.epv; _althold_epv = _local_pos.epv;
_was_in_deadband = true; _was_in_deadband = true;
_manual_height_rate_setpoint_m_s = NAN;
} }
if (_vehicle_status.is_vtol) { if (_vehicle_status.is_vtol) {
@@ -999,7 +1008,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
_param_fw_thr_cruise.get(), _param_fw_thr_cruise.get(),
false, false,
pitch_limit_min, pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL); tecs_status_s::TECS_MODE_NORMAL,
_manual_height_rate_setpoint_m_s);
/* heading control */ /* heading control */
if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH && if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
@@ -1101,7 +1111,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
_param_fw_thr_cruise.get(), _param_fw_thr_cruise.get(),
false, false,
pitch_limit_min, pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL); tecs_status_s::TECS_MODE_NORMAL,
_manual_height_rate_setpoint_m_s);
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = 0; _att_sp.yaw_body = 0;

1
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@@ -174,6 +174,7 @@ private:
float _global_local_alt0{NAN}; float _global_local_alt0{NAN};
float _hold_alt{0.0f}; ///< hold altitude for altitude mode float _hold_alt{0.0f}; ///< hold altitude for altitude mode
float _manual_height_rate_setpoint_m_s{NAN};
float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched
float _hdg_hold_yaw{0.0f}; ///< hold heading for velocity mode float _hdg_hold_yaw{0.0f}; ///< hold heading for velocity mode
bool _hdg_hold_enabled{false}; ///< heading hold enabled bool _hdg_hold_enabled{false}; ///< heading hold enabled