From 4e3fa7cf35aba6186393ce2342e0d6f22269a298 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 16 Mar 2021 12:02:16 +0100 Subject: [PATCH] VTOL: rework forward actuation assist -allow positive pitch offsets in hover -add param for min pitch during LAND Signed-off-by: Silvan Fuhrer --- src/lib/parameters/param_translation.cpp | 10 ++++++- .../vtol_att_control/standard_params.c | 12 --------- src/modules/vtol_att_control/tiltrotor.cpp | 2 +- .../vtol_att_control_main.cpp | 15 ++++++----- .../vtol_att_control/vtol_att_control_main.h | 3 ++- .../vtol_att_control_params.c | 26 +++++++++++++++++++ src/modules/vtol_att_control/vtol_type.cpp | 23 +++++++++++----- src/modules/vtol_att_control/vtol_type.h | 3 ++- 8 files changed, 65 insertions(+), 29 deletions(-) diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 61575f0af9..ac6ae5d122 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -186,6 +186,15 @@ bool param_modify_on_import(bson_node_t node) } } + // 2021-07-12: translate VT_DWN_PITCH_MAX to VT_PITCH_MIN + { + if (strcmp("VT_DWN_PITCH_MAX", node->name) == 0) { + strcpy(node->name, "VT_PITCH_MIN"); + node->d *= -1; + PX4_INFO("copying and inverting sign %s -> %s", "VT_DWN_PITCH_MAX", "VT_PITCH_MIN"); + } + } + // translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10) if (node->type != BSON_INT32) { @@ -193,7 +202,6 @@ bool param_modify_on_import(bson_node_t node) } int64_t *ivalue = &node->i; - const char *cal_id_params[] = { "CAL_ACC0_ID", "CAL_GYRO0_ID", diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 633efa59d1..d0d8b2a9aa 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -61,18 +61,6 @@ */ PARAM_DEFINE_INT32(VT_FWD_THRUST_EN, 0); -/** - * Maximum allowed angle the vehicle is allowed to pitch down to generate forward force - * - * When fixed-wing forward actuation is active (see VT_FW_TRHUST_EN). - * If demanded down pitch exceeds this limmit, the fixed-wing forward actuators are used instead. - * - * @min 0.0 - * @max 45.0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_DWN_PITCH_MAX, 5.0f); - /** * Fixed-wing actuator thrust scale for hover forward flight. * diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 1bab60fc96..a0c607fd08 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -483,6 +483,6 @@ float Tiltrotor::thrust_compensation_for_tilt() // only compensate for tilt angle up to 0.5 * max tilt float compensated_tilt = math::constrain(_tilt_control, 0.0f, 0.5f); - // increase vertical thrust by 1/cos(tilt), limmit to [-1,0] + // increase vertical thrust by 1/cos(tilt), limit to [-1,0] return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f); } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 47bc97444a..42a42056ad 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -91,8 +91,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I"); _params_handles.back_trans_dec_sp = param_find("VT_B_DEC_MSS"); - - _params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); + _params_handles.pitch_min_rad = param_find("VT_PTCH_MIN"); _params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); _params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU"); @@ -100,8 +99,8 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1"); _params_handles.mpc_land_alt2 = param_find("MPC_LAND_ALT2"); - _params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); - _params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); + _params_handles.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN"); + /* fetch initial parameter values */ parameters_update(); @@ -326,8 +325,12 @@ VtolAttitudeControl::parameters_update() _params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); /* maximum down pitch allowed */ - param_get(_params_handles.down_pitch_max, &v); - _params.down_pitch_max = math::radians(v); + param_get(_params_handles.pitch_min_rad, &v); + _params.pitch_min_rad = math::radians(v); + + /* maximum down pitch allowed during landing*/ + param_get(_params_handles.land_pitch_min_rad, &v); + _params.land_pitch_min_rad = math::radians(v); /* scale for fixed wing thrust used for forward acceleration in multirotor mode */ param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 83ea24fc83..0f93fdc72a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -217,7 +217,8 @@ private: param_t fw_motors_off; param_t diff_thrust; param_t diff_thrust_scale; - param_t down_pitch_max; + param_t pitch_min_rad; + param_t land_pitch_min_rad; param_t forward_thrust_scale; param_t dec_to_pitch_ff; param_t dec_to_pitch_i; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index f88b2a0cda..772faa4e80 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -358,3 +358,29 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_MC_ON_FMU, 0); + +/** + * Minimum pitch angle during hover. + * + * Minimum pitch angle during hover flight. If the desired pitch angle is is lower than this value + * then the fixed-wing forward actuation can be used to compensate for the missing thrust in forward direction + * (see VT_FW_TRHUST_EN) + * + * @min -10.0 + * @max 45.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_PTCH_MIN, -5.0f); + +/** + * Minimum pitch angle during hover landing. + * + * Overrides VT_PTCH_MIN when the vehicle is in LAND mode (hovering). + * During landing it can be beneficial to allow lower minimum pitch angles as it can avoid the wings + * generating too much lift and preventing the vehicle from sinking at the desired rate. + * + * @min -10.0 + * @max 45.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_LND_PTCH_MIN, -5.0f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index a913ac2b0b..c06d5f7281 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -559,23 +559,32 @@ float VtolType::pusher_assist() // calculate the desired pitch seen in the heading frame // this value corresponds to the amount the vehicle would try to pitch down - float pitch_down = atan2f(body_z_sp(0), body_z_sp(2)); + const float pitch_setpoint = atan2f(body_z_sp(0), body_z_sp(2)); // normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0 float forward_thrust = 0.0f; + float pitch_setpoint_min = _params->pitch_min_rad; + + if (_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + pitch_setpoint_min = _params->land_pitch_min_rad; // set min pitch during LAND (usually lower to generate less lift) + } + // only allow pitching down up to threshold, the rest of the desired // forward acceleration will be compensated by the pusher/tilt - if (pitch_down < -_params->down_pitch_max) { - // desired roll angle in heading frame stays the same - float roll_new = -asinf(body_z_sp(1)); - forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale; + if (pitch_setpoint < pitch_setpoint_min) { + // desired roll angle in heading frame stays the same + const float roll_new = -asinf(body_z_sp(1)); + + forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _params->forward_thrust_scale; // limit forward actuation to [0, 0.9] forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f); - // return the vehicle to level position - float pitch_new = 0.0f; + // Set the pitch to 0 if the pitch limit is negative (pitch down), but allow a positive (pitch up) pitch. + // This can be used for tiltrotor to make them hover with a positive angle of attack + const float pitch_new = pitch_setpoint_min > 0.f ? pitch_setpoint_min : 0.f; // create corrected desired body z axis in heading frame const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 900d6d6126..c88cb5bbf3 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -70,7 +70,8 @@ struct Params { int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ int32_t diff_thrust; float diff_thrust_scale; - float down_pitch_max; + float pitch_min_rad; + float land_pitch_min_rad; float forward_thrust_scale; float dec_to_pitch_ff; float dec_to_pitch_i;