From 69d6fc2a7fe5cf7ac565e4362642e17a250128d4 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Mon, 22 Aug 2022 11:07:03 +0200 Subject: [PATCH] Do not use a differential input --- .../rover_pos_control/RoverPositionControl.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 68e5c2421f..a8032d711c 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -442,13 +442,19 @@ RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, con const matrix::Vector3f rates_setpoint(rates_sp.roll, rates_sp.pitch, rates_sp.yaw); const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz); - bool lock_integrator = bool(current_velocity.norm() < _param_rate_i_minspeed.get()); + + bool lock_integrator = false; + + if (current_velocity.norm() < _param_rate_i_minspeed.get()) { + _rate_control.resetIntegral(); + } const matrix::Vector3f angular_acceleration{acc.xyz}; - const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt, - lock_integrator); + const matrix::Vector3f desired_angular_acceleration = _rate_control.update(vehicle_rates, rates_setpoint, + angular_acceleration, dt, + lock_integrator); - _steering_input = math::constrain(_steering_input + torque(2), -1.0f, 1.0f); + _steering_input = math::constrain(desired_angular_acceleration(2), -1.0f, 1.0f); _act_controls.control[actuator_controls_s::INDEX_YAW] = _steering_input;