From 29c13d17052432734cf42edd973c9f1b72243096 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 14:05:23 +0100 Subject: [PATCH] TEMPORARY --- ROMFS/px4fmu_common/init.d/rc.heli_defaults | 1 + boards/px4/fmu-v5x/default.px4board | 1 + .../control_allocator/ActuatorEffectiveness/RpmControl.cpp | 7 +++++-- .../control_allocator/ActuatorEffectiveness/RpmControl.hpp | 1 + src/modules/logger/logged_topics.cpp | 4 ++-- 5 files changed, 10 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults index 42d2aca2685e..53ed12fc859c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.heli_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.heli_defaults @@ -12,6 +12,7 @@ param set-default MAV_TYPE 4 param set-default COM_PREARM_MODE 2 param set-default COM_SPOOLUP_TIME 10 +param set-default COM_DISARM_PRFLT -1 # No need for minimum collective pitch (or airmode) to keep torque authority param set-default MPC_MANTHR_MIN 0 diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 8ba41726c086..5b1cca7a2419 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -42,6 +42,7 @@ CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_RPM_CAPTURE=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp index f61a392ad492..064d936f50be 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -68,11 +68,14 @@ 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 b412445048bd..5fd0c96d9115 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -63,6 +63,7 @@ class RpmControl : public ModuleParams 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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index fcf0e6494239..46b5129be020 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -156,7 +156,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("control_allocator_status", 200, 2); add_optional_topic_multi("rate_ctrl_status", 200, 2); add_optional_topic_multi("sensor_hygrometer", 500, 4); - add_optional_topic_multi("rpm", 200); + add_optional_topic_multi("rpm"); add_topic_multi("timesync_status", 1000, 3); add_optional_topic_multi("telemetry_status", 1000, 4); @@ -203,7 +203,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("pps_capture"); // additional control allocation logging - add_topic("actuator_motors", 100); + add_topic("actuator_motors"); add_topic("actuator_servos", 100); add_topic_multi("vehicle_thrust_setpoint", 20, 2); add_topic_multi("vehicle_torque_setpoint", 20, 2);