Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

FW Rate Controller: fix saturation logic for VTOLs with differential thrust enabled #21713

Merged
merged 3 commits into from
Jun 19, 2023
Merged

FW Rate Controller: fix saturation logic for VTOLs with differential thrust enabled #21713

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c73f737
RateControl: allow setting individual saturation flags
MaEtUgR Jun 14, 2023
31a18e5
FixedwingRateControl: rework VTOL differential thrust saturation
MaEtUgR Jun 12, 2023
5ce63a6
FWRateController: use param find for VT_DIFTHR_EN as pure FW build do…
sfuhrer Jun 16, 2023
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
18 changes: 16 additions & 2 deletions src/lib/rate_control/rate_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,13 +47,27 @@ void RateControl::setPidGains(const Vector3f &P, const Vector3f &I, const Vector
_gain_d = D;
}

void RateControl::setSaturationStatus(const Vector<bool, 3> &saturation_positive,
const Vector<bool, 3> &saturation_negative)
void RateControl::setSaturationStatus(const Vector3<bool> &saturation_positive,
const Vector3<bool> &saturation_negative)
{
_control_allocator_saturation_positive = saturation_positive;
_control_allocator_saturation_negative = saturation_negative;
}

void RateControl::setPositiveSaturationFlag(size_t axis, bool is_saturated)
{
if (axis < 3) {
_control_allocator_saturation_positive(axis) = is_saturated;
}
}

void RateControl::setNegativeSaturationFlag(size_t axis, bool is_saturated)
{
if (axis < 3) {
_control_allocator_saturation_negative(axis) = is_saturated;
}
}

Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
const float dt, const bool landed)
{
Expand Down
12 changes: 10 additions & 2 deletions src/lib/rate_control/rate_control.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,16 @@ class RateControl
* Set saturation status
* @param control saturation vector from control allocator
*/
void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
const matrix::Vector<bool, 3> &saturation_negative);
void setSaturationStatus(const matrix::Vector3<bool> &saturation_positive,
const matrix::Vector3<bool> &saturation_negative);

/**
* Set individual saturation flags
* @param axis 0 roll, 1 pitch, 2 yaw
* @param is_saturated value to update the flag with
*/
void setPositiveSaturationFlag(size_t axis, bool is_saturated);
void setNegativeSaturationFlag(size_t axis, bool is_saturated);

/**
* Run one control loop cycle calculation
Expand Down
51 changes: 36 additions & 15 deletions src/modules/fw_rate_control/FixedwingRateControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,8 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) :
_vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_handle_param_vt_fw_difthr_en = param_find("VT_FW_DIFTHR_EN");

/* fetch initial parameter values */
parameters_update();

Expand Down Expand Up @@ -86,6 +88,10 @@ FixedwingRateControl::parameters_update()
// set FF gains to 0 as we add the FF control outside of the rate controller
Vector3f(0.f, 0.f, 0.f));

if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) {
param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en);
}


return PX4_OK;
}
Expand Down Expand Up @@ -278,26 +284,41 @@ void FixedwingRateControl::Run()
_rate_control.resetIntegral();
}

// update saturation status from control allocation feedback
control_allocator_status_s control_allocator_status;

if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
// Update saturation status from control allocation feedback
// TODO: send the unallocated value directly for better anti-windup
Vector3<bool> diffthr_enabled(
_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::ROLL_BIT),
_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::PITCH_BIT),
_param_vt_fw_difthr_en & static_cast<int32_t>(VTOLFixedWingDifferentialThrustEnabledBit::YAW_BIT)
);

if (_vehicle_status.is_vtol_tailsitter) {
// Swap roll and yaw
diffthr_enabled.swapRows(0, 2);
}

if (!control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
// saturation handling for axis controlled by differential thrust (VTOL only)
control_allocator_status_s control_allocator_status;

} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
// Set saturation flags for VTOL differential thrust feature
// If differential thrust is enabled in an axis, assume it's the only torque authority and only update saturation using matrix 0 allocating the motors.
if (_control_allocator_status_subs[0].update(&control_allocator_status)) {
for (size_t i = 0; i < 3; i++) {
if (diffthr_enabled(i)) {
_rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON);
_rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON);
}
}
}

// TODO: send the unallocated value directly for better anti-windup
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
// Set saturation flags for control surface controlled axes
if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
for (size_t i = 0; i < 3; i++) {
if (!diffthr_enabled(i)) {
_rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON);
_rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON);
}
}
}

/* bi-linear interpolation over airspeed for actuator trim scheduling */
Expand Down
10 changes: 10 additions & 0 deletions src/modules/fw_rate_control/FixedwingRateControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,16 @@ class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, publ

bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states

// enum for bitmask of VT_FW_DIFTHR_EN parameter options
enum class VTOLFixedWingDifferentialThrustEnabledBit : int32_t {
YAW_BIT = (1 << 0),
ROLL_BIT = (1 << 1),
PITCH_BIT = (1 << 2),
};

param_t _handle_param_vt_fw_difthr_en{PARAM_INVALID};
int32_t _param_vt_fw_difthr_en{0};

DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
Expand Down
Loading