PX4 Firmware
PX4 Autopilot Software http://px4.io
Subscription.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file Subscription.cpp
36  *
37  */
38 
39 #include "Subscription.hpp"
40 #include <px4_platform_common/defines.h>
41 
42 namespace uORB
43 {
44 
45 bool
47 {
48  // valid ORB_ID required
49  if (_meta == nullptr) {
50  return false;
51  }
52 
53  // check if already subscribed
54  if (_node != nullptr) {
55  return true;
56  }
57 
59 
60  if (device_master != nullptr) {
61  uORB::DeviceNode *node = device_master->getDeviceNode(_meta, _instance);
62 
63  if (node != nullptr) {
64  _node = node;
66 
67  // If there were any previous publications, allow the subscriber to read them
68  const unsigned curr_gen = _node->published_message_count();
69  const uint8_t q_size = _node->get_queue_size();
70 
71  if (q_size < curr_gen) {
72  _last_generation = curr_gen - q_size;
73 
74  } else {
75  _last_generation = 0;
76  }
77 
78  return true;
79  }
80  }
81 
82  return false;
83 }
84 
85 void
87 {
88  if (_node != nullptr) {
90  }
91 
92  _node = nullptr;
93  _last_generation = 0;
94 }
95 
96 bool
98 {
99  if (_meta != nullptr) {
100  // this throttles the relatively expensive calls to getDeviceNode()
101  if ((_last_generation == 0) || (_last_generation < 1000) || (_last_generation % 100 == 0)) {
102  if (subscribe()) {
103  return true;
104  }
105  }
106 
107  if (_node == nullptr) {
108  // use generation to count attempts to subscribe
110  }
111  }
112 
113  return false;
114 }
115 
116 bool
117 Subscription::update(uint64_t *time, void *dst)
118 {
119  if ((time != nullptr) && (dst != nullptr) && advertised()) {
120  // always copy data to dst regardless of update
121  const uint64_t t = _node->copy_and_get_timestamp(dst, _last_generation);
122 
123  if (*time == 0 || *time != t) {
124  *time = t;
125  return true;
126  }
127  }
128 
129  return false;
130 }
131 
132 } // namespace uORB
static uORB::Manager * get_instance()
Method to get the singleton instance for the uORB::Manager.
Definition: uORBManager.hpp:89
uint8_t get_queue_size() const
uORB::DeviceMaster * get_device_master()
Get the DeviceMaster.
Definition: uORBManager.cpp:92
const orb_metadata * _meta
unsigned published_message_count() const
Per-object device instance.
uint64_t copy_and_get_timestamp(void *dst, unsigned &generation)
Copies data and the corresponding generation from a node to the buffer provided.
void remove_internal_subscriber()
Removes the subscriber from the list.
unsigned _last_generation
Subscription&#39;s latest data generation.
void add_internal_subscriber()
Add the subscriber to the node&#39;s list of subscriber.
bool update(void *dst)
Update the struct.
Master control device for ObjDev.
DeviceNode * _node
uORB::DeviceNode * getDeviceNode(const char *node_name)
Public interface for getDeviceNodeLocked().