migrate fw_auto_trim module to fw_rate_control

src/modules/fw_rate_control/CMakeLists.txt

@@ -30,6 +30,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+add_subdirectory(fw_auto_trim)
 
 px4_add_module(
 	MODULE modules__fw_rate_control
@@ -38,6 +39,7 @@ px4_add_module(
 		FixedwingRateControl.cpp
 		FixedwingRateControl.hpp
 	DEPENDS
+		FwAutoTrim
 		px4_work_queue
 		RateControl
 		SlewRate

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -432,6 +432,11 @@ void FixedwingRateControl::Run()
 				_vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
 				_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
 			}
+
+			_auto_trim.update(_vehicle_torque_setpoint, dt);
+
+		} else {
+			_auto_trim.reset();
 		}
 
 		updateActuatorControlsStatus(dt);

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -69,6 +69,8 @@
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/vehicle_torque_setpoint.h>
 
+#include "fw_auto_trim/FwAutoTrim.hpp"
+
 using matrix::Eulerf;
 using matrix::Quatf;
 
@@ -209,6 +211,8 @@ private:
 
 	RateControl _rate_control; ///< class for rate control calculations
 
+	FwAutoTrim _auto_trim{this};
+
 	void updateActuatorControlsStatus(float dt);
 
 	/**

src/modules/fw_rate_control/fw_auto_trim/CMakeLists.txt

@@ -30,17 +30,9 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-px4_add_module(
-	MODULE modules__fw_auto_trim
-	MAIN fw_auto_trim
-	COMPILE_FLAGS
-		${MAX_CUSTOM_OPT_LEVEL}
-	SRCS
+px4_add_library(FwAutoTrim
 	FwAutoTrim.cpp
-		FwAutoTrim.hpp
-	DEPENDS
-		hysteresis
-		mathlib
-		px4_work_queue
 )
+
+# target_link_libraries(FwAutoTrim)
+target_include_directories(FwAutoTrim PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp

@@ -38,13 +38,11 @@
 using namespace time_literals;
 using matrix::Vector3f;
 
-FwAutoTrim::FwAutoTrim(bool is_vtol) :
-	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
-	_vehicle_torque_setpoint_sub(this, ORB_ID(vehicle_torque_setpoint), is_vtol ? 1 : 0)
+FwAutoTrim::FwAutoTrim(ModuleParams *parent) :
+	ModuleParams(parent)
 {
+	_auto_trim_status_pub.advertise();
 	updateParams();
-	reset();
 }
 
 FwAutoTrim::~FwAutoTrim()
@@ -52,35 +50,18 @@ FwAutoTrim::~FwAutoTrim()
 	perf_free(_cycle_perf);
 }
 
-bool FwAutoTrim::init()
-{
-	if (!_vehicle_torque_setpoint_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
-	_auto_trim_status_pub.advertise();
-
-	return true;
-}
-
-void FwAutoTrim::reset()
-{
-}
-
 void FwAutoTrim::updateParams()
 {
 	ModuleParams::updateParams();
 }
 
-void FwAutoTrim::Run()
+void FwAutoTrim::reset()
 {
-	if (should_exit()) {
-		_vehicle_torque_setpoint_sub.unregisterCallback();
-		exit_and_cleanup();
-		return;
+	_state = state::idle;
 }
 
+void FwAutoTrim::update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, const float dt)
+{
 	if (_vehicle_land_detected_sub.updated()) {
 		vehicle_land_detected_s vehicle_land_detected;
 
@@ -89,24 +70,6 @@ void FwAutoTrim::Run()
 		}
 	}
 
-
-	vehicle_torque_setpoint_s vehicle_torque_setpoint;
-
-	if (!_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)
-	    || (vehicle_torque_setpoint.timestamp == _timestamp_last)) {
-		return;
-	}
-
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		updateParams();
-	}
-
 	perf_begin(_cycle_perf);
 
 	if (_vehicle_status_sub.updated()) {
@@ -136,9 +99,6 @@ void FwAutoTrim::Run()
 		}
 	}
 
-	const float dt = (vehicle_torque_setpoint.timestamp - _timestamp_last) * 1e-6f;
-	_timestamp_last = vehicle_torque_setpoint.timestamp;
-
 	const hrt_abstime now = hrt_absolute_time();
 	const Vector3f torque{vehicle_torque_setpoint.xyz};
 
@@ -234,42 +194,6 @@ void FwAutoTrim::publishStatus(const hrt_abstime &timestamp_sample)
 	_auto_trim_status_pub.publish(status_msg);
 }
 
-int FwAutoTrim::task_spawn(int argc, char *argv[])
-{
-	bool is_vtol = false;
-
-	if (argc > 1) {
-		if (strcmp(argv[1], "vtol") == 0) {
-			is_vtol = true;
-		}
-	}
-
-	FwAutoTrim *instance = new FwAutoTrim(is_vtol);
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int FwAutoTrim::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
 int FwAutoTrim::print_status()
 {
 	perf_print_counter(_cycle_perf);
@@ -279,27 +203,3 @@ int FwAutoTrim::print_status()
 
 	return 0;
 }
-
-int FwAutoTrim::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("fw_auto_trim", "auto-trim");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int fw_auto_trim_main(int argc, char *argv[])
-{
-	return FwAutoTrim::main(argc, argv);
-}

src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp

@@ -41,10 +41,8 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
-#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/auto_trim_status.h>
 #include <uORB/topics/parameter_update.h>
@@ -55,41 +53,24 @@
 
 using namespace time_literals;
 
-class FwAutoTrim : public ModuleBase<FwAutoTrim>, public ModuleParams,
-	public px4::WorkItem
+class FwAutoTrim : public ModuleParams
 {
 public:
-	FwAutoTrim(bool is_vtol);
-	~FwAutoTrim() override;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	bool init();
-
-	/** @see ModuleBase::print_status() */
-	int print_status() override;
-
-private:
-	void Run() override;
-	void updateParams() override;
+	FwAutoTrim(ModuleParams *parent);
+	~FwAutoTrim();
 
 	void reset();
+	void update(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
+	int print_status();
 
+protected:
+	void updateParams() override;
+
+private:
 	void publishStatus(const hrt_abstime &timestamp_sample);
 
 	uORB::Publication<auto_trim_status_s> _auto_trim_status_pub{ORB_ID(auto_trim_status)};
 
-	uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub;
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
@@ -104,7 +85,6 @@ private:
 		fail = auto_trim_status_s::STATE_FAIL,
 	} _state{state::idle};
 
-	hrt_abstime _timestamp_last{0};
 	hrt_abstime _state_start_time{0};
 
 	bool _armed{false};