From fafbb687d8d504247996988e6cbafcb40382a5b0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 24 Oct 2020 17:27:07 +0200 Subject: [PATCH] FlightModeManager: fix integral reset on ground This information could also be used for yaw and integral resets of the lower level controllers. --- msg/vehicle_constraints.msg | 1 + src/modules/flight_mode_manager/FlightModeManager.cpp | 2 +- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 4 ++++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_constraints.msg b/msg/vehicle_constraints.msg index 0982e09165..5c598a4659 100644 --- a/msg/vehicle_constraints.msg +++ b/msg/vehicle_constraints.msg @@ -11,4 +11,5 @@ float32 tilt # in radians [0, PI] float32 min_distance_to_ground # in meters float32 max_distance_to_ground # in meters bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) +bool reset_integral # tells controller to reset integrators e.g. since we know the vehicle is not in air float32 minimum_thrust # tell controller what the minimum collective output thrust should be diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 2647ca9ff8..66be8ac5f1 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -454,7 +454,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, // set yaw-sp to current yaw setpoint.yawspeed = 0.f; // prevent any integrator windup - // _control.resetIntegral(); TODO + constraints.reset_integral = true; } _trajectory_setpoint_pub.publish(setpoint); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 5865d119a9..cec1b2a647 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -254,6 +254,10 @@ void MulticopterPositionControl::Run() _control.setConstraints(constraints); _control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get()); + if (constraints.reset_integral) { + _control.resetIntegral(); + } + // Run position control if (_control.update(dt)) { _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now);