From 2eda7d1379593477f46756779546d0a80fc48909 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 25 Mar 2022 08:19:02 +1300 Subject: [PATCH] Revert "Make LandingTargetEstimator a lib and run on work_queue" This reverts commit 37c2d242b159ccf55d6de999b268204fde2eed50. --- 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, 179 insertions(+), 107 deletions(-) create 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 0783f6e2ed..f85f30f424 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -23,6 +23,7 @@ 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 3c7aa69d9d..f6bcc9260b 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -50,7 +50,6 @@ 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 f30c4073f4..146869aa3f 100644 --- a/src/lib/landing_target_estimator/CMakeLists.txt +++ b/src/lib/landing_target_estimator/CMakeLists.txt @@ -31,14 +31,14 @@ # ############################################################################ -px4_add_library(LandingTargetEstimator - LandingTargetEstimator.cpp - KalmanFilter.cpp -) +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 + ) -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 46dbbe562a..3783c3d852 100644 --- a/src/lib/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/lib/landing_target_estimator/LandingTargetEstimator.cpp @@ -50,9 +50,7 @@ namespace landing_target_estimator { -LandingTargetEstimator::LandingTargetEstimator() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +LandingTargetEstimator::LandingTargetEstimator() { _paramHandle.acc_unc = param_find("LTEST_ACC_UNC"); _paramHandle.meas_unc = param_find("LTEST_MEAS_UNC"); @@ -65,54 +63,7 @@ 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 23ac9913f5..521a1e3763 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,16 +58,10 @@ #include #include #include - #include #include #include #include - -#include -#include -#include - #include "KalmanFilter.h" using namespace time_literals; @@ -75,24 +69,19 @@ using namespace time_literals; namespace landing_target_estimator { -class LandingTargetEstimator : public ModuleParams, public px4::ScheduledWorkItem +class LandingTargetEstimator { public: LandingTargetEstimator(); - ~LandingTargetEstimator() override; + virtual ~LandingTargetEstimator() = default; /* * Get new measurements and update the state estimate */ - - bool Start(); - void Stop(); + void update(); protected: - void Run() override; - - void update(); /* * Update uORB topics. @@ -160,11 +149,11 @@ private: float rel_pos_z; } _target_position_report; - 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)}; + 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)}; 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 new file mode 100644 index 0000000000..5cbe7eee18 --- /dev/null +++ b/src/lib/landing_target_estimator/landing_target_estimator_main.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * 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, + 2000, + 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 c72070a58e..d5f9f0efa0 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/CMakeLists.txt @@ -32,16 +32,8 @@ ############################################################################ px4_add_library(FlightTaskAutoPrecisionLanding - FlightTaskAutoPrecisionLanding.cpp +FlightTaskAutoPrecisionLanding.cpp ) -target_link_libraries(FlightTaskAutoPrecisionLanding - PUBLIC - FlightTaskAuto - LandingTargetEstimator -) - -target_include_directories(FlightTaskAutoPrecisionLanding - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} -) +target_link_libraries(FlightTaskAutoPrecisionLanding PUBLIC FlightTaskAuto) +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 d2cb030257..4683e11e19 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp @@ -45,16 +45,6 @@ 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 f2a9755227..d2a320923d 100644 --- a/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.hpp @@ -45,7 +45,6 @@ #include #include -#include #include #include #include @@ -75,8 +74,8 @@ enum class PrecLandMode { class FlightTaskAutoPrecisionLanding : public FlightTaskAuto { public: - FlightTaskAutoPrecisionLanding(); - ~FlightTaskAutoPrecisionLanding(); + FlightTaskAutoPrecisionLanding() = default; + virtual ~FlightTaskAutoPrecisionLanding() = default; bool activate(const trajectory_setpoint_s &last_setpoint) override; @@ -105,8 +104,6 @@ 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)};