fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 07:44:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: replaced all hrt_elapsed() calls and unneeded hrt_ calls to safe performance

Browse Source
This commit is contained in:
Matthias Grob 2017-11-20 14:27:56 +01:00 committed by Beat Küng
parent 9fdb3ace0c
commit 8a4d51c630
3 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/FlightTasks/tasks/FlightTask.cpp
View File

@ -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;

10
src/lib/FlightTasks/tasks/FlightTask.hpp
View File

@ -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<vehicle_local_position_s> _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();

2
src/lib/FlightTasks/tasks/FlightTaskManual.cpp
View File

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