fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 23:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: add SubscriptionArray class that contains all subscriptions

Browse Source 
This is to avoid dynamic (re-)allocations on task switching, due to
orb_{un,}subscribe calls.
This commit is contained in:
Beat Küng
2018-04-05 07:30:12 +02:00
parent b5ecf9824d
commit 9b571abb47
9 changed files with 245 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTask.cpp
+14 -5
View File
@@ -3,23 +3,32 @@
constexpr uint64_t FlightTask::_timeout;
bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) {
return false;
}
return true;
}
int FlightTask::update()
{
_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();
int ret = _evaluate_vehicle_position();
return ret;
}
int FlightTask::_evaluate_vehicle_position()
{
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;
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;
return 0;
} else {