Skip to content
Merged
Show file tree
Hide file tree
Changes from 2 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions docs/en/config/actuators.md
Original file line number Diff line number Diff line change
Expand Up @@ -175,6 +175,8 @@ The fields are:

#### Flap Scale and Spoiler Scale Configuration

The channel setup of flaps and spoilers is described in [Flaps and Spoiler Control with Manual Control](../payloads/generic_actuator_control.md#flaps-and-spoiler-control-with-manual-control).

"Flap-control" and "Spoiler-control" are aerodynamic configurations that can either be commanded manually by the pilot (using RC, say), or are set automatically by the controller.
For example, a pilot or the landing system might engage "Spoiler-control" in order to reduce the airspeed before landing.

Expand Down
14 changes: 14 additions & 0 deletions docs/en/payloads/generic_actuator_control.md
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,20 @@ To map a particular RC channel to an output function `RC AUX n` (and hence it's
For example, to control an actuator attached to AUX pin 3 (say) you would assign the output function `RC AUX 5` to the output `AUX3`.
You could then use set the RC channel to control the `AUX3` output using `RC_MAP_AUX5`.


### Flaps and Spoiler Control with Manual Control

The preferred method to manually actuate spoilers and flaps is to map a manual control switch to an `AUX` output (see [Generic Actuator Control with RC](#generic-actuator-control-with-rc)), and then map that AUX output to the flap or spoiler function using [FW_FLAPS_MAN](../advanced_config/parameter_reference.md#FW_FLAPS_MAN) or [FW_SPOILERS_MAN](../advanced_config/parameter_reference.md#FW_SPOILERS_MAN).
The source for the manual control can be RC or MAVLink.

::: warning
Alternatively, you can define a flaps channel directly on the RC using [RC_MAP_FLAPS](../advanced_config/parameter_reference.md#RC_MAP_FLAPS).
This channel can also be used to control the spoilers by setting [FW_SPOILERS_MAN](../advanced_config/parameter_reference.md#FW_SPOILERS_MAN) to `Flaps channel`.
This method is not possible when the source for the manual control is MAVLink.
This method is not recommended for new setups, and will be removed in a future release.
Use the AUX-based method instead.
:::

## Generic Actuator Control in Missions

To use generic actuator control in a mission you must first [configure the outputs that you want to control using MAVLink](#generic-actuator-control-with-mavlink).
Expand Down
48 changes: 42 additions & 6 deletions src/modules/fw_rate_control/FixedwingRateControl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -469,9 +469,35 @@ void FixedwingRateControl::Run()
// Flaps control
float flaps_control = 0.f; // default to no flaps

/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.flaps)) {
flaps_control = math::max(_manual_control_setpoint.flaps, 0.f); // do not consider negative switch settings
switch (_param_fw_flaps_man.get()) { // do not consider negative switch settings
case 0:
break;

case 1:
flaps_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? math::max(_manual_control_setpoint.aux1, 0.f) : 0.f;
break;

case 2:
flaps_control = PX4_ISFINITE(_manual_control_setpoint.aux2) ? math::max(_manual_control_setpoint.aux2, 0.f) : 0.f;
break;

case 3:
flaps_control = PX4_ISFINITE(_manual_control_setpoint.aux3) ? math::max(_manual_control_setpoint.aux3, 0.f) : 0.f;
break;

case 4:
flaps_control = PX4_ISFINITE(_manual_control_setpoint.aux4) ? math::max(_manual_control_setpoint.aux4, 0.f) : 0.f;
break;

case 5:
flaps_control = PX4_ISFINITE(_manual_control_setpoint.aux5) ? math::max(_manual_control_setpoint.aux5, 0.f) : 0.f;
break;

case 6:
flaps_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? math::max(_manual_control_setpoint.flaps, 0.f) : 0.f;
break;


}

normalized_unsigned_setpoint_s flaps_setpoint;
Expand All @@ -482,19 +508,29 @@ void FixedwingRateControl::Run()
// Spoilers control
float spoilers_control = 0.f; // default to no spoilers

switch (_param_fw_spoilers_man.get()) {
switch (_param_fw_spoilers_man.get()) { // do not consider negative switch settings
case 0:
break;

case 1:
// do not consider negative switch settings
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? math::max(_manual_control_setpoint.flaps, 0.f) : 0.f;
break;

case 2:
// do not consider negative switch settings
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? math::max(_manual_control_setpoint.aux1, 0.f) : 0.f;
break;

case 3:
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.aux2) ? math::max(_manual_control_setpoint.aux2, 0.f) : 0.f;
break;

case 4:
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.aux3) ? math::max(_manual_control_setpoint.aux3, 0.f) : 0.f;
break;

case 5:
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.aux4) ? math::max(_manual_control_setpoint.aux4, 0.f) : 0.f;
break;
}

normalized_unsigned_setpoint_s spoilers_setpoint;
Expand Down
3 changes: 2 additions & 1 deletion src/modules/fw_rate_control/FixedwingRateControl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -213,7 +213,8 @@ class FixedwingRateControl final : public ModuleBase, public ModuleParams,
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,

(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
(ParamInt<px4::params::FW_FLAPS_MAN>) _param_fw_flaps_man
)

RateControl _rate_control; ///< class for rate control calculations
Expand Down
18 changes: 18 additions & 0 deletions src/modules/fw_rate_control/fw_rate_control_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -326,6 +326,24 @@ parameters:
0: Disabled
1: Flaps channel
2: Aux1
3: Aux2
4: Aux3
5: Aux4
6: Aux5
default: 0
FW_FLAPS_MAN:
description:
short: Flap input in manual flight
long: Chose source for manual setting of flaps in manual flight modes.
type: enum
values:
0: Disabled
1: Aux1
2: Aux2
3: Aux3
4: Aux4
5: Aux5
6: Flaps channel
default: 0
FW_ACRO_YAW_EN:
description:
Expand Down
Loading