diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 33a78f3141..4f3fdac91c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -179,6 +179,8 @@ private: float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ + float _althold_epv; /**< the position estimate accuracy when engaging alt hold */ + bool _was_in_deadband; /**< wether the last stick input was in althold deadband */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** deadBand) { - float pitch = (_manual.x - deadBand) / factor; - _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - was_in_deadband = false; - climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH); + /* pitching down */ + float pitch = -(_manual.x - deadBand) / factor; + _hold_alt += (_parameters.max_sink_rate * dt) * pitch; + _was_in_deadband = false; } else if (_manual.x < - deadBand) { - float pitch = (_manual.x + deadBand) / factor; - _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - was_in_deadband = false; - } else if (!was_in_deadband) { + /* pitching up */ + float pitch = -(_manual.x + deadBand) / factor; + _hold_alt += (_parameters.max_climb_rate * 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 * The aircraft should immediately try to fly at this altitude * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; _althold_epv = _global_pos.epv; - was_in_deadband = true; + _was_in_deadband = true; } return climbout_mode;