From 721131a135f9c86cbdd4e2e1ee32dc04b10f798c Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Wed, 11 May 2022 12:53:36 +0200 Subject: [PATCH] fw pos/att ctrl: pass manual nose wheel increments during takeoff ground roll --- src/modules/fw_att_control/FixedwingAttitudeControl.cpp | 3 +++ .../fw_pos_control_l1/FixedwingPositionControl.cpp | 8 ++++++++ .../fw_pos_control_l1/FixedwingPositionControl.hpp | 3 ++- .../runway_takeoff/runway_takeoff_params.c | 8 ++++++++ 4 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 4f825ee589..e27025f433 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -556,6 +556,9 @@ void FixedwingAttitudeControl::Run() if (wheel_control) { yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input); + // XXX: this is an abuse -- used to ferry manual yaw inputs from position controller during auto modes + yaw_u += _att_sp.yaw_sp_move_rate * _param_fw_man_y_sc.get(); + } else { yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2)); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index fee34d393b..49a2af27a6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1424,6 +1424,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo // yaw control is disabled once in "taking off" state _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); + // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. + if (_param_rwto_nudge.get()) { + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.r; + } + // tune up the lateral position control guidance when on the ground if (_att_sp.fw_control_yaw) { _npfg.setPeriod(_param_rwto_l1_period.get()); @@ -2359,6 +2364,9 @@ FixedwingPositionControl::Run() // by default we don't want yaw to be contoller directly with rudder _att_sp.fw_control_yaw = false; + // default to zero - is used (IN A HACKY WAY) to pass direct nose wheel steering via yaw stick to the actuators during auto takeoff + _att_sp.yaw_sp_move_rate = 0.0f; + if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) { reset_landing_state(); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 550ba23551..a346877c0d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -765,7 +765,8 @@ private: (ParamFloat) _param_fw_wing_span, (ParamFloat) _param_fw_wing_height, - (ParamFloat) _param_rwto_l1_period + (ParamFloat) _param_rwto_l1_period, + (ParamBool) _param_rwto_nudge ) }; diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c index 6423c56bfb..8d52b93ee0 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c +++ b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c @@ -144,3 +144,11 @@ PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_L1_PERIOD, 5.0f); + +/** + * Enable use of yaw stick for nudging the wheel during runway ground roll + * + * @boolean + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_NUDGE, 0);