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

fixed-wing: update rate controller integrator handling

Browse Source 
-always reset roll/pitch/yaw integrators at the same time
-reset them while waiting for launch or during FW Takeoff before Climbout
-reset wheel rate integrator only when disarmed

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-08-12 14:54:21 +02:00
parent cd080289c6
commit 5a2127d026
6 changed files with 17 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+4 -17
View File
@@ -908,8 +908,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
publishOrbitStatus(current_sp);
}
const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
switch (position_sp_type) {
case position_setpoint_s::SETPOINT_TYPE_IDLE:
_att_sp.thrust_body[0] = 0.0f;
@@ -933,11 +931,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
break;
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
}
/* Copy thrust output for publication, handle special cases */
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
@@ -1482,8 +1475,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
if (_runway_takeoff.resetIntegrators()) {
// reset integrals except yaw (which also counts for the wheel controller)
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.reset_rate_integrals = true;
// throttle is open loop anyway during ground roll, no need to wind up the integrator
_tecs.resetIntegrals();
@@ -1621,11 +1613,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_att_sp.pitch_body = get_tecs_pitch();
} else {
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Tell the attitude controller to stop integrating while we are waiting for the launch */
_att_sp.reset_rate_integrals = true;
/* Set default roll and pitch setpoints during detection phase */
_att_sp.roll_body = 0.0f;
@@ -2234,9 +2223,7 @@ FixedwingPositionControl::Run()
_npfg.setPeriod(_param_npfg_period.get());
_l1_control.set_l1_period(_param_fw_l1_period.get());
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
_att_sp.reset_rate_integrals = false;
// by default we don't want yaw to be contoller directly with rudder
_att_sp.fw_control_yaw = false;