TEMPORARY

PX4 · Dec 17, 2024 · 29c13d1 · 29c13d1
1 parent a687a86
commit 29c13d1
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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);