diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 0f90b1667c..1433e807e5 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -62,23 +62,13 @@ Standard::Standard(VtolAttitudeControl *attc) : _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; - _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); - _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT"); _params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP"); - _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); - _params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND"); - _params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS"); - _params_handles_standard.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); - _params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); _params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); _params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC"); - _params_handles_standard.airspeed_disabled = param_find("FW_ARSP_MODE"); _params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF"); _params_handles_standard.reverse_output = param_find("VT_B_REV_OUT"); _params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL"); - _params_handles_standard.back_trans_throttle = param_find("VT_B_TRANS_THR"); - _params_handles_standard.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); - } Standard::~Standard() @@ -89,39 +79,16 @@ void Standard::parameters_update() { float v; - int i; /* duration of a forwards transition to fw mode */ - param_get(_params_handles_standard.front_trans_dur, &v); - _params_standard.front_trans_dur = math::constrain(v, 0.0f, 20.0f); - - /* duration of a back transition to mc mode */ - param_get(_params_handles_standard.back_trans_dur, &v); - _params_standard.back_trans_dur = math::constrain(v, 0.0f, 20.0f); + param_get(_params_handles_standard.pusher_ramp_dt, &v); + _params_standard.pusher_ramp_dt = math::constrain(v, 0.0f, 20.0f); /* MC ramp up during back transition to mc mode */ param_get(_params_handles_standard.back_trans_ramp, &v); - _params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params_standard.back_trans_dur); + _params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params->back_trans_duration); - /* target throttle value for pusher motor during the transition to fw mode */ - param_get(_params_handles_standard.pusher_trans, &v); - _params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f); - - /* airspeed at which we should switch to fw mode */ - param_get(_params_handles_standard.airspeed_trans, &v); - _params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f); - - /* airspeed at which we start blending mc/fw controls */ - param_get(_params_handles_standard.airspeed_blend, &v); - _params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f); - - _airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend; - - /* timeout for transition to fw mode */ - param_get(_params_handles_standard.front_trans_timeout, &_params_standard.front_trans_timeout); - - /* minimum time for transition to fw mode */ - param_get(_params_handles_standard.front_trans_time_min, &_params_standard.front_trans_time_min); + _airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend; /* maximum down pitch allowed */ param_get(_params_handles_standard.down_pitch_max, &v); @@ -130,10 +97,6 @@ Standard::parameters_update() /* scale for fixed wing thrust used for forward acceleration in multirotor mode */ param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale); - /* airspeed mode */ - param_get(_params_handles_standard.airspeed_disabled, &i); - _params_standard.airspeed_disabled = math::constrain(i, 0, 1); - /* pitch setpoint offset */ param_get(_params_handles_standard.pitch_setpoint_offset, &v); _params_standard.pitch_setpoint_offset = math::radians(v); @@ -146,13 +109,6 @@ Standard::parameters_update() param_get(_params_handles_standard.reverse_delay, &v); _params_standard.reverse_delay = math::constrain(v, 0.0f, 10.0f); - /* reverse throttle */ - param_get(_params_handles_standard.back_trans_throttle, &v); - _params_standard.back_trans_throttle = math::constrain(v, -1.0f, 1.0f); - - /* mpc cruise speed */ - param_get(_params_handles_standard.mpc_xy_cruise, &_params_standard.mpc_xy_cruise); - } void Standard::update_vtol_state() @@ -163,6 +119,8 @@ void Standard::update_vtol_state() */ float mc_weight = _mc_roll_weight; + float time_now = (float)hrt_absolute_time(); + float time_since_trans_start = time_now - (float)_vtol_schedule.transition_start; if (!_attc->is_fixed_wing_requested()) { @@ -209,9 +167,8 @@ void Standard::update_vtol_state() float x_vel = vel(0); - if (hrt_elapsed_time(&_vtol_schedule.transition_start) > - (_params_standard.back_trans_dur * 1000000.0f) || - (_local_pos->v_xy_valid && x_vel <= _params_standard.mpc_xy_cruise)) { + if (time_since_trans_start > _params->back_trans_duration * 1e6f || + (_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise)) { _vtol_schedule.flight_mode = MC_MODE; } @@ -233,10 +190,9 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - if (((_params_standard.airspeed_disabled == 1 || - _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) && - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) - > (_params_standard.front_trans_time_min * 1000000.0f)) || + if (((_params->airspeed_mode == 1 || + _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed) && + time_since_trans_start > _params->front_trans_time_min * 1e6f) || can_transition_on_ground()) { _vtol_schedule.flight_mode = FW_MODE; @@ -277,6 +233,9 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { float mc_weight = 1.0f; + float time_now = (float)hrt_absolute_time(); + float time_since_trans_start = time_now - (float) + _vtol_schedule.transition_start; // time in microseconds since transition started VtolType::update_transition_state(); @@ -284,32 +243,28 @@ void Standard::update_transition_state() memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { - if (_params_standard.front_trans_dur <= 0.0f) { + if (_params_standard.pusher_ramp_dt <= 0.0f) { // just set the final target throttle value - _pusher_throttle = _params_standard.pusher_trans; + _pusher_throttle = _params->front_trans_throttle; - } else if (_pusher_throttle <= _params_standard.pusher_trans) { + } else if (_pusher_throttle <= _params->front_trans_throttle) { // ramp up throttle to the target throttle value - _pusher_throttle = _params_standard.pusher_trans * - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); + _pusher_throttle = _params->front_trans_throttle * time_since_trans_start / (_params_standard.pusher_ramp_dt * 1e6f); } // do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed if (_airspeed_trans_blend_margin > 0.0f && - _airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend && - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f) - ) { - mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) / + _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend && + time_since_trans_start > _params->front_trans_time_min * 1e6f) { + mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) / _airspeed_trans_blend_margin; // time based blending when no airspeed sensor is set - } else if (_params_standard.airspeed_disabled && - hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1e6f) && - hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) * 1e6f) - ) { - mc_weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - (( - _params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) / - ((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f)); + } else if (_params->airspeed_mode == 1 && + time_since_trans_start < _params->front_trans_time_min * 1e6f && + time_since_trans_start > _params->front_trans_time_min * 1e6f / 2.0f) { + mc_weight = 1.0f - ((time_since_trans_start - _params->front_trans_time_min * 1e6f / 2.0f) / + (_params->front_trans_time_min * 1e6f / 2.0f)); } @@ -320,8 +275,8 @@ void Standard::update_transition_state() _v_att_sp->q_d_valid = true; // check front transition timeout - if (_params_standard.front_trans_timeout > FLT_EPSILON) { - if (hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1e6f)) { + if (_params->front_trans_timeout > FLT_EPSILON) { + if (time_since_trans_start > _params->front_trans_timeout * 1e6f) { // transition timeout occured, abort transition _attc->abort_front_transition("Transition timeout"); } @@ -335,22 +290,21 @@ void Standard::update_transition_state() q_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; - hrt_abstime btrans_start; - btrans_start = _vtol_schedule.transition_start + uint64_t(_params_standard.reverse_delay) * 1000000.0f; + float btrans_start = (float)_vtol_schedule.transition_start + _params_standard.reverse_delay * 1e6f; + _pusher_throttle = 0.0f; - if (hrt_absolute_time() >= btrans_start) { + if (time_now >= btrans_start) { // Handle throttle reversal for active breaking - float thrscale = (float)hrt_elapsed_time(&btrans_start) / (_params_standard.front_trans_dur * - 1000000.0f); + float thrscale = (time_now - btrans_start) / (_params_standard.pusher_ramp_dt * 1e6f); thrscale = math::constrain(thrscale, 0.0f, 1.0f); - _pusher_throttle = thrscale * _params_standard.back_trans_throttle; + _pusher_throttle = thrscale * _params->back_trans_throttle; } // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_ramp > FLT_EPSILON) { - mc_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / - ((_params_standard.back_trans_ramp) * 1000000.0f); + mc_weight = time_since_trans_start / + ((_params_standard.back_trans_ramp) * 1e6f); } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index d1b1b631ce..bed2de9396 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -67,41 +67,23 @@ public: private: struct { - float front_trans_dur; - float back_trans_dur; + float pusher_ramp_dt; float back_trans_ramp; - float pusher_trans; - float airspeed_blend; - float airspeed_trans; - float front_trans_timeout; - float front_trans_time_min; float down_pitch_max; float forward_thrust_scale; - int32_t airspeed_disabled; float pitch_setpoint_offset; float reverse_output; float reverse_delay; - float back_trans_throttle; - float mpc_xy_cruise; } _params_standard; struct { - param_t front_trans_dur; - param_t back_trans_dur; + param_t pusher_ramp_dt; param_t back_trans_ramp; - param_t pusher_trans; - param_t airspeed_blend; - param_t airspeed_trans; - param_t front_trans_timeout; - param_t front_trans_time_min; param_t down_pitch_max; param_t forward_thrust_scale; - param_t airspeed_disabled; param_t pitch_setpoint_offset; param_t reverse_output; param_t reverse_delay; - param_t back_trans_throttle; - param_t mpc_xy_cruise; } _params_handles_standard; enum vtol_mode { diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index c74ba7595a..94a2b1d45f 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -39,16 +39,6 @@ * @author Roman Bapst */ -/** - * Target throttle value for pusher/puller motor during the transition to fw mode - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f); /** * Maximum allowed down-pitch the controller is able to demand. This prevents large, negative @@ -113,14 +103,12 @@ PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 0.0f); PARAM_DEFINE_FLOAT(VT_B_REV_DEL, 0.0f); /** - * Thottle output during back transition - * For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking - * For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking + * Defines the time window during which the pusher throttle will be ramped up linearly to VT_F_TRANS_THR during a transition + * to fixed wing mode. Zero or negative values will produce an instant throttle rise to VT_F_TRANS_THR. * - * @min -1 - * @max 1 + * @max 20 * @increment 0.01 * @decimal 2 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f); +PARAM_DEFINE_FLOAT(VT_PSHER_RMP_DT, 3.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 1b3535cfff..c77b28ec8d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -77,6 +77,17 @@ VtolAttitudeControl::VtolAttitudeControl() _params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM"); _params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); + _params_handles.front_trans_duration = param_find("VT_F_TRANS_DUR"); + _params_handles.back_trans_duration = param_find("VT_B_TRANS_DUR"); + _params_handles.transition_airspeed = param_find("VT_ARSP_TRANS"); + _params_handles.front_trans_throttle = param_find("VT_F_TRANS_THR"); + _params_handles.back_trans_throttle = param_find("VT_B_TRANS_THR"); + _params_handles.airspeed_blend = param_find("VT_ARSP_BLEND"); + _params_handles.airspeed_mode = param_find("FW_ARSP_MODE"); + _params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); + _params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); + + _params_handles.wv_takeoff = param_find("VT_WV_TKO_EN"); _params_handles.wv_land = param_find("VT_WV_LND_EN"); _params_handles.wv_loiter = param_find("VT_WV_LTR_EN"); @@ -487,6 +498,17 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.wv_land, &l); _params.wv_land = (l == 1); + + param_get(_params_handles.front_trans_duration, &_params.front_trans_duration); + param_get(_params_handles.back_trans_duration, &_params.back_trans_duration); + param_get(_params_handles.transition_airspeed, &_params.transition_airspeed); + param_get(_params_handles.front_trans_throttle, &_params.front_trans_throttle); + param_get(_params_handles.back_trans_throttle, &_params.back_trans_throttle); + param_get(_params_handles.airspeed_blend, &_params.airspeed_blend); + param_get(_params_handles.airspeed_mode, &_params.airspeed_mode); + param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout); + param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise); + // update the parameters of the instances of base VtolType if (_vtol_type != nullptr) { _vtol_type->parameters_update(); 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 492259356a..e98a15a520 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -192,6 +192,15 @@ private: param_t wv_takeoff; param_t wv_loiter; param_t wv_land; + param_t front_trans_duration; + param_t back_trans_duration; + param_t transition_airspeed; + param_t front_trans_throttle; + param_t back_trans_throttle; + param_t airspeed_blend; + param_t airspeed_mode; + param_t front_trans_timeout; + param_t mpc_xy_cruise; } _params_handles{}; /* for multicopters it is usual to have a non-zero idle speed of the engines 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 8e98761d79..196a4c25c5 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -137,6 +137,36 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f); */ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 4.0f); +/** + * Target throttle value for the transition to fixed wing flight. + * standard vtol: pusher + * tailsitter, tiltrotor: main throttle + * + * @min 0.0 + * @max 1.0 + * @increment 0.01 + * @decimal 3 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_THR, 0.7f); + +/** + * Target throttle value for the transition to hover flight. + * standard vtol: pusher + * tailsitter, tiltrotor: main throttle + * + * Note for standard vtol: + * For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking + * For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking + * + * @min -1 + * @max 1 + * @increment 0.01 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f); + /** * Approximate deceleration during back transition * diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 1aa7d18e8f..a2d0a45a37 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -61,6 +61,15 @@ struct Params { bool wv_takeoff; bool wv_loiter; bool wv_land; + float front_trans_duration; + float back_trans_duration; + float transition_airspeed; + float front_trans_throttle; + float back_trans_throttle; + float airspeed_blend; + int32_t airspeed_mode; + float front_trans_timeout; + float mpc_xy_cruise; }; // Has to match 1:1 msg/vtol_vehicle_status.msg