From 80f3fefcc342e54a8b99d21feecd8c45db341e42 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 18 Feb 2016 21:49:00 -0500
Subject: [PATCH] FW adjust throttle to airspeed mapping for ALTCTL

---
 .../fw_pos_control_l1_main.cpp                | 29 ++++++++++++++-----
 1 file changed, 22 insertions(+), 7 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index fb18df5dc6..3d45b0d722 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -452,6 +452,7 @@ private:
 	float		get_tecs_pitch();
 	float		get_tecs_thrust();
 
+	float		get_demanded_airspeed();
 	float		calculate_target_airspeed(float airspeed_demand);
 	void		calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d,
 			const struct position_setpoint_triplet_s &pos_sp_triplet);
@@ -900,6 +901,25 @@ FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
 	l1_control::g_control = nullptr;
 }
 
+float
+FixedwingPositionControl::get_demanded_airspeed()
+{
+	float altctrl_airspeed = 0;
+	// neutral throttle corresponds to trim airspeed
+	if (_manual.z < 0.5f) {
+		// lower half of throttle is min to trim airspeed
+		altctrl_airspeed = _parameters.airspeed_min +
+							(_parameters.airspeed_trim - _parameters.airspeed_min) *
+							_manual.z * 2;
+	} else {
+		// upper half of throttle is trim to max airspeed
+		altctrl_airspeed = _parameters.airspeed_trim +
+							(_parameters.airspeed_max - _parameters.airspeed_trim) *
+							(_manual.z * 2 - 1);
+	}
+	return altctrl_airspeed;
+}
+
 float
 FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 {
@@ -1769,10 +1789,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		_control_mode_current = FW_POSCTRL_MODE_POSITION;
 
-		/* Get demanded airspeed */
-		float altctrl_airspeed = _parameters.airspeed_min +
-					 (_parameters.airspeed_max - _parameters.airspeed_min) *
-					 _manual.z;
+		float altctrl_airspeed = get_demanded_airspeed();
 
 		/* update desired altitude based on user pitch stick input */
 		bool climbout_requested = update_desired_altitude(dt);
@@ -1888,9 +1905,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
 
 		/* Get demanded airspeed */
-		float altctrl_airspeed = _parameters.airspeed_min +
-					 (_parameters.airspeed_max - _parameters.airspeed_min) *
-					 _manual.z;
+		float altctrl_airspeed = get_demanded_airspeed();
 
 		/* update desired altitude based on user pitch stick input */
 		bool climbout_requested = update_desired_altitude(dt);