multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized/manual 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
@@ -47,6 +47,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>

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

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,34 @@ 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;
+		control_allocator_status_s control_allocator_status;
+
+		if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)
+		    && _control_allocator_status_sub.update(&control_allocator_status)
+		    && (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);
+			matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust);
+
+			thrust_body_sp(0) = 0.f; // ignore for now
+			thrust_body_sp(1) = 0.f; // ignore for now
+
+			thrust_body_unallocated(0) = 0.f; // ignore for now
+			thrust_body_unallocated(1) = 0.f; // ignore for now
+
+
+			matrix::Vector3f thrust_sp          = q_att.rotateVector(thrust_body_sp);
+			matrix::Vector3f thrust_unallocated = q_att.rotateVector(thrust_body_unallocated);
 
-		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 +198,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) - thrust_unallocated(2)));
 
 				bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);
 
@@ -191,7 +210,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,12 @@
 #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 <uORB/topics/control_allocator_status.h>
 
 #include "zero_order_hover_thrust_ekf.hpp"
 
@@ -101,9 +103,12 @@ 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)};
+	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
 
 	hrt_abstime _timestamp_last{0};