mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-09 07:11:36 +08:00
Revert "git move landing_target_estimator to lib dir"
This reverts commit 17ec100fa9ba6a804b36dc0279df080817fbff81.
This commit is contained in:
Julian Oes
committed by
Alessandro Simovic
Alessandro Simovic
parent
2eda7d1379
commit
5878b0c721
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
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
|
||||
)
|
||||
|
||||
@@ -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 KalmanFilter.h
|
||||
* Simple Kalman Filter for variable gain low-passing
|
||||
*
|
||||
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "KalmanFilter.h"
|
||||
|
||||
namespace landing_target_estimator
|
||||
{
|
||||
KalmanFilter::KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
|
||||
{
|
||||
init(initial, covInit);
|
||||
}
|
||||
|
||||
void KalmanFilter::init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit)
|
||||
{
|
||||
_x = initial;
|
||||
_covariance = covInit;
|
||||
}
|
||||
|
||||
void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11)
|
||||
{
|
||||
matrix::Vector<float, 2> initial;
|
||||
initial(0) = initial0;
|
||||
initial(1) = initial1;
|
||||
matrix::Matrix<float, 2, 2> covInit;
|
||||
covInit(0, 0) = covInit00;
|
||||
covInit(1, 1) = covInit11;
|
||||
|
||||
init(initial, covInit);
|
||||
}
|
||||
|
||||
void KalmanFilter::predict(float dt, float acc, float acc_unc)
|
||||
{
|
||||
_x(0) += _x(1) * dt + dt * dt / 2 * acc;
|
||||
_x(1) += acc * dt;
|
||||
|
||||
matrix::Matrix<float, 2, 2> A; // propagation matrix
|
||||
A(0, 0) = 1;
|
||||
A(1, 1) = 1;
|
||||
A(0, 1) = dt;
|
||||
|
||||
matrix::Matrix<float, 2, 1> G; // noise model
|
||||
G(0, 0) = dt * dt / 2;
|
||||
G(1, 0) = dt;
|
||||
|
||||
matrix::Matrix<float, 2, 2> process_noise = G * G.transpose() * acc_unc;
|
||||
|
||||
_covariance = A * _covariance * A.transpose() + process_noise;
|
||||
}
|
||||
|
||||
bool KalmanFilter::update(float meas, float measUnc)
|
||||
{
|
||||
|
||||
// H = [1, 0]
|
||||
_residual = meas - _x(0);
|
||||
|
||||
// H * P * H^T simply selects P(0,0)
|
||||
_innovCov = _covariance(0, 0) + measUnc;
|
||||
|
||||
// outlier rejection
|
||||
float beta = _residual / _innovCov * _residual;
|
||||
|
||||
// 5% false alarm probability
|
||||
if (beta > 3.84f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
matrix::Vector<float, 2> kalmanGain;
|
||||
kalmanGain(0) = _covariance(0, 0);
|
||||
kalmanGain(1) = _covariance(1, 0);
|
||||
kalmanGain /= _innovCov;
|
||||
|
||||
_x += kalmanGain * _residual;
|
||||
|
||||
matrix::Matrix<float, 2, 2> identity;
|
||||
identity.identity();
|
||||
|
||||
matrix::Matrix<float, 2, 2> KH; // kalmanGain * H
|
||||
KH(0, 0) = kalmanGain(0);
|
||||
KH(1, 0) = kalmanGain(1);
|
||||
|
||||
_covariance = (identity - KH) * _covariance;
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
void KalmanFilter::getState(matrix::Vector<float, 2> &state)
|
||||
{
|
||||
state = _x;
|
||||
}
|
||||
|
||||
void KalmanFilter::getState(float &state0, float &state1)
|
||||
{
|
||||
state0 = _x(0);
|
||||
state1 = _x(1);
|
||||
}
|
||||
|
||||
void KalmanFilter::getCovariance(matrix::Matrix<float, 2, 2> &covariance)
|
||||
{
|
||||
covariance = _covariance;
|
||||
}
|
||||
|
||||
void KalmanFilter::getCovariance(float &cov00, float &cov11)
|
||||
{
|
||||
cov00 = _covariance(0, 0);
|
||||
cov11 = _covariance(1, 1);
|
||||
}
|
||||
|
||||
void KalmanFilter::getInnovations(float &innov, float &innovCov)
|
||||
{
|
||||
innov = _residual;
|
||||
innovCov = _innovCov;
|
||||
}
|
||||
|
||||
} // namespace landing_target_estimator
|
||||
@@ -0,0 +1,150 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 KalmanFilter.h
|
||||
* Simple Kalman Filter for variable gain low-passing
|
||||
*
|
||||
* Constant velocity model. Prediction according to
|
||||
* x_{k+1} = A * x_{k}
|
||||
* with A = [1 dt; 0 1]
|
||||
*
|
||||
* Update with a direct measurement of the first state:
|
||||
* H = [1 0]
|
||||
*
|
||||
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/Matrix.hpp>
|
||||
#include <matrix/Vector.hpp>
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace landing_target_estimator
|
||||
{
|
||||
class KalmanFilter
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Default constructor, state not initialized
|
||||
*/
|
||||
KalmanFilter() {};
|
||||
|
||||
/**
|
||||
* Constructor, initialize state
|
||||
*/
|
||||
KalmanFilter(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
|
||||
|
||||
/**
|
||||
* Default desctructor
|
||||
*/
|
||||
virtual ~KalmanFilter() {};
|
||||
|
||||
/**
|
||||
* Initialize filter state
|
||||
* @param initial initial state
|
||||
* @param covInit initial covariance
|
||||
*/
|
||||
void init(matrix::Vector<float, 2> &initial, matrix::Matrix<float, 2, 2> &covInit);
|
||||
|
||||
/**
|
||||
* Initialize filter state, only specifying diagonal covariance elements
|
||||
* @param initial0 first initial state
|
||||
* @param initial1 second initial state
|
||||
* @param covInit00 initial variance of first state
|
||||
* @param covinit11 initial variance of second state
|
||||
*/
|
||||
void init(float initial0, float initial1, float covInit00, float covInit11);
|
||||
|
||||
/**
|
||||
* Predict the state with an external acceleration estimate
|
||||
* @param dt Time delta in seconds since last state change
|
||||
* @param acc Acceleration estimate
|
||||
* @param acc_unc Variance of acceleration estimate
|
||||
*/
|
||||
void predict(float dt, float acc, float acc_unc);
|
||||
|
||||
/**
|
||||
* Update the state estimate with a measurement
|
||||
* @param meas state measeasurement
|
||||
* @param measUnc measurement uncertainty
|
||||
* @return update success (measurement not rejected)
|
||||
*/
|
||||
bool update(float meas, float measUnc);
|
||||
|
||||
/**
|
||||
* Get the current filter state
|
||||
* @param x1 State
|
||||
*/
|
||||
void getState(matrix::Vector<float, 2> &state);
|
||||
|
||||
/**
|
||||
* Get the current filter state
|
||||
* @param state0 First state
|
||||
* @param state1 Second state
|
||||
*/
|
||||
void getState(float &state0, float &state1);
|
||||
|
||||
/**
|
||||
* Get state covariance
|
||||
* @param covariance Covariance of the state
|
||||
*/
|
||||
void getCovariance(matrix::Matrix<float, 2, 2> &covariance);
|
||||
|
||||
/**
|
||||
* Get state variances (diagonal elements)
|
||||
* @param cov00 Variance of first state
|
||||
* @param cov11 Variance of second state
|
||||
*/
|
||||
void getCovariance(float &cov00, float &cov11);
|
||||
|
||||
/**
|
||||
* Get measurement innovation and covariance of last update call
|
||||
* @param innov Measurement innovation
|
||||
* @param innovCov Measurement innovation covariance
|
||||
*/
|
||||
void getInnovations(float &innov, float &innovCov);
|
||||
|
||||
private:
|
||||
matrix::Vector<float, 2> _x; // state
|
||||
|
||||
matrix::Matrix<float, 2, 2> _covariance; // state covariance
|
||||
|
||||
float _residual{0.0f}; // residual of last measurement update
|
||||
|
||||
float _innovCov{0.0f}; // innovation covariance of last measurement update
|
||||
};
|
||||
} // namespace landing_target_estimator
|
||||
@@ -0,0 +1,12 @@
|
||||
menuconfig MODULES_LANDING_TARGET_ESTIMATOR
|
||||
bool "landing_target_estimator"
|
||||
default n
|
||||
---help---
|
||||
Enable support for landing_target_estimator
|
||||
|
||||
menuconfig USER_LANDING_TARGET_ESTIMATOR
|
||||
bool "landing_target_estimator running as userspace module"
|
||||
default y
|
||||
depends on BOARD_PROTECTED && MODULES_LANDING_TARGET_ESTIMATOR
|
||||
---help---
|
||||
Put landing_target_estimator in userspace memory
|
||||
@@ -0,0 +1,308 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 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>
|
||||
|
||||
#include "LandingTargetEstimator.h"
|
||||
|
||||
#define SEC2USEC 1000000.0f
|
||||
|
||||
namespace landing_target_estimator
|
||||
{
|
||||
|
||||
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()
|
||||
{
|
||||
_check_params(false);
|
||||
|
||||
_update_topics();
|
||||
|
||||
/* predict */
|
||||
if (_estimator_initialized) {
|
||||
if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) {
|
||||
PX4_INFO("Lost sight of Marker");
|
||||
_estimator_initialized = false;
|
||||
|
||||
} else {
|
||||
float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC;
|
||||
|
||||
// predict target position with the help of accel data
|
||||
matrix::Vector3f a{_vehicle_acceleration.xyz};
|
||||
|
||||
if (_vehicleAttitude_valid && _vehicle_acceleration_valid) {
|
||||
matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
|
||||
_R_att = matrix::Dcm<float>(q_att);
|
||||
a = _R_att * a;
|
||||
|
||||
} else {
|
||||
a.zero();
|
||||
}
|
||||
|
||||
_kalman_filter_x.predict(dt, -a(0), _params.acc_unc);
|
||||
_kalman_filter_y.predict(dt, -a(1), _params.acc_unc);
|
||||
|
||||
_last_predict = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
if (!_new_sensorReport) {
|
||||
// nothing to do
|
||||
return;
|
||||
}
|
||||
|
||||
// mark this sensor measurement as consumed
|
||||
_new_sensorReport = false;
|
||||
|
||||
|
||||
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);
|
||||
|
||||
_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;
|
||||
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);
|
||||
|
||||
if (!update_x || !update_y) {
|
||||
if (!_faulty) {
|
||||
_faulty = true;
|
||||
PX4_INFO("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
|
||||
}
|
||||
|
||||
} else {
|
||||
_faulty = false;
|
||||
}
|
||||
|
||||
if (!_faulty) {
|
||||
// only publish if both measurements were good
|
||||
|
||||
_target_pose.timestamp = _target_position_report.timestamp;
|
||||
|
||||
float x, xvel, y, yvel, covx, covx_v, covy, covy_v;
|
||||
_kalman_filter_x.getState(x, xvel);
|
||||
_kalman_filter_x.getCovariance(covx, covx_v);
|
||||
|
||||
_kalman_filter_y.getState(y, yvel);
|
||||
_kalman_filter_y.getCovariance(covy, covy_v);
|
||||
|
||||
_target_pose.is_static = (_params.mode == TargetMode::Stationary);
|
||||
|
||||
_target_pose.rel_pos_valid = true;
|
||||
_target_pose.rel_vel_valid = true;
|
||||
_target_pose.x_rel = x;
|
||||
_target_pose.y_rel = y;
|
||||
_target_pose.z_rel = _target_position_report.rel_pos_z ;
|
||||
_target_pose.vx_rel = xvel;
|
||||
_target_pose.vy_rel = yvel;
|
||||
|
||||
_target_pose.cov_x_rel = covx;
|
||||
_target_pose.cov_y_rel = covy;
|
||||
|
||||
_target_pose.cov_vx_rel = covx_v;
|
||||
_target_pose.cov_vy_rel = covy_v;
|
||||
|
||||
if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) {
|
||||
_target_pose.x_abs = x + _vehicleLocalPosition.x;
|
||||
_target_pose.y_abs = y + _vehicleLocalPosition.y;
|
||||
_target_pose.z_abs = _target_position_report.rel_pos_z + _vehicleLocalPosition.z;
|
||||
_target_pose.abs_pos_valid = true;
|
||||
|
||||
} else {
|
||||
_target_pose.abs_pos_valid = false;
|
||||
}
|
||||
|
||||
_targetPosePub.publish(_target_pose);
|
||||
|
||||
_last_update = hrt_absolute_time();
|
||||
_last_predict = _last_update;
|
||||
}
|
||||
|
||||
float innov_x, innov_cov_x, innov_y, innov_cov_y;
|
||||
_kalman_filter_x.getInnovations(innov_x, innov_cov_x);
|
||||
_kalman_filter_y.getInnovations(innov_y, innov_cov_y);
|
||||
|
||||
_target_innovations.timestamp = _target_position_report.timestamp;
|
||||
_target_innovations.innov_x = innov_x;
|
||||
_target_innovations.innov_cov_x = innov_cov_x;
|
||||
_target_innovations.innov_y = innov_y;
|
||||
_target_innovations.innov_cov_y = innov_cov_y;
|
||||
|
||||
_targetInnovationsPub.publish(_target_innovations);
|
||||
}
|
||||
}
|
||||
|
||||
void LandingTargetEstimator::_check_params(const bool force)
|
||||
{
|
||||
if (_parameter_update_sub.updated() || force) {
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
_update_params();
|
||||
}
|
||||
}
|
||||
|
||||
void LandingTargetEstimator::_update_topics()
|
||||
{
|
||||
_vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition);
|
||||
_vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude);
|
||||
_vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration);
|
||||
|
||||
|
||||
if (_irlockReportSub.update(&_irlockReport)) { //
|
||||
_new_irlockReport = true;
|
||||
|
||||
if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) {
|
||||
// don't have the data needed for an update
|
||||
return;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
|
||||
return;
|
||||
}
|
||||
|
||||
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(2) = 1.0f;
|
||||
|
||||
// rotate unit ray according to sensor orientation
|
||||
_S_att = get_rot_matrix(_params.sensor_yaw);
|
||||
sensor_ray = _S_att * sensor_ray;
|
||||
|
||||
// rotate the unit ray into the navigation frame
|
||||
matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
|
||||
_R_att = matrix::Dcm<float>(q_att);
|
||||
sensor_ray = _R_att * sensor_ray;
|
||||
|
||||
if (fabsf(sensor_ray(2)) < 1e-6f) {
|
||||
// z component of measurement unsafe, don't use this measurement
|
||||
return;
|
||||
}
|
||||
|
||||
_dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z;
|
||||
|
||||
// scale the ray s.t. the z component has length of _uncertainty_scale
|
||||
_target_position_report.timestamp = _irlockReport.timestamp;
|
||||
_target_position_report.rel_pos_x = sensor_ray(0) / sensor_ray(2) * _dist_z;
|
||||
_target_position_report.rel_pos_y = sensor_ray(1) / sensor_ray(2) * _dist_z;
|
||||
_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;
|
||||
|
||||
_new_sensorReport = true;
|
||||
|
||||
} else if (_uwbDistanceSub.update(&_uwbDistance)) {
|
||||
if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) {
|
||||
// don't have the data needed for an update
|
||||
PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) {
|
||||
PX4_WARN("Marker position reading invalid!");
|
||||
return;
|
||||
}
|
||||
|
||||
_new_sensorReport = true;
|
||||
|
||||
// The coordinate system is NED (north-east-down)
|
||||
// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
|
||||
// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
|
||||
// To change POV we negate every Axis:
|
||||
_target_position_report.timestamp = _uwbDistance.timestamp;
|
||||
_target_position_report.rel_pos_x = -_uwbDistance.position[0];
|
||||
_target_position_report.rel_pos_y = -_uwbDistance.position[1];
|
||||
_target_position_report.rel_pos_z = -_uwbDistance.position[2];
|
||||
}
|
||||
}
|
||||
|
||||
void LandingTargetEstimator::_update_params()
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
} // namespace landing_target_estimator
|
||||
@@ -0,0 +1,188 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 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 <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>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/irlock_report.h>
|
||||
#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>
|
||||
#include <matrix/Matrix.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include "KalmanFilter.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
namespace landing_target_estimator
|
||||
{
|
||||
|
||||
class LandingTargetEstimator
|
||||
{
|
||||
public:
|
||||
|
||||
LandingTargetEstimator();
|
||||
virtual ~LandingTargetEstimator() = default;
|
||||
|
||||
/*
|
||||
* Get new measurements and update the state estimate
|
||||
*/
|
||||
void update();
|
||||
|
||||
protected:
|
||||
|
||||
/*
|
||||
* Update uORB topics.
|
||||
*/
|
||||
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;
|
||||
|
||||
uORB::Publication<landing_target_pose_s> _targetPosePub{ORB_ID(landing_target_pose)};
|
||||
landing_target_pose_s _target_pose{};
|
||||
|
||||
uORB::Publication<landing_target_innovations_s> _targetInnovationsPub{ORB_ID(landing_target_innovations)};
|
||||
landing_target_innovations_s _target_innovations{};
|
||||
|
||||
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;
|
||||
float rel_pos_y;
|
||||
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)};
|
||||
|
||||
vehicle_local_position_s _vehicleLocalPosition{};
|
||||
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
|
||||
bool _vehicleLocalPosition_valid{false};
|
||||
bool _vehicleAttitude_valid{false};
|
||||
bool _vehicle_acceleration_valid{false};
|
||||
bool _new_irlockReport{false};
|
||||
bool _new_sensorReport{false};
|
||||
bool _estimator_initialized{false};
|
||||
// keep track of whether last measurement was rejected
|
||||
bool _faulty{false};
|
||||
|
||||
matrix::Dcmf _R_att; //Orientation of the body frame
|
||||
matrix::Dcmf _S_att; //Orientation of the sensor relative to body frame
|
||||
matrix::Vector2f _rel_pos;
|
||||
KalmanFilter _kalman_filter_x;
|
||||
KalmanFilter _kalman_filter_y;
|
||||
hrt_abstime _last_predict{0}; // timestamp of last filter prediction
|
||||
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();
|
||||
};
|
||||
} // namespace landing_target_estimator
|
||||
@@ -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) <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,
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,189 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014-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_params.c
|
||||
* Landing target estimator algorithm parameters.
|
||||
*
|
||||
* @author Nicolas de Palezieux (Sunflower Labs) <ndepal@gmail.com>
|
||||
* @author Mohammed Kabir <kabir@uasys.io>
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* Landing target mode
|
||||
*
|
||||
* Configure the mode of the landing target. Depending on the mode, the landing target observations are used differently to aid position estimation.
|
||||
*
|
||||
* Mode Moving: The landing target may be moving around while in the field of view of the vehicle. Landing target measurements are not used to aid positioning.
|
||||
* Mode Stationary: The landing target is stationary. Measured velocity w.r.t. the landing target is used to aid velocity estimation.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Landing target Estimator
|
||||
* @value 0 Moving
|
||||
* @value 1 Stationary
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LTEST_MODE, 0);
|
||||
|
||||
/**
|
||||
* Acceleration uncertainty
|
||||
*
|
||||
* Variance of acceleration measurement used for landing target position prediction.
|
||||
* Higher values results in tighter following of the measurements and more lenient outlier rejection
|
||||
*
|
||||
* @unit (m/s^2)^2
|
||||
* @min 0.01
|
||||
* @decimal 2
|
||||
*
|
||||
* @group Landing target Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_ACC_UNC, 10.0f);
|
||||
|
||||
/**
|
||||
* Landing target measurement uncertainty
|
||||
*
|
||||
* Variance of the landing target measurement from the driver.
|
||||
* Higher values result in less aggressive following of the measurement and a smoother output as well as fewer rejected measurements.
|
||||
*
|
||||
* @unit tan(rad)^2
|
||||
* @decimal 4
|
||||
*
|
||||
* @group Landing target Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_MEAS_UNC, 0.005f);
|
||||
|
||||
/**
|
||||
* Initial landing target position uncertainty
|
||||
*
|
||||
* Initial variance of the relative landing target position in x and y direction
|
||||
*
|
||||
* @unit m^2
|
||||
* @min 0.001
|
||||
* @decimal 3
|
||||
*
|
||||
* @group Landing target Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f);
|
||||
|
||||
/**
|
||||
* Initial landing target velocity uncertainty
|
||||
*
|
||||
* Initial variance of the relative landing target velocity in x and y directions
|
||||
*
|
||||
* @unit (m/s)^2
|
||||
* @min 0.001
|
||||
* @decimal 3
|
||||
*
|
||||
* @group Landing target Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 0.1f);
|
||||
|
||||
/**
|
||||
* Scale factor for sensor measurements in sensor x axis
|
||||
*
|
||||
* Landing target x measurements are scaled by this factor before being used
|
||||
*
|
||||
* @min 0.01
|
||||
* @decimal 3
|
||||
*
|
||||
* @group Landing target Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f);
|
||||
|
||||
/**
|
||||
* Scale factor for sensor measurements in sensor y axis
|
||||
*
|
||||
* Landing target y measurements are scaled by this factor before being used
|
||||
*
|
||||
* @min 0.01
|
||||
* @decimal 3
|
||||
*
|
||||
* @group Landing target Estimator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f);
|
||||
|
||||
|
||||
/**
|
||||
* Rotation of IRLOCK sensor relative to airframe
|
||||
*
|
||||
* Default orientation of Yaw 90°
|
||||
*
|
||||
* @value 0 No rotation
|
||||
* @value 1 Yaw 45°
|
||||
* @value 2 Yaw 90°
|
||||
* @value 3 Yaw 135°
|
||||
* @value 4 Yaw 180°
|
||||
* @value 5 Yaw 225°
|
||||
* @value 6 Yaw 270°
|
||||
* @value 7 Yaw 315°
|
||||
*
|
||||
* @min -1
|
||||
* @max 40
|
||||
* @reboot_required true
|
||||
* @group Landing Target Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LTEST_SENS_ROT, 2);
|
||||
|
||||
/**
|
||||
* X Position of IRLOCK in body frame (forward)
|
||||
*
|
||||
* @reboot_required true
|
||||
* @unit m
|
||||
* @decimal 3
|
||||
* @group Landing Target Estimator
|
||||
*
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_SENS_POS_X, 0.0f);
|
||||
|
||||
/**
|
||||
* Y Position of IRLOCK in body frame (right)
|
||||
*
|
||||
* @reboot_required true
|
||||
* @unit m
|
||||
* @decimal 3
|
||||
* @group Landing Target Estimator
|
||||
*
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_SENS_POS_Y, 0.0f);
|
||||
|
||||
/**
|
||||
* Z Position of IRLOCK in body frame (downward)
|
||||
*
|
||||
* @reboot_required true
|
||||
* @unit m
|
||||
* @decimal 3
|
||||
* @group Landing Target Estimator
|
||||
*
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LTEST_SENS_POS_Z, 0.0f);
|
||||
Reference in New Issue
Block a user