From 2eba1847fdfc43bbd0c266bf2b77d61ea9cc2e7f Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 11 Feb 2022 14:19:50 +0100 Subject: [PATCH] HTE: add new parameter HTE_THR_RANGE to define range of estimated thrust Signed-off-by: Silvan Fuhrer --- .../MulticopterHoverThrustEstimator.cpp | 7 ++++++- .../MulticopterHoverThrustEstimator.hpp | 3 ++- .../hover_thrust_estimator_params.c | 19 ++++++++++++++++++- .../zero_order_hover_thrust_ekf.cpp | 4 ++-- .../zero_order_hover_thrust_ekf.hpp | 6 +++++- 5 files changed, 33 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 8f4d8b808c..c1cdd9f806 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -86,6 +86,11 @@ void MulticopterHoverThrustEstimator::updateParams() } _hover_thrust_ekf.setAccelInnovGate(_param_hte_acc_gate.get()); + + _hover_thrust_ekf.setMinHoverThrust(math::constrain(_param_mpc_thr_hover.get() - _param_hte_thr_range.get(), 0.f, + 0.8f)); + _hover_thrust_ekf.setMaxHoverThrust(math::constrain(_param_mpc_thr_hover.get() + _param_hte_thr_range.get(), 0.2f, + 0.9f)); } void MulticopterHoverThrustEstimator::Run() diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index 2e88f6a8f5..596bb95483 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -121,6 +121,7 @@ private: (ParamFloat) _param_hte_ht_noise, (ParamFloat) _param_hte_acc_gate, (ParamFloat) _param_hte_ht_err_init, + (ParamFloat) _param_hte_thr_range, (ParamFloat) _param_hte_vxy_thr, (ParamFloat) _param_hte_vz_thr, (ParamFloat) _param_mpc_thr_hover diff --git a/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c b/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c index 3e1af502d0..fd46e23c95 100644 --- a/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c +++ b/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -113,3 +113,20 @@ PARAM_DEFINE_FLOAT(HTE_VXY_THR, 10.0); * @group Hover Thrust Estimator */ PARAM_DEFINE_FLOAT(HTE_VZ_THR, 2.0); + +/** + * Max deviation from MPC_THR_HOVER + * + * Defines the range of the hover thrust estimate around MPC_THR_HOVER. + * A value of 0.2 with MPC_THR_HOVER at 0.5 results in a range of [0.3, 0.7]. + * + * Set to a large value if the vehicle operates in varying physical conditions that + * affect the required hover thrust strongly (e.g. differently sized payloads). + * + * @decimal 2 + * @min 0.01 + * @max 0.4 + * @unit normalized_thrust + * @group Hover Thrust Estimator + */ +PARAM_DEFINE_FLOAT(HTE_THR_RANGE, 0.2); diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp index 64fec9d074..c7bf990c11 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -121,7 +121,7 @@ inline bool ZeroOrderHoverThrustEkf::isTestRatioPassing(const float innov_test_r inline void ZeroOrderHoverThrustEkf::updateState(const float K, const float innov) { - _hover_thr = math::constrain(_hover_thr + K * innov, 0.1f, 0.9f); + _hover_thr = math::constrain(_hover_thr + K * innov, _hover_thr_min, _hover_thr_max); } inline void ZeroOrderHoverThrustEkf::updateStateCovariance(const float K, const float H) diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp index f2610b8008..15f742d0d7 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -83,6 +83,8 @@ public: void setMeasurementNoiseScale(float scale) { _acc_var_scale = scale * scale; } void setHoverThrustStdDev(float hover_thrust_noise) { _state_var = hover_thrust_noise * hover_thrust_noise; } void setAccelInnovGate(float gate_size) { _gate_size = gate_size; } + void setMinHoverThrust(float hover_thrust_min) { _hover_thr_min = hover_thrust_min; } + void setMaxHoverThrust(float hover_thrust_max) { _hover_thr_max = hover_thrust_max; } float getHoverThrustEstimate() const { return _hover_thr; } float getHoverThrustEstimateVar() const { return _state_var; } @@ -93,6 +95,8 @@ public: private: float _hover_thr{0.5f}; + float _hover_thr_min{0.1f}; + float _hover_thr_max{0.9f}; float _gate_size{3.f}; float _state_var{0.01f}; ///< Initial hover thrust uncertainty variance (thrust^2)