From 3c0fa76e2f7768d48764052e94113803fc4a61a7 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Wed, 23 Feb 2022 14:20:33 +0100 Subject: [PATCH] Make LandingTargetEstimator a lib and run on work_queue Currently only precision landing is using the LandingTargetEstimator and it is a waste to have it running in the background all the time. With this change the estimator is only started once the precision landing flight task is activated, and stopped when the flight task is deactivated. --- boards/px4/sitl/default.px4board | 1 - src/lib/CMakeLists.txt | 1 + .../landing_target_estimator/CMakeLists.txt | 20 +-- .../LandingTargetEstimator.cpp | 51 +++++- .../LandingTargetEstimator.h | 29 ++-- .../landing_target_estimator_main.cpp | 153 ------------------ .../tasks/AutoPrecisionLanding/CMakeLists.txt | 14 +- .../FlightTaskAutoPrecisionLanding.cpp | 10 ++ .../FlightTaskAutoPrecisionLanding.hpp | 7 +- 9 files changed, 107 insertions(+), 179 deletions(-) delete mode 100644 src/lib/landing_target_estimator/landing_target_estimator_main.cpp diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index f85f30f424..0783f6e2ed 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -23,7 +23,6 @@ CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y -CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y CONFIG_MODULES_LOGGER=y diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f6bcc9260b..3c7aa69d9d 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -50,6 +50,7 @@ add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL) add_subdirectory(geo EXCLUDE_FROM_ALL) add_subdirectory(hysteresis EXCLUDE_FROM_ALL) add_subdirectory(l1 EXCLUDE_FROM_ALL) +add_subdirectory(landing_target_estimator EXCLUDE_FROM_ALL) add_subdirectory(led EXCLUDE_FROM_ALL) add_subdirectory(matrix EXCLUDE_FROM_ALL) add_subdirectory(mathlib EXCLUDE_FROM_ALL) diff --git a/src/lib/landing_target_estimator/CMakeLists.txt b/src/lib/landing_target_estimator/CMakeLists.txt index 146869aa3f..f30c4073f4 100644 --- a/src/lib/landing_target_estimator/CMakeLists.txt +++ b/src/lib/landing_target_estimator/CMakeLists.txt @@ -31,14 +31,14 @@ # ############################################################################ -px4_add_module( - MODULE modules__landing_target_estimator - MAIN landing_target_estimator - COMPILE_FLAGS - SRCS - landing_target_estimator_main.cpp - LandingTargetEstimator.cpp - KalmanFilter.cpp - DEPENDS - ) +px4_add_library(LandingTargetEstimator + LandingTargetEstimator.cpp + KalmanFilter.cpp +) +target_compile_options(LandingTargetEstimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +target_link_libraries(LandingTargetEstimator + PUBLIC + px4_work_queue +) diff --git a/src/lib/landing_target_estimator/LandingTargetEstimator.cpp b/src/lib/landing_target_estimator/LandingTargetEstimator.cpp index 3783c3d852..46dbbe562a 100644 --- a/src/lib/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/lib/landing_target_estimator/LandingTargetEstimator.cpp @@ -50,7 +50,9 @@ namespace landing_target_estimator { -LandingTargetEstimator::LandingTargetEstimator() +LandingTargetEstimator::LandingTargetEstimator() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { _paramHandle.acc_unc = param_find("LTEST_ACC_UNC"); _paramHandle.meas_unc = param_find("LTEST_MEAS_UNC"); @@ -63,7 +65,54 @@ LandingTargetEstimator::LandingTargetEstimator() _paramHandle.offset_x = param_find("LTEST_SENS_POS_X"); _paramHandle.offset_y = param_find("LTEST_SENS_POS_Y"); _paramHandle.offset_z = param_find("LTEST_SENS_POS_Z"); +} + +LandingTargetEstimator::~LandingTargetEstimator() +{ + Stop(); +} + +bool LandingTargetEstimator::Start() +{ + // force initialize parameters update _check_params(true); + + // messages needed for estimation + if (!_vehicleLocalPositionSub.registerCallback()) { + PX4_ERR("landing_target_estimator callback registration failed"); + } + + if (!_attitudeSub.registerCallback()) { + PX4_ERR("landing_target_estimator callback registration failed"); + } + + if (!_vehicle_acceleration_sub.registerCallback()) { + PX4_ERR("landing_target_estimator callback registration failed"); + } + + if (!_irlockReportSub.registerCallback()) { + PX4_ERR("landing_target_estimator callback registration failed"); + } + + return true; +} + +void LandingTargetEstimator::Stop() +{ + // clear all registered callbacks + _vehicleLocalPositionSub.unregisterCallback(); + _attitudeSub.unregisterCallback(); + _vehicle_acceleration_sub.unregisterCallback(); + _irlockReportSub.unregisterCallback(); + + Deinit(); +} + +void LandingTargetEstimator::Run() +{ + ScheduleDelayed(10_ms); + + update(); } void LandingTargetEstimator::update() diff --git a/src/lib/landing_target_estimator/LandingTargetEstimator.h b/src/lib/landing_target_estimator/LandingTargetEstimator.h index 521a1e3763..23ac9913f5 100644 --- a/src/lib/landing_target_estimator/LandingTargetEstimator.h +++ b/src/lib/landing_target_estimator/LandingTargetEstimator.h @@ -42,12 +42,12 @@ #pragma once -#include #include #include #include #include #include +#include #include #include #include @@ -58,10 +58,16 @@ #include #include #include + #include #include #include #include + +#include +#include +#include + #include "KalmanFilter.h" using namespace time_literals; @@ -69,19 +75,24 @@ using namespace time_literals; namespace landing_target_estimator { -class LandingTargetEstimator +class LandingTargetEstimator : public ModuleParams, public px4::ScheduledWorkItem { public: LandingTargetEstimator(); - virtual ~LandingTargetEstimator() = default; + ~LandingTargetEstimator() override; /* * Get new measurements and update the state estimate */ - void update(); + + bool Start(); + void Stop(); protected: + void Run() override; + + void update(); /* * Update uORB topics. @@ -149,11 +160,11 @@ private: float rel_pos_z; } _target_position_report; - uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)}; - uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)}; + uORB::SubscriptionCallbackWorkItem _vehicleLocalPositionSub{this, ORB_ID(vehicle_local_position)}; + uORB::SubscriptionCallbackWorkItem _attitudeSub{this, ORB_ID(vehicle_attitude)}; + uORB::SubscriptionCallbackWorkItem _vehicle_acceleration_sub{this, ORB_ID(vehicle_acceleration)}; + uORB::SubscriptionCallbackWorkItem _irlockReportSub{this, ORB_ID(irlock_report)}; + uORB::SubscriptionCallbackWorkItem _uwbDistanceSub{this, ORB_ID(uwb_distance)}; vehicle_local_position_s _vehicleLocalPosition{}; vehicle_attitude_s _vehicleAttitude{}; diff --git a/src/lib/landing_target_estimator/landing_target_estimator_main.cpp b/src/lib/landing_target_estimator/landing_target_estimator_main.cpp deleted file mode 100644 index bb666600d1..0000000000 --- a/src/lib/landing_target_estimator/landing_target_estimator_main.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file landing_target_estimator_main.cpp - * Landing target position estimator. Filter and publish the position of a landing target on the ground as observed by an onboard sensor. - * - * @author Nicolas de Palezieux (Sunflower Labs) - * @author Mohammed Kabir - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "LandingTargetEstimator.h" - - -namespace landing_target_estimator -{ - -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -/* Run main loop at this rate in Hz. */ -static constexpr uint32_t landing_target_estimator_UPDATE_RATE_HZ = 50; - -/** - * Landing target position estimator app start / stop handling function - * This makes the module accessible from the nuttx shell - * @ingroup apps - */ -extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[]); - -/** - * Mainloop of daemon. - */ -int landing_target_estimator_thread_main(int argc, char *argv[]); - -/** -* Main entry point for this module -**/ -int landing_target_estimator_main(int argc, char *argv[]) -{ - - if (argc < 2) { - goto exiterr; - } - - if (argc >= 2 && !strcmp(argv[1], "start")) { - if (thread_running) { - PX4_INFO("already running"); - /* this is not an error */ - return 0; - } - - thread_should_exit = false; - daemon_task = px4_task_spawn_cmd("landing_target_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2100, - landing_target_estimator_thread_main, - (argv) ? (char *const *)&argv[2] : nullptr); - return 0; - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - - if (!thread_running) { - PX4_WARN("landing_target_estimator not running"); - } - - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - PX4_INFO("running"); - - } else { - PX4_INFO("not started"); - } - - return 0; - } - -exiterr: - PX4_WARN("usage: landing_target_estimator {start|stop|status}"); - return 1; -} - -int landing_target_estimator_thread_main(int argc, char *argv[]) -{ - PX4_DEBUG("starting"); - - thread_running = true; - - LandingTargetEstimator est; - - while (!thread_should_exit) { - est.update(); - px4_usleep(1000000 / landing_target_estimator_UPDATE_RATE_HZ); - } - - PX4_DEBUG("exiting"); - - thread_running = false; - - return 0; -} - -} diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt index d5f9f0efa0..c72070a58e 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt @@ -32,8 +32,16 @@ ############################################################################ px4_add_library(FlightTaskAutoPrecisionLanding -FlightTaskAutoPrecisionLanding.cpp + FlightTaskAutoPrecisionLanding.cpp ) -target_link_libraries(FlightTaskAutoPrecisionLanding PUBLIC FlightTaskAuto) -target_include_directories(FlightTaskAutoPrecisionLanding PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(FlightTaskAutoPrecisionLanding + PUBLIC + FlightTaskAuto + LandingTargetEstimator +) + +target_include_directories(FlightTaskAutoPrecisionLanding + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp index 4683e11e19..d2cb030257 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -45,6 +45,16 @@ static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; +FlightTaskAutoPrecisionLanding::FlightTaskAutoPrecisionLanding() +{ + _landing_target_estimator.Start(); +} + +FlightTaskAutoPrecisionLanding::~FlightTaskAutoPrecisionLanding() +{ + _landing_target_estimator.Stop(); +} + bool FlightTaskAutoPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp index d2a320923d..f2a9755227 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp @@ -45,6 +45,7 @@ #include #include +#include #include #include #include @@ -74,8 +75,8 @@ enum class PrecLandMode { class FlightTaskAutoPrecisionLanding : public FlightTaskAuto { public: - FlightTaskAutoPrecisionLanding() = default; - virtual ~FlightTaskAutoPrecisionLanding() = default; + FlightTaskAutoPrecisionLanding(); + ~FlightTaskAutoPrecisionLanding(); bool activate(const trajectory_setpoint_s &last_setpoint) override; @@ -104,6 +105,8 @@ private: bool check_state_conditions(PrecLandState state); void slewrate(float &sp_x, float &sp_y); + landing_target_estimator::LandingTargetEstimator _landing_target_estimator; + landing_target_pose_s _target_pose{}; /**< precision landing target position */ uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)};