fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 23:09:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL: add flap and spoiler support

Browse Source 
- including slew rate limiting
- adds option to set spoiler setting during the Land phase (hover)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-04-27 17:06:36 +02:00
parent 4b8f93de5c
commit 64ff31aa08
8 changed files with 83 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/vtol_att_control/CMakeLists.txt
View File

@ -39,5 +39,7 @@ px4_add_module(
vtol_type.cpp
tailsitter.cpp
standard.cpp
DEPENDS
SlewRate
)

15
src/modules/vtol_att_control/standard.cpp
View File

@ -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;
}

18
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -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;

6
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -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);

3
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -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<VtolAttitudeControl>, 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};

12
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -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);

18
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -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();
}

18
src/modules/vtol_att_control/vtol_type.h
View File

@ -46,6 +46,10 @@
#include <lib/mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/slew_rate/SlewRate.hpp>
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<float> _spoiler_setpoint_with_slewrate;
SlewRate<float> _flaps_setpoint_with_slewrate;
float _dt{0.0025f}; // time step [s]
private: