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

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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

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

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

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)

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)