diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 7938c47bae..4100cdb4a3 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -39,5 +39,8 @@ then param set FW_RR_P 0.3 fi +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + set HIL yes set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index e1f2cdfe8d..52c512ed8c 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -11,4 +11,7 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_x +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 7aa888c763..c590f9ac86 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -11,4 +11,7 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_+ +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 57bcd24d02..0369da5980 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -11,4 +11,7 @@ sh /etc/init.d/rc.fw_defaults set HIL yes +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 5e0b6cd746..278df193aa 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -36,5 +36,8 @@ fi set HIL yes +# Enable gamepad / joystick support +param set COM_RC_IN_MODE 2 + # Set the AERT mixer for HIL (even if the malolo is a flying wing) set MIXER AERT 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..ceea71fca9 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; @@ -1485,7 +1502,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - pitch_limit_min, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); @@ -1595,7 +1612,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - pitch_limit_min, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL);