FlightTaskAuto: pure virtual reset method

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)) {

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*/
 

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));
+	_reset();
-	_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;
-
 	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 */

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. */
 };