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