diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index e6316b1459..517828377f 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -39,5 +39,7 @@ px4_add_module( vtol_type.cpp tailsitter.cpp standard.cpp + DEPENDS + SlewRate ) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5b1c36b8ee..2feee91ee0 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -288,6 +288,10 @@ void Standard::update_transition_state() } } + // set spoiler and flaps to 0 + _flaps_setpoint_with_slewrate.update(0.f, _dt); + _spoiler_setpoint_with_slewrate.update(0.f, _dt); + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { if (_v_control_mode->flag_control_climb_rate_enabled) { @@ -370,8 +374,9 @@ void Standard::fill_actuator_outputs() fw_out[actuator_controls_s::INDEX_PITCH] = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_PITCH]; fw_out[actuator_controls_s::INDEX_YAW] = 0; fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; - fw_out[actuator_controls_s::INDEX_FLAPS] = 0; - fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0; + fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); + fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); + fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0.f; break; @@ -391,7 +396,8 @@ void Standard::fill_actuator_outputs() fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW]; fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; - fw_out[actuator_controls_s::INDEX_FLAPS] = fw_in[actuator_controls_s::INDEX_FLAPS]; + fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); + fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); fw_out[actuator_controls_s::INDEX_AIRBRAKES] = _reverse_output; break; @@ -409,7 +415,8 @@ void Standard::fill_actuator_outputs() fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH]; fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW]; fw_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; - fw_out[actuator_controls_s::INDEX_FLAPS] = fw_in[actuator_controls_s::INDEX_FLAPS]; + fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); + fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0; break; } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 2bbe68dff6..ed79497788 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -370,6 +370,10 @@ void Tiltrotor::update_transition_state() // add minimum throttle for front transition _thrust_transition = math::max(_thrust_transition, FRONTTRANS_THR_MIN); + // set spoiler and flaps to 0 + _flaps_setpoint_with_slewrate.update(0.f, _dt); + _spoiler_setpoint_with_slewrate.update(0.f, _dt); + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely _tilt_control = math::constrain(_params_tiltrotor.tilt_transition + @@ -392,6 +396,10 @@ void Tiltrotor::update_transition_state() // if this is not then then there is race condition where the fw rate controller still publishes a zero sample throttle after transition _v_att_sp->thrust_body[0] = _thrust_transition; + // set spoiler and flaps to 0 + _flaps_setpoint_with_slewrate.update(0.f, _dt); + _spoiler_setpoint_with_slewrate.update(0.f, _dt); + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { // set idle speed for rotary wing mode @@ -534,9 +542,9 @@ void Tiltrotor::fill_actuator_outputs() fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control; if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) { - fw_out[actuator_controls_s::INDEX_ROLL] = 0; - fw_out[actuator_controls_s::INDEX_PITCH] = 0; - fw_out[actuator_controls_s::INDEX_YAW] = 0; + fw_out[actuator_controls_s::INDEX_ROLL] = 0; + fw_out[actuator_controls_s::INDEX_PITCH] = 0; + fw_out[actuator_controls_s::INDEX_YAW] = 0; } else { fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; @@ -547,6 +555,10 @@ void Tiltrotor::fill_actuator_outputs() _torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW]; } + fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); + fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); + fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0; + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; 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 318eb5e1d2..089e6567d7 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -109,6 +109,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN"); + _params_handles.vt_spoiler_mc_ld = param_find("VT_SPOILER_MC_LD"); /* fetch initial parameter values */ parameters_update(); @@ -372,6 +373,8 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1); param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2); + param_get(_params_handles.vt_spoiler_mc_ld, &_params.vt_spoiler_mc_ld); + // update the parameters of the instances of base VtolType if (_vtol_type != nullptr) { _vtol_type->parameters_update(); @@ -401,6 +404,7 @@ VtolAttitudeControl::Run() #endif // !ENABLE_LOCKSTEP_SCHEDULER + const float dt = math::min((now - _last_run_timestamp) / 1e6f, kMaxVTOLAttitudeControlTimeStep); _last_run_timestamp = now; if (!_initialized) { @@ -415,6 +419,8 @@ VtolAttitudeControl::Run() } } + _vtol_type->setDt(dt); + perf_begin(_loop_perf); const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in); 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 39e7a9322e..9ad9b90356 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -92,6 +92,8 @@ using namespace time_literals; extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); +static constexpr float kMaxVTOLAttitudeControlTimeStep = 0.1f; // max time step [s] + class VtolAttitudeControl : public ModuleBase, public px4::WorkItem { public: @@ -249,6 +251,7 @@ private: param_t mpc_land_alt1; param_t mpc_land_alt2; param_t sys_ctrl_alloc; + param_t vt_spoiler_mc_ld; } _params_handles{}; hrt_abstime _last_run_timestamp{0}; 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 72c927c09e..c903ce070c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -384,3 +384,15 @@ PARAM_DEFINE_FLOAT(VT_PTCH_MIN, -5.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_LND_PTCH_MIN, -5.0f); + +/** + * Spoiler setting while landing (hover) + * + * @unit norm + * @min -1 + * @max 1 + * @decimal 1 + * @increment 0.05 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_SPOILER_MC_LD, 0.f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index bbf9bf1dba..2a03659e7b 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -136,6 +136,9 @@ bool VtolType::init() } } + _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRateVtol); + _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRateVtol); + return true; } @@ -157,6 +160,16 @@ void VtolType::update_mc_state() _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; + + float spoiler_setpoint_hover = 0.f; + + if (_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + spoiler_setpoint_hover = _params->vt_spoiler_mc_ld; + } + + _spoiler_setpoint_with_slewrate.update(spoiler_setpoint_hover, _dt); + _flaps_setpoint_with_slewrate.update(0.f, _dt); } void VtolType::update_fw_state() @@ -205,6 +218,9 @@ void VtolType::update_fw_state() } check_quadchute_condition(); + + _spoiler_setpoint_with_slewrate.update(_actuators_fw_in->control[actuator_controls_s::INDEX_SPOILERS], _dt); + _flaps_setpoint_with_slewrate.update(_actuators_fw_in->control[actuator_controls_s::INDEX_FLAPS], _dt); } void VtolType::update_transition_state() @@ -215,8 +231,6 @@ void VtolType::update_transition_state() _last_loop_ts = t_now; _throttle_blend_start_ts = t_now; - - check_quadchute_condition(); } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 1476a53d76..6d30489669 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -46,6 +46,10 @@ #include #include #include +#include + +static constexpr float kFlapSlewRateVtol = 1.f; // minimum time from none to full flap deflection [s] +static constexpr float kSpoilerSlewRateVtol = 1.f; // minimum time from none to full spoiler deflection [s] struct Params { int32_t ctrl_alloc; @@ -81,6 +85,7 @@ struct Params { int32_t vt_forward_thrust_enable_mode; float mpc_land_alt1; float mpc_land_alt2; + float vt_spoiler_mc_ld; }; // Has to match 1:1 msg/vtol_vehicle_status.msg @@ -207,6 +212,14 @@ public: virtual void parameters_update() = 0; + /** + * @brief Set current time delta + * + * @param dt Current time delta [s] + */ + void setDt(float dt) {_dt = dt; } + +protected: VtolAttitudeControl *_attc; mode _vtol_mode; @@ -292,6 +305,11 @@ public: float update_and_get_backtransition_pitch_sp(); + SlewRate _spoiler_setpoint_with_slewrate; + SlewRate _flaps_setpoint_with_slewrate; + + float _dt{0.0025f}; // time step [s] + private: