diff --git a/src/modules/fw_rate_control/CMakeLists.txt b/src/modules/fw_rate_control/CMakeLists.txt index 595c8314af..9845e3ee4e 100644 --- a/src/modules/fw_rate_control/CMakeLists.txt +++ b/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 diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 1f2cff00d6..8ed4070932 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/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); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index ec3b7e1c9e..d58a10cb7f 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -69,6 +69,8 @@ #include #include +#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); /** diff --git a/src/modules/fw_auto_trim/CMakeLists.txt b/src/modules/fw_rate_control/fw_auto_trim/CMakeLists.txt similarity index 89% rename from src/modules/fw_auto_trim/CMakeLists.txt rename to src/modules/fw_rate_control/fw_auto_trim/CMakeLists.txt index cc5be41293..5a1fb9584a 100644 --- a/src/modules/fw_auto_trim/CMakeLists.txt +++ b/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 - FwAutoTrim.cpp - FwAutoTrim.hpp - DEPENDS - hysteresis - mathlib - px4_work_queue +px4_add_library(FwAutoTrim + FwAutoTrim.cpp ) + +# target_link_libraries(FwAutoTrim) +target_include_directories(FwAutoTrim PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/fw_auto_trim/FwAutoTrim.cpp b/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp similarity index 73% rename from src/modules/fw_auto_trim/FwAutoTrim.cpp rename to src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.cpp index 6375a0723c..03c8df8ffe 100644 --- a/src/modules/fw_auto_trim/FwAutoTrim.cpp +++ b/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 ×tamp_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); -} diff --git a/src/modules/fw_auto_trim/FwAutoTrim.hpp b/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp similarity index 83% rename from src/modules/fw_auto_trim/FwAutoTrim.hpp rename to src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp index 94d8633df1..0ccb9fb563 100644 --- a/src/modules/fw_auto_trim/FwAutoTrim.hpp +++ b/src/modules/fw_rate_control/fw_auto_trim/FwAutoTrim.hpp @@ -41,10 +41,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -55,41 +53,24 @@ using namespace time_literals; -class FwAutoTrim : public ModuleBase, 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 ×tamp_sample); uORB::Publication _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}; diff --git a/src/modules/fw_auto_trim/Kconfig b/src/modules/fw_rate_control/fw_auto_trim/Kconfig similarity index 100% rename from src/modules/fw_auto_trim/Kconfig rename to src/modules/fw_rate_control/fw_auto_trim/Kconfig