diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp index ec6fc700c0..9d758890d7 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask.cpp @@ -5,9 +5,10 @@ constexpr uint64_t FlightTask::_timeout; int FlightTask::update() { - _time = hrt_elapsed_time(&_starting_time_stamp) / 1e6f; - _deltatime = math::min(hrt_elapsed_time(&_last_time_stamp), _timeout) / 1e6f; - _last_time_stamp = hrt_absolute_time(); + _time_stamp_current = hrt_absolute_time(); + _time = (_time_stamp_current - _time_stamp_activate) / 1e6f; + _deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f; + _time_stamp_last = _time_stamp_current; updateSubscriptions(); _evaluate_vehicle_position(); return 0; @@ -15,7 +16,7 @@ int FlightTask::update() void FlightTask::_evaluate_vehicle_position() { - if (hrt_elapsed_time(&_sub_vehicle_local_position.get().timestamp) < _timeout) { + if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) { _position = matrix::Vector3f(&_sub_vehicle_local_position.get().x); _velocity = matrix::Vector3f(&_sub_vehicle_local_position.get().vx); _yaw = _sub_vehicle_local_position.get().yaw; diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 1cf3b218a5..1aca5aac2e 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -63,7 +63,7 @@ public: */ virtual int activate() { - _starting_time_stamp = hrt_absolute_time(); + _time_stamp_activate = hrt_absolute_time(); FlightTask::update(); return 0; }; @@ -89,10 +89,13 @@ public: }; protected: + /* time abstraction */ static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */ - float _time = 0; /**< passed time in seconds since the task was activated */ float _deltatime = 0; /**< passed time in seconds since the task was last updated */ + hrt_abstime _time_stamp_activate = 0; /**< time stamp when task was activated */ + hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */ + hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */ /* Current vehicle position for every task */ matrix::Vector3f _position; /**< current vehicle position */ @@ -117,9 +120,6 @@ protected: private: uORB::Subscription _sub_vehicle_local_position; - hrt_abstime _starting_time_stamp = 0; /**< time stamp when task was activated */ - hrt_abstime _last_time_stamp = 0; /**< time stamp when task was last updated */ - vehicle_local_position_setpoint_s _vehicle_local_position_setpoint; /**< Output position setpoint that every task has */ void _evaluate_vehicle_position(); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 8e77d88759..267aa39eb2 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -120,7 +120,7 @@ int FlightTaskManual::update() int FlightTaskManual::_evaluate_sticks() { - if (hrt_elapsed_time(&_sub_manual_control_setpoint.get().timestamp) < _timeout) { + if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < _timeout) { /* get data and scale correctly */ _sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */ _sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */