FlightTasks: added timer in base class that counts from activation on including possibility to reset in subclass

src/lib/FlightTasks/FlightTasks.hpp

@@ -55,7 +55,7 @@ public:
 	 * @param TODO
 	 * @return 0 on success, >0 on error otherwise
 	 */
-	int update(manual_control_setpoint_s *manual_control_setpoint, vehicle_local_position_s *vehicle_local_position) { return Orbit.update(NULL, NULL); };
+	int update(manual_control_setpoint_s *manual_control_setpoint, vehicle_local_position_s *vehicle_position) { return Orbit.update(NULL, NULL); };
 
 	/**
 	 * Call to get result of the task execution

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -54,7 +54,11 @@ public:
 	 * Call once on the event where you switch to the task
 	 * @return 0 on success, >0 on error otherwise
 	 */
-	virtual int activate() = 0;
+	virtual int activate()
+	{
+		_reset_time();
+		return 0;
+	};
 
 	/**
 	 * Call once on the event of switching away from the task
@@ -67,8 +71,11 @@ public:
 	 * @param TODO
 	 * @return 0 on success, >0 on error otherwise
 	 */
-	virtual int update(manual_control_setpoint_s *manual_control_setpoint,
-			   vehicle_local_position_s *vehicle_local_position) = 0;
+	virtual int update(manual_control_setpoint_s *manual_control_setpoint, vehicle_local_position_s *vehicle_position)
+	{
+		_time = hrt_elapsed_time(&_starting_time_stamp) / 1e6;
+		return 0;
+	};
 
 	/**
 	 * Call to get result of the task execution
@@ -78,6 +85,9 @@ public:
 
 protected:
 
+	float _get_time() { return _time; }
+	void _reset_time() { _starting_time_stamp = hrt_absolute_time(); };
+
 	void _set_position_setpoint(const matrix::Vector3f position_setpoint)
 	{
 		_vehicle_position_setpoint.x = position_setpoint(0);
@@ -101,6 +111,8 @@ protected:
 
 private:
 
+	float _time = 0; /*< passed time in seconds since the task was activated */
+	hrt_abstime _starting_time_stamp; /*< time stamp when task was activated */
 	vehicle_local_position_setpoint_s _vehicle_position_setpoint;
 
 };

src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp

@@ -55,7 +55,7 @@ public:
 	 */
 	virtual int activate()
 	{
-		_set_position_setpoint(matrix::Vector3f());
+		FlightTask::activate();
 		return 0;
 	};
 
@@ -72,8 +72,8 @@ public:
 	 */
 	virtual int update(manual_control_setpoint_s *manual_control_setpoint, vehicle_local_position_s *vehicle_position)
 	{
-		float time = hrt_absolute_time() / 1e6;
-		_set_position_setpoint(matrix::Vector3f(0.1f * time, 0.f, -2.f));
+		FlightTask::update(NULL, NULL);
+		_set_position_setpoint(matrix::Vector3f(0.1f * _get_time(), 0.f, -2.f));
 		return 0;
 	};