Add synthetic airspeed based on thrust

ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5

@@ -7,6 +7,7 @@
 #
 # @board px4_fmu-v2 exclude
 # @board bitcraze_crazyflie exclude
+# @board px4_fmu-v6x exclude
 #
 
 . ${R}etc/init.d/rc.mc_defaults

docs/assets/airframes/fw/reptile_dragon_2/reptile_dragon_2_params.params

@@ -1,5 +1,5 @@
 param set-default ASPD_DO_CHECKS 19
-param set-default ASPD_FALLBACK_GW 1
+param set-default ASPD_FALLBACK 1
 param set-default BAT1_N_CELLS 4
 param set-default BAT1_V_CHARGED 4.2000
 param set-default BAT1_V_EMPTY 2.9000
@@ -61,4 +61,4 @@ param set-default PWM_AUX_TIM3 100
 param set-default SDLOG_PROFILE 17
 param set-default SENS_EN_MS4525DO 1
 param set-default TRIM_PITCH 0.1000
-# Make sure to add all params from the current airframe (ID=2100) as well
+# Make sure to add all params from the current airframe (ID=2100) as well

docs/assets/airframes/fw/turbo_timber_evolution/tteparams.params

@@ -1,5 +1,5 @@
 param set-default ASPD_DO_CHECKS 19
-param set-default ASPD_FALLBACK_GW 1
+param set-default ASPD_FALLBACK 1
 param set-default BAT1_N_CELLS 4
 param set-default BAT1_V_CHARGED 4.2000
 param set-default BAT1_V_EMPTY 2.5000
@@ -80,4 +80,4 @@ param set-default SENS_EN_MS4525DO 1
 param set-default SER_TEL1_BAUD 115200
 param set-default TRIM_PITCH -0.4000
 param set-default UAVCAN_ENABLE 0
-# Make sure to add all params from the current airframe (ID=2100) as well
+# Make sure to add all params from the current airframe (ID=2100) as well

msg/px4_msgs_old/msg/AirspeedValidatedV0.msg

@@ -0,0 +1,18 @@
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp				# time since system start (microseconds)
+
+float32 indicated_airspeed_m_s			# indicated airspeed in m/s (IAS), set to NAN if invalid
+float32 calibrated_airspeed_m_s     		# calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid
+float32 true_airspeed_m_s			# true filtered airspeed in m/s (TAS), set to NAN if invalid
+
+float32 calibrated_ground_minus_wind_m_s 	# CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
+float32 true_ground_minus_wind_m_s 		# TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
+
+bool airspeed_sensor_measurement_valid 		# True if data from at least one airspeed sensor is declared valid.
+
+int8 selected_airspeed_index 			# 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid
+
+float32 airspeed_derivative_filtered		# filtered indicated airspeed derivative [m/s/s]
+float32 throttle_filtered			# filtered fixed-wing throttle [-]
+float32 pitch_filtered				# filtered pitch [rad]

msg/translation_node/translations/all_translations.h

@@ -11,3 +11,4 @@
 //#include "example_translation_service_v1.h"
 
 #include "translation_vehicle_status_v1.h"
+#include "translation_airspeed_validated_v1.h"

msg/translation_node/translations/translation_airspeed_validated_v1.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+// Translate AirspeedValidated v0 <--> v1
+#include <px4_msgs_old/msg/airspeed_validated_v0.hpp>
+#include <px4_msgs/msg/airspeed_validated.hpp>
+
+class AirspeedValidatedV1Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::AirspeedValidatedV0;
+	static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+	using MessageNewer = px4_msgs::msg::AirspeedValidated;
+	static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+	static constexpr const char* kTopic = "fmu/out/airspeed_validated";
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+		// Set msg_newer from msg_older
+		msg_newer.timestamp = msg_older.timestamp;
+		msg_newer.indicated_airspeed_m_s = msg_older.indicated_airspeed_m_s;
+		msg_newer.calibrated_airspeed_m_s = msg_older.calibrated_airspeed_m_s;
+		msg_newer.true_airspeed_m_s = msg_older.true_airspeed_m_s;
+		msg_newer.airspeed_source = msg_older.selected_airspeed_index;
+		msg_newer.calibrated_ground_minus_wind_m_s = msg_older.calibrated_ground_minus_wind_m_s;
+		msg_newer.calibraded_airspeed_synth_m_s = NAN;
+		msg_newer.airspeed_derivative_filtered = msg_older.airspeed_derivative_filtered;
+		msg_newer.throttle_filtered = msg_older.throttle_filtered;
+		msg_newer.pitch_filtered = msg_older.pitch_filtered;
+	}
+
+	static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+		// Set msg_older from msg_newer
+		msg_older.timestamp = msg_newer.timestamp;
+		msg_older.indicated_airspeed_m_s = msg_newer.indicated_airspeed_m_s;
+		msg_older.calibrated_airspeed_m_s = msg_newer.calibrated_airspeed_m_s;
+		msg_older.true_airspeed_m_s = msg_newer.true_airspeed_m_s;
+		msg_older.calibrated_ground_minus_wind_m_s = msg_newer.calibrated_ground_minus_wind_m_s;
+		msg_older.true_ground_minus_wind_m_s = msg_newer.calibrated_ground_minus_wind_m_s;
+		msg_older.airspeed_sensor_measurement_valid = msg_newer.airspeed_source > 0 && msg_newer.airspeed_source <= 3;
+		msg_older.selected_airspeed_index = msg_newer.airspeed_source;
+		msg_older.airspeed_derivative_filtered = msg_newer.airspeed_derivative_filtered;
+		msg_older.throttle_filtered = msg_newer.throttle_filtered;
+		msg_older.pitch_filtered = msg_newer.pitch_filtered;
+	}
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(AirspeedValidatedV1Translation);

msg/versioned/AirspeedValidated.msg

@@ -1,18 +1,22 @@
-uint32 MESSAGE_VERSION = 0
+uint32 MESSAGE_VERSION = 1
 
 uint64 timestamp				# time since system start (microseconds)
 
-float32 indicated_airspeed_m_s			# indicated airspeed in m/s (IAS), set to NAN if invalid
-float32 calibrated_airspeed_m_s     		# calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid
-float32 true_airspeed_m_s			# true filtered airspeed in m/s (TAS), set to NAN if invalid
+float32 indicated_airspeed_m_s			# [m/s] Indicated airspeed (IAS), set to NAN if invalid
+float32 calibrated_airspeed_m_s     		# [m/s] Calibrated airspeed (CAS), set to NAN if invalid
+float32 true_airspeed_m_s			# [m/s] True airspeed (TAS), set to NAN if invalid
 
-float32 calibrated_ground_minus_wind_m_s 	# CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
-float32 true_ground_minus_wind_m_s 		# TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
-
-bool airspeed_sensor_measurement_valid 		# True if data from at least one airspeed sensor is declared valid.
-
-int8 selected_airspeed_index 			# 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid
+int8 airspeed_source				# Source of currently published airspeed values
+int8 DISABLED = -1
+int8 GROUND_MINUS_WIND = 0
+int8 SENSOR_1 = 1
+int8 SENSOR_2 = 2
+int8 SENSOR_3 = 3
+int8 SYNTHETIC = 4
 
+# debug states
+float32 calibrated_ground_minus_wind_m_s 	# CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
+float32 calibraded_airspeed_synth_m_s		# synthetic airspeed in m/s, set to NAN if invalid
 float32 airspeed_derivative_filtered		# filtered indicated airspeed derivative [m/s/s]
 float32 throttle_filtered			# filtered fixed-wing throttle [-]
 float32 pitch_filtered				# filtered pitch [rad]

src/drivers/osd/msp_osd/uorb_to_msp.cpp

@@ -267,9 +267,9 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
 	//raw_gps.hdop = vehicle_gps_position_struct.hdop
 	raw_gps.numSat = vehicle_gps_position.satellites_used;
 
-	if (airspeed_validated.airspeed_sensor_measurement_valid
+	if (airspeed_validated.airspeed_source >= airspeed_validated_s::GROUND_MINUS_WIND
 	    && PX4_ISFINITE(airspeed_validated.indicated_airspeed_m_s)
-	    && airspeed_validated.indicated_airspeed_m_s > 0) {
+	    && airspeed_validated.indicated_airspeed_m_s > 0.f) {
 		raw_gps.groundSpeed = airspeed_validated.indicated_airspeed_m_s * 100;
 
 	} else {

src/lib/airspeed/airspeed.cpp

@@ -214,3 +214,8 @@ float calc_calibrated_from_true_airspeed(float speed_true, float air_density)
 {
 	return speed_true * sqrtf(air_density / kAirDensitySeaLevelStandardAtmos);
 }
+
+float calc_true_from_calibrated_airspeed(float speed_calibrated, float air_density)
+{
+	return speed_calibrated * sqrtf(kAirDensitySeaLevelStandardAtmos / air_density);
+}

src/lib/airspeed/airspeed.h

@@ -137,6 +137,8 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe
  */
 __EXPORT float calc_calibrated_from_true_airspeed(float speed_true, float air_density);
 
+__EXPORT float calc_true_from_calibrated_airspeed(float speed_calibrated, float air_density);
+
 __END_DECLS
 
 #endif

src/lib/parameters/param_translation.cpp

@@ -142,5 +142,13 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2025-03-19: translate ASPD_FALLBACK_GW to ASPD_FALLBACK
+	{
+		if (strcmp("ASPD_FALLBACK_GW", node->name) == 0) {
+			strcpy(node->name, "ASPD_FALLBACK");
+			PX4_INFO("copying %s -> %s", "ASPD_FALLBACK_GW", "ASPD_FALLBACK");
+		}
+	}
+
 	return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
 }

src/modules/airspeed_selector/AirspeedValidator.cpp

@@ -60,7 +60,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	check_load_factor(input_data.accel_z);
 	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.hdg_test_ratio,
 				  input_data.ground_velocity, input_data.gnss_valid);
