multicopter hover thrust estimator use vehicle_thrust_setpoint (work …

…in stabilized mode)

src/modules/mc_att_control/mc_att_control.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -48,6 +48,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/autotune_attitude_control_status.h>
+#include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -103,6 +104,7 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
 	uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
@@ -122,6 +124,8 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 
 	matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector
 
+	float _hover_thrust{NAN};
+
 	float _man_yaw_sp{0.f};				/**< current yaw setpoint in manual mode */
 	float _man_tilt_max;			/**< maximum tilt allowed for manual flight [rad] */
 	AlphaFilter<float> _man_x_input_filter;

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -105,6 +105,20 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 {
 	const float throttle_min = _landed ? 0.0f : _param_mpc_manthr_min.get();
 
+	{
+		hover_thrust_estimate_s hte;
+
+		if (_hover_thrust_estimate_sub.update(&hte)) {
+			if (hte.valid) {
+				_hover_thrust = hte.hover_thrust;
+			}
+		}
+	}
+
+	if (!PX4_ISFINITE(_hover_thrust)) {
+		_hover_thrust = _param_mpc_thr_hover.get();
+	}
+
 	// throttle_stick_input is in range [0, 1]
 	switch (_param_mpc_thr_curve.get()) {
 	case 1: // no rescaling to hover throttle
@@ -113,7 +127,7 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 	default: // 0 or other: rescale to hover throttle at 0.5 stick
 		return math::gradual3(throttle_stick_input,
 				      0.f, .5f, 1.f,
-				      throttle_min, _param_mpc_thr_hover.get(), _param_mpc_thr_max.get());
+				      throttle_min, _hover_thrust, _param_mpc_thr_max.get());
 	}
 }
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run()
 		}
 	}
 
-	// new local position setpoint needed every iteration
-	if (!_vehicle_local_position_setpoint_sub.updated()) {
-		return;
-	}
-
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update
@@ -166,10 +161,24 @@ void MulticopterHoverThrustEstimator::Run()
 
 		_hover_thrust_ekf.predict(dt);
 
-		vehicle_local_position_setpoint_s local_pos_sp;
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+
+		vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
+
+		if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)
+		    && (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms)
+		    && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms)
+		   ) {
+			const matrix::Quatf q_att{vehicle_attitude.q};
+
+			matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz);
+			thrust_body_sp(0) = 0.f; // ignore for now
+			thrust_body_sp(1) = 0.f; // ignore for now
+
+			matrix::Vector3f thrust_sp = q_att.rotateVector(thrust_body_sp);
 
-		if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) {
-			if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
+			if (PX4_ISFINITE(thrust_sp(2))) {
 				// Inform the hover thrust estimator about the measured vertical
 				// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
 				// Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects
@@ -179,7 +188,7 @@ void MulticopterHoverThrustEstimator::Run()
 									1.f);
 
 				_hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z));
-				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
+				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_sp(2));
 
 				bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);
 
@@ -191,7 +200,7 @@ void MulticopterHoverThrustEstimator::Run()
 				_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
 				_valid = _valid_hysteresis.get_state();
 
-				publishStatus(local_pos.timestamp);
+				publishStatus(vehicle_thrust_setpoint.timestamp);
 			}
 		}
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -54,10 +54,11 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
 
 #include "zero_order_hover_thrust_ekf.hpp"
 
@@ -101,9 +102,11 @@ class MulticopterHoverThrustEstimator : public ModuleBase<MulticopterHoverThrust
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
 
 	hrt_abstime _timestamp_last{0};