diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index dd504480d6..23242a5526 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -22,7 +22,6 @@ CONFIG_MODULES_FW_POS_CONTROL_L1=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 0552b21db7..eaf41ef750 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -49,6 +49,7 @@ add_subdirectory(geo) add_subdirectory(hysteresis) add_subdirectory(l1) add_subdirectory(landing_slope) +add_subdirectory(landing_target_estimator) add_subdirectory(led) add_subdirectory(matrix) add_subdirectory(mathlib) 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 ef22a14bcc..e07c7f7ba8 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 9e1223f970..83f8079517 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 vehicle_local_position_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 c679f2e158..7ff8814878 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 vehicle_local_position_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)};