FWPositionController: change way of estimating airspeed derivative

Use local_position.a in direction of local_position.v - wind when wind is valid,
and otherwise simply input 0 TAS_rate_estimate to TECS.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/TecsStatus.msg

@@ -9,7 +9,7 @@ float32 true_airspeed_sp		# True airspeed setpoint [m/s]
 float32 true_airspeed_filtered		# True airspeed filtered [m/s]
 float32 true_airspeed_derivative_sp	# True airspeed derivative setpoint [m/s^2]
 float32 true_airspeed_derivative	# True airspeed derivative [m/s^2]
-float32 true_airspeed_derivative_raw	# True airspeed derivative raw [m/s^2]
+float32 ground_speed_derivative_raw	# Ground speed derivative in direction of travel raw [m/s^2]
 
 float32 total_energy_rate_sp		# Total energy rate setpoint [m^2/s^3]
 float32 total_energy_rate		# Total energy rate estimate [m^2/s^3]

src/modules/fw_path_navigation/FixedwingPositionControl.cpp

@@ -466,7 +466,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 
 void
 FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
-		float true_airspeed_derivative_raw, float throttle_trim)
+		float ground_speed_derivative_raw, float throttle_trim)
 {
 	tecs_status_s t{};
 
@@ -491,7 +491,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
 	t.true_airspeed_filtered = debug_output.true_airspeed_filtered;
 	t.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
 	t.true_airspeed_derivative = debug_output.true_airspeed_derivative;
-	t.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
+	t.ground_speed_derivative_raw = ground_speed_derivative_raw;
 	t.total_energy_rate = debug_output.control.total_energy_rate_estimate;
 	t.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
 	t.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
@@ -2553,6 +2553,13 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
 					 throttle_max);
 
+	const Vector3f local_pos_a = Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
+	const Vector3f groundspee_minus_wind = Vector3f{_local_pos.vx - _wind_vel(0), _local_pos.vy - _wind_vel(1), _local_pos.vz};
+
+	// only do acceleration control in case wind is valid, and otherwise set it to 0
+	const float acceleration_in_direction_of_airspeed_vector = _wind_valid ? groundspee_minus_wind.unit_or_zero().dot(
+				local_pos_a) : 0.f;
+
 	_tecs.update(_pitch - radians(_param_fw_psp_off.get()),
 		     _current_altitude,
 		     alt_sp,
@@ -2566,11 +2573,11 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     pitch_max_rad - radians(_param_fw_psp_off.get()),
 		     desired_max_climbrate,
 		     desired_max_sinkrate,
-		     _body_acceleration_x,
+		     acceleration_in_direction_of_airspeed_vector,
 		     -_local_pos.vz,
 		     hgt_rate_sp);
 
-	tecs_status_publish(alt_sp, airspeed_sp, -_local_pos.vz, throttle_trim_comp);
+	tecs_status_publish(alt_sp, airspeed_sp, acceleration_in_direction_of_airspeed_vector, throttle_trim_comp);
 }
 
 float

src/modules/fw_path_navigation/FixedwingPositionControl.hpp

@@ -445,7 +445,7 @@ private:
 
 	void status_publish();
 	void landing_status_publish();
-	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
+	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float ground_speed_derivative_raw,
 				 float throttle_trim);
 	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);