From 69ea4df45cdeb432dd5d4e13f9b4bd9ea18a5f84 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Tue, 13 Feb 2018 13:47:11 +0100
Subject: [PATCH] FlightTaskAuto: pure virtual reset method

---
 src/lib/FlightTasks/tasks/FlightTaskAuto.cpp  |  8 ++++++-
 src/lib/FlightTasks/tasks/FlightTaskAuto.hpp  |  2 ++
 .../FlightTasks/tasks/FlightTaskAutoLine.cpp  | 22 ++++++++++++-------
 .../FlightTasks/tasks/FlightTaskAutoLine.hpp  |  4 ++++
 4 files changed, 27 insertions(+), 9 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
index 66081c1ce4..62a695393d 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
@@ -95,7 +95,13 @@ bool FlightTaskAuto::_evaluateTriplets()
 	}
 
 	_yaw_wp = _sub_triplet_setpoint->get().current.yaw;
-	_type = (WaypointType)_sub_triplet_setpoint->get().current.type;
+	WaypointType type = (WaypointType)_sub_triplet_setpoint->get().current.type;
+
+	if (type != _type) {
+		_reset();
+	}
+
+	_type = type;
 
 	/* We need to have a valid current triplet */
 	if (_isFinite(_sub_triplet_setpoint->get().current)) {
diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
index bdc6a22dbd..b2ee30e9d5 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
@@ -80,6 +80,8 @@ protected:
 		0.0f; /**< Cruise speed with which multicopter flies and gets set by triplet. If no valid, default cruise speed is used. */
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
 
+	virtual void _reset() = 0; /**< Method called one type has changed. */
+
 private:
 	control::BlockParamFloat _mc_cruise_default; /**< Default mc cruise speed*/
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
index aaaa0f169f..147b19e854 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
@@ -59,14 +59,7 @@ FlightTaskAutoLine::FlightTaskAutoLine(control::SuperBlock *parent, const char *
 
 bool FlightTaskAutoLine::activate()
 {
-	_vel_sp_xy = matrix::Vector2f(&_velocity(0));
-	_pos_sp_xy = matrix::Vector2f(&_position(0));
-	_vel_sp_z = _velocity(2);
-	_pos_sp_z = _position(2);
-	_destination = _target;
-	_origin = _prev_wp;
-	_speed_at_target = 0.0f;
-
+	_reset();
 	return FlightTaskAuto::activate();
 }
 
@@ -101,6 +94,19 @@ bool FlightTaskAutoLine::update()
 	return true;
 }
 
+
+void FlightTaskAutoLine::_reset()
+{
+	/* Set setpoints equal current state. */
+	_vel_sp_xy = matrix::Vector2f(&_velocity(0));
+	_pos_sp_xy = matrix::Vector2f(&_position(0));
+	_vel_sp_z = _velocity(2);
+	_pos_sp_z = _position(2);
+	_destination = _target;
+	_origin = _prev_wp;
+	_speed_at_target = 0.0f;
+}
+
 void FlightTaskAutoLine::_generateIdleSetpoints()
 {
 	/* Send zero thrust setpoint */
diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
index 226a7da647..3f271bb202 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
@@ -93,5 +93,9 @@ protected:
 	void _generateSetpoints(); /**< Generate velocity and position setpoint for following line. */
 	void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */
 	void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */
+
+private:
+	float _getVelocityFromAngle(const float angle); /** Computes the speed at target depending on angle. */
+	void _reset(); /** Resets setpoint. */
 	float _getVelcoityFromAngle(const float angle); /** Computes the speed at target depending on angle. */
 };