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

FW attitdue controller: use allocator status for anti-windup

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-08-12 16:22:57 +02:00
parent 6aab44e425
commit 57e052d02d
2 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -415,6 +415,50 @@ void FixedwingAttitudeControl::Run()
_wheel_ctrl.reset_integrator();
}
// update saturation status from control allocation feedback
control_allocator_status_s control_allocator_status;
if (_vehicle_status.is_vtol) {
if (_control_allocator_status_subs[1].update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
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;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
}
}
// TODO: send the unallocated value directly for better anti-windup
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
}
} else {
if (_control_allocator_status_subs[0].update(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
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;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
}
}
// TODO: send the unallocated value directly for better anti-windup
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
}
}
/* Prepare data for attitude controllers */
ECL_ControlData control_input{};
control_input.roll = euler_angles.phi();

4
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -56,12 +56,14 @@
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
@ -127,6 +129,8 @@ private:
uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
uORB::SubscriptionMultiArray<control_allocator_status_s, 2> _control_allocator_status_subs{ORB_ID::control_allocator_status};
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;