diff --git a/posix-configs/SITL/init/ekf2/plane b/posix-configs/SITL/init/ekf2/plane index e9cc2796af..521b5a6e21 100644 --- a/posix-configs/SITL/init/ekf2/plane +++ b/posix-configs/SITL/init/ekf2/plane @@ -46,6 +46,7 @@ param set FW_PR_FF 0.40 param set FW_PR_I 0.05 param set FW_PR_P 0.05 +param set FW_W_EN 1 param set RWTO_TKOFF 1 replay tryapplyparams diff --git a/posix-configs/SITL/init/lpe/plane b/posix-configs/SITL/init/lpe/plane index c4da63c665..3df84cf36a 100644 --- a/posix-configs/SITL/init/lpe/plane +++ b/posix-configs/SITL/init/lpe/plane @@ -46,6 +46,7 @@ param set FW_PR_FF 0.40 param set FW_PR_I 0.05 param set FW_PR_P 0.05 +param set FW_W_EN 1 param set RWTO_TKOFF 1 replay tryapplyparams diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 1df1db2975..f37e8b6981 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include using matrix::Eulerf; using matrix::Quatf; @@ -177,6 +178,8 @@ private: float roll_to_yaw_ff; int32_t y_coordinated_method; float y_rmax; + + bool w_en; float w_p; float w_i; float w_ff; @@ -209,11 +212,11 @@ private: float rattitude_thres; - int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ + int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ - int bat_scale_en; /**< Battery scaling enabled */ + int32_t bat_scale_en; /**< Battery scaling enabled */ - } _parameters; /**< local copies of interesting parameters */ + } _parameters{}; /**< local copies of interesting parameters */ struct { @@ -238,6 +241,8 @@ private: param_t roll_to_yaw_ff; param_t y_coordinated_method; param_t y_rmax; + + param_t w_en; param_t w_p; param_t w_i; param_t w_ff; @@ -272,7 +277,7 @@ private: param_t bat_scale_en; - } _parameter_handles; /**< handles for interesting parameters */ + } _parameter_handles{}; /**< handles for interesting parameters */ // Rotation matrix and euler angles to extract from control state math::Matrix<3, 3> _R; @@ -429,6 +434,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); _parameter_handles.roll_to_yaw_ff = param_find("FW_RLL_TO_YAW_FF"); + _parameter_handles.w_en = param_find("FW_W_EN"); _parameter_handles.w_p = param_find("FW_WR_P"); _parameter_handles.w_i = param_find("FW_WR_I"); _parameter_handles.w_ff = param_find("FW_WR_FF"); @@ -529,6 +535,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.roll_to_yaw_ff, &(_parameters.roll_to_yaw_ff)); + int32_t wheel_enabled = 0; + param_get(_parameter_handles.w_en, &wheel_enabled); + _parameters.w_en = (wheel_enabled == 1); + param_get(_parameter_handles.w_p, &(_parameters.w_p)); param_get(_parameter_handles.w_i, &(_parameters.w_i)); param_get(_parameter_handles.w_ff, &(_parameters.w_ff)); @@ -806,7 +816,7 @@ FixedwingAttitudeControl::task_main() _pitch = euler_angles(1); _yaw = euler_angles(2); - if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { + if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to @@ -1118,7 +1128,7 @@ FixedwingAttitudeControl::task_main() float yaw_u = 0.0f; - if (_att_sp.fw_control_yaw == true) { + if (_parameters.w_en && _att_sp.fw_control_yaw) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } else { diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 7540d26007..37ae13f680 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -287,6 +287,15 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); */ PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f); +/** + * Enable wheel steering controller + * + * @boolean + * @group FW Attitude Control + */ +PARAM_DEFINE_INT32(FW_W_EN, 0); + + /** * Wheel steering rate proportional gain *