diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 0fa21c749a..9039d398e9 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/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 diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 7877de685d..0b1d2ddce2 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/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; }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index 4f160f1187..c564f9ed27 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/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; };