fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 19:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW: rework spoiler/flap control logic

Browse Source 
- remove separate flaperon controls input to mixer instead enable spoiler support
- add slew rate limiting on both flap and spoiler controls
- add spoiler configuration for Landing and Descend
- add trimming from spoiler deflection
- FW Attitude control: remove FW_FLAPS_SCL param -->
    The flap settings for takeoff and landing are now specified relative to full range.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-05-03 15:11:21 +02:00
parent 21a2892f47
commit 4b8f93de5c
14 changed files with 167 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+49 -39
View File
@@ -67,6 +67,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
_rate_ctrl_status_pub.advertise();
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate);
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
@@ -382,11 +384,14 @@ void FixedwingAttitudeControl::Run()
/* if we are in rotary wing mode, do nothing */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
_spoiler_setpoint_with_slewrate.setForcedValue(0.f);
_flaps_setpoint_with_slewrate.setForcedValue(0.f);
perf_end(_loop_perf);
return;
}
control_flaps(dt);
controlFlaps(dt);
controlSpoilers(dt);
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
@@ -498,8 +503,11 @@ void FixedwingAttitudeControl::Run()
}
/* add trim increment if flaps are deployed */
trim_roll += _flaps_applied * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_applied * _param_fw_dtrim_p_flps.get();
trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get();
trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get();
// add trim increment from spoilers (only pitch)
trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get();
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
@@ -645,11 +653,11 @@ void FixedwingAttitudeControl::Run()
_actuators.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[5] = _manual_control_setpoint.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
_actuators.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[7] = _manual_control_setpoint.aux3;
_actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
@@ -697,18 +705,16 @@ void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime &timestam
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void FixedwingAttitudeControl::control_flaps(const float dt)
void FixedwingAttitudeControl::controlFlaps(const float dt)
{
/* default flaps to center */
float flap_control = 0.0f;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
flap_control = _manual_control_setpoint.flaps * _param_fw_flaps_scl.get();
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
flap_control = _manual_control_setpoint.flaps;
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
} else if (_vcontrol_mode.flag_control_auto_enabled) {
switch (_att_sp.apply_flaps) {
case vehicle_attitude_setpoint_s::FLAPS_OFF:
@@ -716,50 +722,54 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
break;
case vehicle_attitude_setpoint_s::FLAPS_LAND:
flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_lnd_scl.get();
flap_control = _param_fw_flaps_lnd_scl.get();
break;
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_to_scl.get();
flap_control = _param_fw_flaps_to_scl.get();
break;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
_flaps_applied += (_flaps_applied - flap_control) < 0 ? dt : -dt;
_flaps_setpoint_with_slewrate.update(flap_control, dt);
}
} else {
_flaps_applied = flap_control;
}
void FixedwingAttitudeControl::controlSpoilers(const float dt)
{
float spoiler_control = 0.f;
/* default flaperon to center */
float flaperon_control = 0.0f;
if (_vcontrol_mode.flag_control_manual_enabled) {
switch (_param_fw_spoilers_man.get()) {
case 0:
break;
/* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
case 1:
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f;
break;
flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
case 2:
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? _manual_control_setpoint.aux1 : 0.f;
break;
}
} else if (_vcontrol_mode.flag_control_auto_enabled
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
} else if (_vcontrol_mode.flag_control_auto_enabled) {
switch (_att_sp.apply_spoilers) {
case vehicle_attitude_setpoint_s::SPOILERS_OFF:
spoiler_control = 0.f;
break;
if (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) {
flaperon_control = _param_fw_flaperon_scl.get();
case vehicle_attitude_setpoint_s::SPOILERS_LAND:
spoiler_control = _param_fw_spoilers_lnd.get();
break;
} else {
flaperon_control = 0.0f;
case vehicle_attitude_setpoint_s::SPOILERS_DESCEND:
spoiler_control = _param_fw_spoilers_desc.get();
break;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt;
} else {
_flaperons_applied = flaperon_control;
}
_spoiler_setpoint_with_slewrate.update(spoiler_control, dt);
}
void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt)