AirspeedValidated: correct typo

Fixes:
 - False positive airspeed failures due to first principle check. This
   was caused by mixing different throttle units in the check: Some with
   battery scaling, some without. As the battery depletes, this
   difference can already trigger the quite sensitive check. Now it uses
   purely TECS setpoints again, which are without battery scaling.
 - Slightly wrong synthetic airspeed as battery depletes. Previously,
   the synthetic airspeed used thrust from vehicle_thrust_setpoint_0,
   which already includes battery scaling. Battery scaling is intended
   to offset the effects of a depleted battery, so we command a higher
   throttle when the battery is empty but we do not go faster. The TECS
   throttle setpoint more closely reflects the physical response of the
   vehicle and so it is preferrable to use that for synthetic airspeed.

Small simplifications without changing functionality:
 - update throttle filter only on TECS update. This removes the need to
   check for data recency inside of it. Functionality is kept -
   in any case if synthetic airspeed is demanded outside of fixed wing
   we give trim airspeed. For this we add an explicit if
 - remove current time argument from update_throttle_filter, is already
   class variable

msg/versioned/AirspeedValidated.msg

@@ -18,10 +18,10 @@ int8 SOURCE_GROUND_MINUS_WIND = 0  # Ground speed minus wind
 int8 SOURCE_SENSOR_1 = 1           # Sensor 1
 int8 SOURCE_SENSOR_2 = 2           # Sensor 2
 int8 SOURCE_SENSOR_3 = 3           # Sensor 3
-int8 SOURCE_SYNTHETIC = 4          # Synthetic airspeed 
+int8 SOURCE_SYNTHETIC = 4          # Synthetic airspeed
 
 float32 calibrated_ground_minus_wind_m_s  # [m/s] [@invalid NaN] CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption
-float32 calibraded_airspeed_synth_m_s     # [m/s] [@invalid NaN] Synthetic airspeed
-float32 airspeed_derivative_filtered      # [m/s^2] Filtered indicated airspeed derivative 
-float32 throttle_filtered                 # [-] Filtered fixed-wing throttle 
-float32 pitch_filtered                    # [rad] Filtered pitch 
+float32 calibrated_airspeed_synth_m_s     # [m/s] [@invalid NaN] Synthetic airspeed
+float32 airspeed_derivative_filtered      # [m/s^2] Filtered indicated airspeed derivative
+float32 throttle_filtered                 # [-] Filtered fixed-wing throttle
+float32 pitch_filtered                    # [rad] Filtered pitch

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -225,7 +225,7 @@ private:
 	void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */
 	void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */
 	float get_synthetic_airspeed(float throttle);
-	void update_throttle_filter(hrt_abstime t_now);
+	void update_throttle_filter();
 };
 
 AirspeedModule::AirspeedModule():
@@ -364,7 +364,6 @@ AirspeedModule::Run()
 	poll_topics();
 	update_wind_estimator_sideslip();
 	update_ground_minus_wind_airspeed();
-	update_throttle_filter(_time_now_usec);
 
 	if (_number_of_airspeed_sensors > 0) {
 
@@ -571,7 +570,9 @@ void AirspeedModule::poll_topics()
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
 	_position_setpoint_sub.update(&_position_setpoint);
 
-	_tecs_status_sub.update(&_tecs_status);
+	if (_tecs_status_sub.update(&_tecs_status)) {
+		update_throttle_filter();
+	}
 
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude;
@@ -741,7 +742,7 @@ void AirspeedModule::select_airspeed_and_publish()
 	airspeed_validated_s airspeed_validated = {};
 	airspeed_validated.timestamp = _time_now_usec;
 	airspeed_validated.calibrated_ground_minus_wind_m_s = NAN;
-	airspeed_validated.calibraded_airspeed_synth_m_s = NAN;
+	airspeed_validated.calibrated_airspeed_synth_m_s = NAN;
 	airspeed_validated.indicated_airspeed_m_s = NAN;
 	airspeed_validated.calibrated_airspeed_m_s = NAN;
 	airspeed_validated.true_airspeed_m_s = NAN;
@@ -763,7 +764,7 @@ void AirspeedModule::select_airspeed_and_publish()
 		airspeed_validated.calibrated_airspeed_m_s = _ground_minus_wind_CAS;
 		airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
 		airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
-		airspeed_validated.calibraded_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered);
+		airspeed_validated.calibrated_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered);
 
 		break;
 
@@ -773,7 +774,7 @@ void AirspeedModule::select_airspeed_and_publish()
 			float synthetic_airspeed = get_synthetic_airspeed(airspeed_validated.throttle_filtered);
 			airspeed_validated.calibrated_airspeed_m_s = synthetic_airspeed;
 			airspeed_validated.indicated_airspeed_m_s = synthetic_airspeed;
-			airspeed_validated.calibraded_airspeed_synth_m_s = synthetic_airspeed;
+			airspeed_validated.calibrated_airspeed_synth_m_s = synthetic_airspeed;
 			airspeed_validated.true_airspeed_m_s =
 				calc_true_from_calibrated_airspeed(synthetic_airspeed, _vehicle_air_data.rho);
 			break;
@@ -784,7 +785,7 @@ void AirspeedModule::select_airspeed_and_publish()
 		airspeed_validated.calibrated_airspeed_m_s = _airspeed_validator[valid_airspeed_index - 1].get_CAS();
 		airspeed_validated.true_airspeed_m_s = _airspeed_validator[valid_airspeed_index - 1].get_TAS();
 		airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS;
-		airspeed_validated.calibraded_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered);
+		airspeed_validated.calibrated_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered);
 		break;
 	}
 
@@ -817,8 +818,15 @@ float AirspeedModule::get_synthetic_airspeed(float throttle)
 	_flight_phase_estimation_sub.update();
 	flight_phase_estimation_s flight_phase_estimation = _flight_phase_estimation_sub.get();
 
-	if (flight_phase_estimation.flight_phase != flight_phase_estimation_s::FLIGHT_PHASE_LEVEL
+	if (_vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+	    || flight_phase_estimation.flight_phase != flight_phase_estimation_s::FLIGHT_PHASE_LEVEL
 	    || _time_now_usec - flight_phase_estimation.timestamp > 1_s) {
+
+		// When climbing, sinking, or in transition, we supply trim
+		// airspeed. This could be optimised -- but as synthetic
+		// airspeed is only a last resort for failed airspeed sensor,
+		// and airspeed dead-reckoning is itself already not expected to
+		// be accurate, it is probably not worth it.
 		synthetic_airspeed = _param_fw_airspd_trim.get();
 
 	} else if (throttle < _param_fw_thr_trim.get() && _param_fw_thr_aspd_min.get() > 0.f) {
@@ -838,30 +846,18 @@ float AirspeedModule::get_synthetic_airspeed(float throttle)
 	return synthetic_airspeed;
 }
 
-void AirspeedModule::update_throttle_filter(hrt_abstime now)
+void AirspeedModule::update_throttle_filter()
 {
-	if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		vehicle_thrust_setpoint_s vehicle_thrust_setpoint_0{};
-		_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint_0);
+	const float throttle_sp = _tecs_status.throttle_sp;
 
-		float forward_thrust = vehicle_thrust_setpoint_0.xyz[0];
+	const float dt = static_cast<float>(_time_now_usec - _t_last_throttle_fw) * 1e-6f;
+	_t_last_throttle_fw = _time_now_usec;
 
-		// if VTOL, use the total thrust vector length (otherwise needs special handling for tailsitters and tiltrotors)
-		if (_vehicle_status.is_vtol) {
-			forward_thrust = sqrtf(vehicle_thrust_setpoint_0.xyz[0] * vehicle_thrust_setpoint_0.xyz[0] +
-					       vehicle_thrust_setpoint_0.xyz[1] * vehicle_thrust_setpoint_0.xyz[1] +
-					       vehicle_thrust_setpoint_0.xyz[2] * vehicle_thrust_setpoint_0.xyz[2]);
-		}
+	if (dt < FLT_EPSILON || dt > 1.f) {
+		_throttle_filtered.reset(throttle_sp);
 
-		const float dt = static_cast<float>(now - _t_last_throttle_fw) * 1e-6f;
-		_t_last_throttle_fw = now;
-
-		if (dt < FLT_EPSILON || dt > 1.f) {
-			_throttle_filtered.reset(forward_thrust);
-
-		} else {
-			_throttle_filtered.update(forward_thrust, dt);
-		}
+	} else {
+		_throttle_filtered.update(throttle_sp, dt);
 	}
 }
 

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

@@ -165,8 +165,6 @@ void FwLateralLongitudinalControl::Run()
 			_landed = landed.landed;
 		}
 
-		_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
-
 		_vehicle_status_sub.update();
 		_control_mode_sub.update();
 
@@ -420,8 +418,13 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
 			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
 
 		} else {
-			//We can't infer the flight phase , do nothing, estimation is reset at each step
+			// We can't infer the flight phase , do nothing, estimation is reset at each step
+			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
+
 		}
+
+		_flight_phase_estimation_pub.get().timestamp = hrt_absolute_time();
+		_flight_phase_estimation_pub.update();
 	}
 }