fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 20:17:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactored landing_target_estimator to use the LP work queue

Browse Source
This commit is contained in:
Jacob Dahl
2022-05-03 12:02:04 -08:00
parent 138772386f
commit 17540cd525
6 changed files with 112 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/landing_target_estimator/CMakeLists.txt
-2
View File
@@ -36,9 +36,7 @@ px4_add_module(
MAIN landing_target_estimator
COMPILE_FLAGS
SRCS
landing_target_estimator_main.cpp
LandingTargetEstimator.cpp
KalmanFilter.cpp
DEPENDS
)
src/modules/landing_target_estimator/KalmanFilter.cpp
-4
View File
@@ -41,8 +41,6 @@
#include "KalmanFilter.h"
namespace landing_target_estimator
{
KalmanFilter::KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
{
init(initial, covInit);
@@ -149,5 +147,3 @@ void KalmanFilter::getInnovations(float &innov, float &innovCov)
innov = _residual;
innovCov = _innovCov;
}
} // namespace landing_target_estimator
src/modules/landing_target_estimator/KalmanFilter.h
-3
View File
@@ -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
src/modules/landing_target_estimator/LandingTargetEstimator.cpp
+77 -62
View File
@@ -31,14 +31,6 @@
*
****************************************************************************/
/*
* @file LandingTargetEstimator.cpp
*
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
* @author Mohammed Kabir <kabir@uasys.io>
*
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
@@ -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<float, 3> 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<enum Rotation>(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);
}
src/modules/landing_target_estimator/LandingTargetEstimator.h
+35 -74
View File
@@ -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) <ndepal@gmail.com>
* @author Mohammed Kabir <kabir@uasys.io>
*
*/
#pragma once
#include <px4_platform_common/workqueue.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/drv_hrt.h>
#include <parameters/param.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -55,8 +47,6 @@
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/landing_target_innovations.h>
#include <uORB/topics/uwb_distance.h>
#include <uORB/topics/uwb_grid.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/parameter_update.h>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
@@ -66,35 +56,36 @@
using namespace time_literals;
namespace landing_target_estimator
{
class LandingTargetEstimator
class LandingTargetEstimator : public ModuleBase<LandingTargetEstimator>, 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<landing_target_pose_s> _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<px4::params::LTEST_MODE>) _param_mode,
(ParamFloat<px4::params::LTEST_ACC_UNC>) _param_acc_unc,
(ParamFloat<px4::params::LTEST_MEAS_UNC>) _param_meas_unc,
(ParamFloat<px4::params::LTEST_POS_UNC_IN>) _param_pos_unc_in,
(ParamFloat<px4::params::LTEST_VEL_UNC_IN>) _param_vel_unc_in,
(ParamFloat<px4::params::LTEST_SCALE_X>) _param_scale_x,
(ParamFloat<px4::params::LTEST_SCALE_Y>) _param_scale_y,
(ParamInt<px4::params::LTEST_SENS_ROT>) _param_sens_rot,
(ParamFloat<px4::params::LTEST_SENS_POS_X>) _param_sens_pos_x,
(ParamFloat<px4::params::LTEST_SENS_POS_Y>) _param_sens_pos_y,
(ParamFloat<px4::params::LTEST_SENS_POS_Z>) _param_sens_pos_z
)
};
} // namespace landing_target_estimator
src/modules/landing_target_estimator/landing_target_estimator_main.cpp
-153
View File
@@ -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) <ndepal@gmail.com>
* @author Mohammed Kabir <kabir@uasys.io>
*
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#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;
}
}