diff --git a/src/modules/landing_target_estimator/CMakeLists.txt b/src/modules/landing_target_estimator/CMakeLists.txt index 146869aa3f..879f39e619 100644 --- a/src/modules/landing_target_estimator/CMakeLists.txt +++ b/src/modules/landing_target_estimator/CMakeLists.txt @@ -36,9 +36,7 @@ px4_add_module( MAIN landing_target_estimator COMPILE_FLAGS SRCS - landing_target_estimator_main.cpp LandingTargetEstimator.cpp KalmanFilter.cpp - DEPENDS ) diff --git a/src/modules/landing_target_estimator/KalmanFilter.cpp b/src/modules/landing_target_estimator/KalmanFilter.cpp index e4636b6e0f..c72ce5faf2 100644 --- a/src/modules/landing_target_estimator/KalmanFilter.cpp +++ b/src/modules/landing_target_estimator/KalmanFilter.cpp @@ -41,8 +41,6 @@ #include "KalmanFilter.h" -namespace landing_target_estimator -{ KalmanFilter::KalmanFilter(matrix::Vector &initial, matrix::Matrix &covInit) { init(initial, covInit); @@ -149,5 +147,3 @@ void KalmanFilter::getInnovations(float &innov, float &innovCov) innov = _residual; innovCov = _innovCov; } - -} // namespace landing_target_estimator diff --git a/src/modules/landing_target_estimator/KalmanFilter.h b/src/modules/landing_target_estimator/KalmanFilter.h index 3d466d6b06..09ccbe6a94 100644 --- a/src/modules/landing_target_estimator/KalmanFilter.h +++ b/src/modules/landing_target_estimator/KalmanFilter.h @@ -53,8 +53,6 @@ #pragma once -namespace landing_target_estimator -{ class KalmanFilter { public: @@ -147,4 +145,3 @@ private: float _innovCov{0.0f}; // innovation covariance of last measurement update }; -} // namespace landing_target_estimator diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index ef22a14bcc..8d67f15b68 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -31,14 +31,6 @@ * ****************************************************************************/ -/* - * @file LandingTargetEstimator.cpp - * - * @author Nicolas de Palezieux (Sunflower Labs) - * @author Mohammed Kabir - * - */ - #include #include #include @@ -47,26 +39,14 @@ #define SEC2USEC 1000000.0f -namespace landing_target_estimator +LandingTargetEstimator::LandingTargetEstimator() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { - -LandingTargetEstimator::LandingTargetEstimator() -{ - _paramHandle.acc_unc = param_find("LTEST_ACC_UNC"); - _paramHandle.meas_unc = param_find("LTEST_MEAS_UNC"); - _paramHandle.pos_unc_init = param_find("LTEST_POS_UNC_IN"); - _paramHandle.vel_unc_init = param_find("LTEST_VEL_UNC_IN"); - _paramHandle.mode = param_find("LTEST_MODE"); - _paramHandle.scale_x = param_find("LTEST_SCALE_X"); - _paramHandle.scale_y = param_find("LTEST_SCALE_Y"); - _paramHandle.sensor_yaw = param_find("LTEST_SENS_ROT"); - _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"); _check_params(true); } -void LandingTargetEstimator::update() +void LandingTargetEstimator::Run() { _check_params(false); @@ -74,8 +54,8 @@ void LandingTargetEstimator::update() /* predict */ if (_estimator_initialized) { - if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) { - PX4_WARN("Timeout"); + if (hrt_absolute_time() - _last_update > TARGET_UPDATE_TIMEOUT_US) { + PX4_INFO("Target lost"); _estimator_initialized = false; } else { @@ -93,8 +73,8 @@ void LandingTargetEstimator::update() a.zero(); } - _kalman_filter_x.predict(dt, -a(0), _params.acc_unc); - _kalman_filter_y.predict(dt, -a(1), _params.acc_unc); + _kalman_filter_x.predict(dt, -a(0), _param_acc_unc.get()); + _kalman_filter_y.predict(dt, -a(1), _param_acc_unc.get()); _last_predict = hrt_absolute_time(); } @@ -108,21 +88,23 @@ void LandingTargetEstimator::update() // mark this sensor measurement as consumed _new_sensorReport = false; + if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) { + return; + } if (!_estimator_initialized) { float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f; float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f; - PX4_INFO("Init %.2f %.2f", (double)vx_init, (double)vy_init); - _kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init); - _kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init); + PX4_INFO("Target acquired %.2f %.2f", (double)vx_init, (double)vy_init); + _kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _param_pos_unc_in.get(), _param_vel_unc_in.get()); + _kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _param_pos_unc_in.get(), _param_vel_unc_in.get()); _estimator_initialized = true; _last_update = hrt_absolute_time(); _last_predict = _last_update; } else { - // update - const float measurement_uncertainty = _params.meas_unc * _dist_z * _dist_z; + const float measurement_uncertainty = _param_meas_unc.get() * _dist_z * _dist_z; bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty); bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty); @@ -148,7 +130,7 @@ void LandingTargetEstimator::update() _kalman_filter_y.getState(y, yvel); _kalman_filter_y.getCovariance(covy, covy_v); - _target_pose.is_static = (_params.mode == TargetMode::Stationary); + _target_pose.is_static = ((TargetMode)_param_mode.get() == TargetMode::Stationary); _target_pose.rel_pos_valid = true; _target_pose.rel_vel_valid = true; @@ -200,7 +182,7 @@ void LandingTargetEstimator::_check_params(const bool force) parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); - _update_params(); + ModuleParams::updateParams(); } } @@ -224,12 +206,12 @@ void LandingTargetEstimator::_update_topics() } matrix::Vector sensor_ray; // ray pointing towards target in body frame - sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward - sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right + sensor_ray(0) = _irlockReport.pos_x * _param_scale_x.get(); // forward + sensor_ray(1) = _irlockReport.pos_y * _param_scale_y.get(); // right sensor_ray(2) = 1.0f; // rotate unit ray according to sensor orientation - _S_att = get_rot_matrix(_params.sensor_yaw); + _S_att = get_rot_matrix((Rotation)_param_sens_rot.get()); sensor_ray = _S_att * sensor_ray; // rotate the unit ray into the navigation frame @@ -242,7 +224,7 @@ void LandingTargetEstimator::_update_topics() return; } - _dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z; + _dist_z = _vehicleLocalPosition.dist_bottom - _param_sens_pos_z.get(); // scale the ray s.t. the z component has length of _uncertainty_scale _target_position_report.timestamp = _irlockReport.timestamp; @@ -251,8 +233,8 @@ void LandingTargetEstimator::_update_topics() _target_position_report.rel_pos_z = _dist_z; // Adjust relative position according to sensor offset - _target_position_report.rel_pos_x += _params.offset_x; - _target_position_report.rel_pos_y += _params.offset_y; + _target_position_report.rel_pos_x += _param_sens_pos_x.get(); + _target_position_report.rel_pos_y += _param_sens_pos_y.get(); _new_sensorReport = true; @@ -282,28 +264,61 @@ void LandingTargetEstimator::_update_topics() } } -void LandingTargetEstimator::_update_params() +int LandingTargetEstimator::start() { - param_get(_paramHandle.acc_unc, &_params.acc_unc); - param_get(_paramHandle.meas_unc, &_params.meas_unc); - param_get(_paramHandle.pos_unc_init, &_params.pos_unc_init); - param_get(_paramHandle.vel_unc_init, &_params.vel_unc_init); - - int32_t mode = 0; - param_get(_paramHandle.mode, &mode); - _params.mode = (TargetMode)mode; - - param_get(_paramHandle.scale_x, &_params.scale_x); - param_get(_paramHandle.scale_y, &_params.scale_y); - - int32_t sensor_yaw = 0; - param_get(_paramHandle.sensor_yaw, &sensor_yaw); - _params.sensor_yaw = static_cast(sensor_yaw); - - param_get(_paramHandle.offset_x, &_params.offset_x); - param_get(_paramHandle.offset_y, &_params.offset_y); - param_get(_paramHandle.offset_z, &_params.offset_z); + ScheduleOnInterval(1000000 / SAMPLE_RATE); + return PX4_OK; } +int LandingTargetEstimator::task_spawn(int argc, char *argv[]) +{ + LandingTargetEstimator *instance = new LandingTargetEstimator(); -} // namespace landing_target_estimator + if (!instance) { + PX4_ERR("driver allocation failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->start(); + return 0; +} + +int LandingTargetEstimator::print_usage(const char *reason) +{ + if (reason) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Background process running periodically on the LP work queue which filters and estimates a targets position. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("landing_target_estimator", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND("stop"); + PRINT_MODULE_USAGE_COMMAND("status"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int LandingTargetEstimator::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + PX4_INFO("not running"); + return PX4_ERROR; + } + + return print_usage("Unrecognized command."); +} + +extern "C" __EXPORT int landing_target_estimator_main(int argc, char *argv[]) +{ + return LandingTargetEstimator::main(argc, argv); +} diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 521a1e3763..fdc6dd4a86 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -31,22 +31,14 @@ * ****************************************************************************/ -/* - * @file LandingTargetEstimator.h - * 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 - * - */ #pragma once -#include +#include +#include +#include #include -#include #include -#include #include #include #include @@ -55,8 +47,6 @@ #include #include #include -#include -#include #include #include #include @@ -66,35 +56,36 @@ using namespace time_literals; -namespace landing_target_estimator -{ - -class LandingTargetEstimator +class LandingTargetEstimator : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem { public: LandingTargetEstimator(); virtual ~LandingTargetEstimator() = default; - /* - * Get new measurements and update the state estimate - */ - void update(); + static int print_usage(const char *reason = nullptr); + static int custom_command(int argc, char *argv[]); -protected: + static int task_spawn(int argc, char *argv[]); - /* - * Update uORB topics. - */ + int start(); + +private: + + void Run() override; + + void _check_params(const bool force); void _update_topics(); - - /* - * Update parameters. - */ void _update_params(); - /* timeout after which filter is reset if target not seen */ - static constexpr uint32_t landing_target_estimator_TIMEOUT_US = 2000000; + enum class TargetMode { + Moving = 0, + Stationary + }; + + static constexpr uint32_t TARGET_UPDATE_TIMEOUT_US{2000000}; + static constexpr uint32_t SAMPLE_RATE{50}; // samples per second uORB::Publication _targetPosePub{ORB_ID(landing_target_pose)}; landing_target_pose_s _target_pose{}; @@ -104,44 +95,6 @@ protected: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; -private: - - enum class TargetMode { - Moving = 0, - Stationary - }; - - /** - * Handles for parameters - **/ - struct { - param_t acc_unc; - param_t meas_unc; - param_t pos_unc_init; - param_t vel_unc_init; - param_t mode; - param_t scale_x; - param_t scale_y; - param_t offset_x; - param_t offset_y; - param_t offset_z; - param_t sensor_yaw; - } _paramHandle; - - struct { - float acc_unc; - float meas_unc; - float pos_unc_init; - float vel_unc_init; - TargetMode mode; - float scale_x; - float scale_y; - float offset_x; - float offset_y; - float offset_z; - enum Rotation sensor_yaw; - } _params; - struct { hrt_abstime timestamp; float rel_pos_x; @@ -159,7 +112,6 @@ private: vehicle_attitude_s _vehicleAttitude{}; vehicle_acceleration_s _vehicle_acceleration{}; irlock_report_s _irlockReport{}; - uwb_grid_s _uwbGrid{}; uwb_distance_s _uwbDistance{}; // keep track of which topics we have received @@ -181,8 +133,17 @@ private: hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout) float _dist_z{1.0f}; - void _check_params(const bool force); - - void _update_state(); + DEFINE_PARAMETERS( + (ParamInt) _param_mode, + (ParamFloat) _param_acc_unc, + (ParamFloat) _param_meas_unc, + (ParamFloat) _param_pos_unc_in, + (ParamFloat) _param_vel_unc_in, + (ParamFloat) _param_scale_x, + (ParamFloat) _param_scale_y, + (ParamInt) _param_sens_rot, + (ParamFloat) _param_sens_pos_x, + (ParamFloat) _param_sens_pos_y, + (ParamFloat) _param_sens_pos_z + ) }; -} // namespace landing_target_estimator diff --git a/src/modules/landing_target_estimator/landing_target_estimator_main.cpp b/src/modules/landing_target_estimator/landing_target_estimator_main.cpp deleted file mode 100644 index bb666600d1..0000000000 --- a/src/modules/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; -} - -}