-	check_first_principle(input_data.timestamp, input_data.fixed_wing_tecs_throttle,
+	check_first_principle(input_data.timestamp, input_data.fixed_wing_throttle_filtered,
 			      input_data.fixed_wing_tecs_throttle_trim, input_data.tecs_timestamp, input_data.q_att);
 	update_airspeed_valid_status(input_data.timestamp);
 }
@@ -298,31 +298,28 @@ AirspeedValidator::check_first_principle(const uint64_t timestamp, const float t
 		return;
 	}
 
-	const float dt = static_cast<float>(timestamp - _time_last_first_principle_check) / 1_s;
+	const float dt = static_cast<float>(timestamp - _time_last_first_principle_check) * 1e-6f;
 	_time_last_first_principle_check = timestamp;
 
 	// update filters
 	if (dt < FLT_EPSILON || dt > 1.f) {
 		// reset if dt is too large
 		_IAS_derivative.reset(0.f);
-		_throttle_filtered.reset(throttle_fw);
 		_pitch_filtered.reset(pitch);
 		_time_last_first_principle_check_passing = timestamp;
 
 	} else {
 		// update filters, with different time constant
 		_IAS_derivative.setParameters(dt, 5.f);
-		_throttle_filtered.setParameters(dt, 0.5f);
 		_pitch_filtered.setParameters(dt, 1.5f);
 
 		_IAS_derivative.update(_IAS);
-		_throttle_filtered.update(throttle_fw);
 		_pitch_filtered.update(pitch);
 	}
 
 	// declare high throttle if more than 5% above trim
 	const float high_throttle_threshold = math::min(throttle_trim + kHighThrottleDelta, _param_throttle_max);
-	const bool high_throttle = _throttle_filtered.getState() > high_throttle_threshold;
+	const bool high_throttle = throttle_fw > high_throttle_threshold;
 	const bool pitching_down = _pitch_filtered.getState() < _param_psp_off;
 
 	// check if the airspeed derivative is too low given the throttle and pitch

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -68,7 +68,7 @@ struct airspeed_validator_update_data {
 	float vel_test_ratio;
 	float hdg_test_ratio;
 	bool in_fixed_wing_flight;
-	float fixed_wing_tecs_throttle;
+	float fixed_wing_throttle_filtered;
 	float fixed_wing_tecs_throttle_trim;
 	uint64_t tecs_timestamp;
 };
@@ -89,7 +89,6 @@ class AirspeedValidator
 	bool get_airspeed_valid() { return _airspeed_valid; }
 	float get_CAS_scale_validated() {return _CAS_scale_validated;}
 	float get_airspeed_derivative() { return _IAS_derivative.getState(); }
-	float get_throttle_filtered() { return _throttle_filtered.getState(); }
 	float get_pitch_filtered() { return _pitch_filtered.getState(); }
 
 	airspeed_wind_s get_wind_estimator_states(uint64_t timestamp);
@@ -181,7 +180,6 @@ class AirspeedValidator
 	bool _first_principle_check_failed{false}; ///< first principle check has detected failure
 	float _aspd_fp_t_window{0.f}; ///< time window for first principle check
 	FilteredDerivative<float> _IAS_derivative; ///< indicated airspeed derivative for first principle check
-	AlphaFilter<float> _throttle_filtered; ///< filtered throttle for first principle check
 	AlphaFilter<float> _pitch_filtered; ///< filtered pitch for first principle check
 	hrt_abstime _time_last_first_principle_check{0}; ///< time airspeed first principle was last checked (uSec)
 	hrt_abstime _time_last_first_principle_check_passing{0}; ///< time airspeed first principle was last passing (uSec)