dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

HTE: add new parameter HTE_THR_RANGE to define range of estimated thrust 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-02-11 14:19:50 +01:00 committed by Mathieu Bresciani
parent 34805e43fd
commit 2eba1847fd
5 changed files with 33 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
View File

@ -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()

3
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp
View File

@ -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<px4::params::HTE_HT_NOISE>) _param_hte_ht_noise,
(ParamFloat<px4::params::HTE_ACC_GATE>) _param_hte_acc_gate,
(ParamFloat<px4::params::HTE_HT_ERR_INIT>) _param_hte_ht_err_init,
(ParamFloat<px4::params::HTE_THR_RANGE>) _param_hte_thr_range,
(ParamFloat<px4::params::HTE_VXY_THR>) _param_hte_vxy_thr,
(ParamFloat<px4::params::HTE_VZ_THR>) _param_hte_vz_thr,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover

19
src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c
View File

@ -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);

4
src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp
View File

@ -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)

6
src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp
View File

@ -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)