diff --git a/msg/Rpm.msg b/msg/Rpm.msg index ca69e50fdb..b4de2cf422 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +# rpm values of 0.0 mean within a timeout there is no movement measured float32 rpm_estimate # filtered revolutions per minute float32 rpm_raw diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp index f61a392ad4..53d9766e2d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -68,11 +68,15 @@ float RpmControl::getActuatorCorrection() const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); + + _rpm_invalid = rpm.rpm_estimate < 1.f; } } // Timeout - if (now > _timestamp_last_measurement + 1_s) { + const bool timeout = now > _timestamp_last_measurement + 1_s; + + if (_rpm_invalid || timeout) { _pid.resetIntegral(); _actuator_correction = 0.f; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index b412445048..5fd0c96d91 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -63,6 +63,7 @@ private: static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + bool _rpm_invalid{true}; PID _pid; float _spoolup_progress{0.f}; // [0,1] hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout