Compensate VTOL transition time for air density (#19293)

* vtol: compensate front transition minimum time and front transiton openloop time
for air density

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/vtol_att_control/standard.cpp

@@ -179,7 +179,7 @@ void Standard::update_vtol_state()
 
 			const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
 					&& !_params->airspeed_disabled;
-			const bool minimum_trans_time_elapsed = time_since_trans_start > _params->front_trans_time_min;
+			const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime();
 
 			bool transition_to_fw = false;
 
@@ -261,14 +261,14 @@ void Standard::update_transition_state()
 		    PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
 		    _airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
 		    _airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend &&
-		    time_since_trans_start > _params->front_trans_time_min) {
+		    time_since_trans_start > getMinimumFrontTransitionTime()) {
 
 			mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
 		} else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
-			mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min;
+			mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime();
 			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
 
 		}

src/modules/vtol_att_control/tiltrotor.cpp

@@ -180,13 +180,13 @@ void Tiltrotor::update_vtol_state()
 
 				bool transition_to_p2 = false;
 
-				if (time_since_trans_start > _params->front_trans_time_min) {
+				if (time_since_trans_start > getMinimumFrontTransitionTime()) {
 					if (airspeed_triggers_transition) {
 						transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
 
 					} else {
 						transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
-								   time_since_trans_start > _params->front_trans_time_openloop;;
+								   time_since_trans_start > getOpenLoopFrontTransitionTime();
 					}
 				}
 
@@ -359,9 +359,9 @@ void Tiltrotor::update_transition_state()
 
 		// without airspeed do timed weight changes
 		if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
-		    time_since_trans_start > _params->front_trans_time_min) {
-			_mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) /
-					  (_params->front_trans_time_openloop - _params->front_trans_time_min);
+		    time_since_trans_start > getMinimumFrontTransitionTime()) {
+			_mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) /
+					  (getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());
 			_mc_yaw_weight = _mc_roll_weight;
 		}
 

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -460,6 +460,12 @@ VtolAttitudeControl::Run()
 		action_request_poll();
 		vehicle_cmd_poll();
 
+		vehicle_air_data_s air_data;
+
+		if (_vehicle_air_data_sub.update(&air_data)) {
+			_air_density = air_data.rho;
+		}
+
 		// check if mc and fw sp were updated
 		bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
 		bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -68,6 +68,7 @@
 #include <uORB/topics/action_request.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/tecs_status.h>
@@ -125,6 +126,8 @@ public:
 	bool get_immediate_transition() {return _immediate_transition;}
 	void reset_immediate_transition() {_immediate_transition = false;}
 
+	float getAirDensity() const { return _air_density; }
+
 	struct actuator_controls_s 			*get_actuators_fw_in() {return &_actuators_fw_in;}
 	struct actuator_controls_s 			*get_actuators_mc_in() {return &_actuators_mc_in;}
 	struct actuator_controls_s 			*get_actuators_out0() {return &_actuators_out_0;}
@@ -159,6 +162,7 @@ private:
 
 	uORB::Subscription _action_request_sub{ORB_ID(action_request)};
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};			// airspeed subscription
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 	uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)};
 	uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)};			// setpoint subscription
@@ -206,6 +210,8 @@ private:
 	vehicle_local_position_setpoint_s	_local_pos_sp{};
 	vtol_vehicle_status_s 			_vtol_vehicle_status{};
 
+	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};	// [kg/m^3]
+
 	Params _params{};	// struct holding the parameters
 
 	struct {

src/modules/vtol_att_control/vtol_type.cpp

@@ -648,3 +648,32 @@ float VtolType::pusher_assist()
 	return forward_thrust;
 
 }
+
+float VtolType::getFrontTransitionTimeFactor() const
+{
+	// assumptions: transition_time = transition_true_airspeed / average_acceleration (thrust)
+	// transition_true_airspeed ~ sqrt(rho0 / rh0)
+	// average_acceleration ~ rho / rho0
+	// transition_time ~ sqrt(rho0/rh0) * rho0 / rho
+
+	// low value: hot day at 4000m AMSL with some margin
+	// high value: cold day at 0m AMSL with some margin
+	const float rho = math::constrain(_attc->getAirDensity(), 0.7f, 1.5f);
+
+	if (PX4_ISFINITE(rho)) {
+		float rho0_over_rho = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / rho;
+		return sqrtf(rho0_over_rho) * rho0_over_rho;
+	}
+
+	return 1.0f;
+}
+
+float VtolType::getMinimumFrontTransitionTime() const
+{
+	return getFrontTransitionTimeFactor() * _params->front_trans_time_min;
+}
+
+float VtolType::getOpenLoopFrontTransitionTime() const
+{
+	return getFrontTransitionTimeFactor() * _params->front_trans_time_openloop;
+}

src/modules/vtol_att_control/vtol_type.h

@@ -195,6 +195,16 @@ public:
 
 	bool was_in_trans_mode() {return _flag_was_in_trans_mode;}
 
+	/**
+	 * @return Minimum front transition time scaled for air density (if available) [s]
+	*/
+	float getMinimumFrontTransitionTime() const;
+
+	/**
+	* @return Minimum open-loop front transition time scaled for air density (if available) [s]
+	*/
+	float getOpenLoopFrontTransitionTime() const;
+
 	virtual void parameters_update() = 0;
 
 	VtolAttitudeControl *_attc;
@@ -329,6 +339,11 @@ private:
 
 	void stopBlendingThrottleAfterFrontTransition() { _throttle_blend_start_ts = 0; }
 
+	/**
+	 * @return Transition time scale factor for density.
+	*/
+	float getFrontTransitionTimeFactor() const;
+
 };
 
 #endif