From 91c0f19121d9dbb80fa284483f4b1f4f79fc7e55 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 14 Jul 2020 15:24:40 +0200
Subject: [PATCH] FlightTaskAuto: use new Sticks class for assisted land speed

---
 .../tasks/Auto/FlightTaskAuto.cpp             |  1 -
 .../tasks/Auto/FlightTaskAuto.hpp             |  1 -
 .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 28 +++++++++----------
 .../tasks/AutoMapper/FlightTaskAutoMapper.hpp |  5 ++--
 4 files changed, 17 insertions(+), 18 deletions(-)

diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
index 592221631c..dbcda481bf 100644
--- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
@@ -64,7 +64,6 @@ bool FlightTaskAuto::updateInitialize()
 	bool ret = FlightTask::updateInitialize();
 
 	_sub_home_position.update();
-	_sub_manual_control_setpoint.update();
 	_sub_vehicle_status.update();
 	_sub_triplet_setpoint.update();
 
diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
index fca28f6185..16c88fbaa0 100644
--- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
@@ -108,7 +108,6 @@ protected:
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
 
 	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
-	uORB::SubscriptionData<manual_control_setpoint_s>	_sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
 	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
 
 	State _current_state{State::none};
diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
index 3c342bbed1..981e65a720 100644
--- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
@@ -40,6 +40,10 @@
 
 using namespace matrix;
 
+FlightTaskAutoMapper::FlightTaskAutoMapper() :
+	_sticks(this)
+{};
+
 bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
 	bool ret = FlightTaskAuto::activate(last_setpoint);
@@ -176,15 +180,6 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear()
 
 float FlightTaskAutoMapper::_getLandSpeed()
 {
-	bool rc_assist_enabled = _param_mpc_land_rc_help.get();
-	bool rc_is_valid = !_sub_vehicle_status.get().rc_signal_lost;
-
-	float throttle = 0.5f;
-
-	if (rc_is_valid && rc_assist_enabled) {
-		throttle = _sub_manual_control_setpoint.get().z;
-	}
-
 	float speed = 0;
 
 	if (_dist_to_ground > _param_mpc_land_alt1.get()) {
@@ -194,13 +189,18 @@ float FlightTaskAutoMapper::_getLandSpeed()
 		const float land_speed = math::gradual(_dist_to_ground,
 						       _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
 						       _param_mpc_land_speed.get(), _constraints.speed_down);
-		const float head_room = _constraints.speed_down - land_speed;
 
-		speed = land_speed + 2 * (0.5f - throttle) * head_room;
+		// user input assisted land speed
+		if (_param_mpc_land_rc_help.get()) {
+			_sticks.evaluateSticks(_time_stamp_current);
+			const float head_room = _constraints.speed_down - land_speed;
 
-		// Allow minimum assisted land speed to be half of parameter
-		if (speed < land_speed * 0.5f) {
-			speed = land_speed * 0.5f;
+			speed = land_speed + _sticks.getPositionExpo()(2) * head_room;
+
+			// Allow minimum assisted land speed to be half of parameter
+			if (speed < land_speed * 0.5f) {
+				speed = land_speed * 0.5f;
+			}
 		}
 	}
 
diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
index 5f82bd3a80..457419708e 100644
--- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
+++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
@@ -41,11 +41,12 @@
 #pragma once
 
 #include "FlightTaskAuto.hpp"
+#include "Sticks.hpp"
 
 class FlightTaskAutoMapper : public FlightTaskAuto
 {
 public:
-	FlightTaskAutoMapper() = default;
+	FlightTaskAutoMapper();
 	virtual ~FlightTaskAutoMapper() = default;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 	bool update() override;
@@ -76,7 +77,7 @@ protected:
 				       );
 
 private:
-
+	Sticks _sticks;
 	void _reset(); /**< Resets member variables to current vehicle state */
 	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
 	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */