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

make flaps and flaperons continuous

Browse Source
This commit is contained in:
tumbili 2015-10-14 13:56:16 +02:00 committed by Roman
parent 36df3a0499
commit 1ae7221593
1 changed files with 41 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -161,6 +161,10 @@ private:
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
bool _debug; /**< if set to true, print debug output */
float _flaps_cmd_last;
float _flaperons_cmd_last;
struct {
float tconst;
float p_p;
@ -363,7 +367,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */
_setpoint_valid(false),
_debug(false)
_debug(false),
_flaps_cmd_last(0),
_flaperons_cmd_last(0)
{
/* safely initialize structs */
_ctrl_state = {};
@ -821,6 +827,8 @@ FixedwingAttitudeControl::task_main()
/* default flaps to center */
float flaps_control = 0.0f;
static float delta_flaps = 0;
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale;
@ -828,9 +836,25 @@ FixedwingAttitudeControl::task_main()
flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
}
// move the actual control value continuous with time
static hrt_abstime t_flaps_changed = 0;
if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) {
t_flaps_changed = hrt_absolute_time();
delta_flaps = flaps_control - _flaps_cmd_last;
_flaps_cmd_last = flaps_control;
}
static float flaps_applied = 0.0f;
if (fabsf(flaps_applied - flaps_control) > 0.01f) {
flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000;
}
/* default flaperon to center */
float flaperon = 0.0f;
static float delta_flaperon = 0.0f;
/* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) {
flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
@ -838,6 +862,20 @@ FixedwingAttitudeControl::task_main()
flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
}
// move the actual control value continuous with time
static hrt_abstime t_flaperons_changed = 0;
if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) {
t_flaperons_changed = hrt_absolute_time();
delta_flaperon = flaperon - _flaperons_cmd_last;
_flaperons_cmd_last = flaperon;
}
static float flaperon_applied = 0.0f;
if (fabsf(flaperon_applied - flaperon) > 0.01f) {
flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000;
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale around tuning airspeed */
@ -1141,9 +1179,9 @@ FixedwingAttitudeControl::task_main()
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
}
_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control;
_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied;
_actuators.control[5] = _manual.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied;
_actuators.control[7] = _manual.aux3;
/* lazily publish the setpoint only once available */