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
@@ -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>
@@ -98,6 +99,12 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+<<<<<<< HEAD
+=======
+	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 */
+>>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode))
 	uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
@@ -118,13 +125,22 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 
 	matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */
 
+<<<<<<< HEAD
 	float _man_yaw_sp{0.f};                 /**< current yaw setpoint in manual mode */
 	float _man_tilt_max;                    /**< maximum tilt allowed for manual flight [rad] */
 
 	SlewRate<float> _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air
 	SlewRate<float> _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up
 	AlphaFilter<float> _man_roll_input_filter;
 	AlphaFilter<float> _man_pitch_input_filter;
+=======
+	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;
+	AlphaFilter<float> _man_y_input_filter;
+>>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode))
 
 	hrt_abstime _last_run{0};
 	hrt_abstime _last_attitude_setpoint{0};

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
@@ -103,16 +103,41 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 {
 	float thrust = 0.f;
 
+<<<<<<< HEAD
+=======
+	{
+		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]
+>>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode))
 	switch (_param_mpc_thr_curve.get()) {
 	case 1: // no rescaling to hover throttle
 		thrust = math::interpolate(throttle_stick_input, -1.f, 1.f,
 					   _manual_throttle_minimum.getState(), _param_mpc_thr_max.get());
 		break;
 
+<<<<<<< HEAD
 	default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle
 		thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f},
 		{_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()});
 		break;
+=======
+	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, _hover_thrust, _param_mpc_thr_max.get());
+>>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode))
 	}
 
 	return math::min(thrust, _manual_throttle_maximum.getState());

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