|
|
|
@ -49,12 +49,14 @@ std::map<std::string, uORB::DeviceNode*> uORB::DeviceMaster::_node_map;
@@ -49,12 +49,14 @@ std::map<std::string, uORB::DeviceNode*> uORB::DeviceMaster::_node_map;
|
|
|
|
|
uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp) |
|
|
|
|
{ |
|
|
|
|
uORB::DeviceNode::SubscriberData *sd; |
|
|
|
|
|
|
|
|
|
if (filp) { |
|
|
|
|
sd = (uORB::DeviceNode::SubscriberData *)(filp->priv); |
|
|
|
|
} |
|
|
|
|
else { |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
sd = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return sd; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -74,8 +76,9 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name,
@@ -74,8 +76,9 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name,
|
|
|
|
|
|
|
|
|
|
uORB::DeviceNode::~DeviceNode() |
|
|
|
|
{ |
|
|
|
|
if (_data != nullptr) |
|
|
|
|
if (_data != nullptr) { |
|
|
|
|
delete[] _data; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -105,9 +108,10 @@ uORB::DeviceNode::open(device::file_t *filp)
@@ -105,9 +108,10 @@ uORB::DeviceNode::open(device::file_t *filp)
|
|
|
|
|
ret = VDev::open(filp); |
|
|
|
|
|
|
|
|
|
/* open failed - not the publisher anymore */ |
|
|
|
|
if (ret != PX4_OK) |
|
|
|
|
if (ret != PX4_OK) { |
|
|
|
|
_publisher = 0; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return ret; |
|
|
|
|
} |
|
|
|
@ -118,8 +122,9 @@ uORB::DeviceNode::open(device::file_t *filp)
@@ -118,8 +122,9 @@ uORB::DeviceNode::open(device::file_t *filp)
|
|
|
|
|
/* allocate subscriber data */ |
|
|
|
|
SubscriberData *sd = new SubscriberData; |
|
|
|
|
|
|
|
|
|
if (nullptr == sd) |
|
|
|
|
if (nullptr == sd) { |
|
|
|
|
return -ENOMEM; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
memset(sd, 0, sizeof(*sd)); |
|
|
|
|
|
|
|
|
@ -177,12 +182,14 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
@@ -177,12 +182,14 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
|
|
|
|
|
SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); |
|
|
|
|
|
|
|
|
|
/* if the object has not been written yet, return zero */ |
|
|
|
|
if (_data == nullptr) |
|
|
|
|
if (_data == nullptr) { |
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* if the caller's buffer is the wrong size, that's an error */ |
|
|
|
|
if (buflen != _meta->o_size) |
|
|
|
|
if (buflen != _meta->o_size) { |
|
|
|
|
return -EIO; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Perform an atomic copy & state update |
|
|
|
@ -190,8 +197,9 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
@@ -190,8 +197,9 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
|
|
|
|
|
lock(); |
|
|
|
|
|
|
|
|
|
/* if the caller doesn't want the data, don't give it to them */ |
|
|
|
|
if (nullptr != buffer) |
|
|
|
|
if (nullptr != buffer) { |
|
|
|
|
memcpy(buffer, _data, _meta->o_size); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* track the last generation that the file has seen */ |
|
|
|
|
sd->generation = _generation; |
|
|
|
@ -227,19 +235,22 @@ uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen)
@@ -227,19 +235,22 @@ uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen)
|
|
|
|
|
lock(); |
|
|
|
|
|
|
|
|
|
/* re-check size */ |
|
|
|
|
if (nullptr == _data) |
|
|
|
|
if (nullptr == _data) { |
|
|
|
|
_data = new uint8_t[_meta->o_size]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
unlock(); |
|
|
|
|
|
|
|
|
|
/* failed or could not allocate */ |
|
|
|
|
if (nullptr == _data) |
|
|
|
|
if (nullptr == _data) { |
|
|
|
|
return -ENOMEM; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* If write size does not match, that is an error */ |
|
|
|
|
if (_meta->o_size != buflen) |
|
|
|
|
if (_meta->o_size != buflen) { |
|
|
|
|
return -EIO; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* Perform an atomic copy. */ |
|
|
|
|
lock(); |
|
|
|
@ -312,8 +323,9 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
@@ -312,8 +323,9 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
|
|
|
|
|
/* call the devnode write method with no file pointer */ |
|
|
|
|
ret = devnode->write(nullptr, (const char *)data, meta->o_size); |
|
|
|
|
|
|
|
|
|
if (ret < 0) |
|
|
|
|
if (ret < 0) { |
|
|
|
|
return ERROR; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (ret != (int)meta->o_size) { |
|
|
|
|
errno = EIO; |
|
|
|
@ -324,15 +336,15 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
@@ -324,15 +336,15 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
|
|
|
|
|
* if the write is successful, send the data over the Multi-ORB link |
|
|
|
|
*/ |
|
|
|
|
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); |
|
|
|
|
if( ch != nullptr ) |
|
|
|
|
{ |
|
|
|
|
if( ch->send_message( meta->o_name, meta->o_size, (uint8_t*)data ) != 0 ) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
if (ch != nullptr) { |
|
|
|
|
if (ch->send_message(meta->o_name, meta->o_size, (uint8_t *)data) != 0) { |
|
|
|
|
warnx("[uORB::DeviceNode::publish(%d)]: Error Sending [%s] topic data over comm_channel", |
|
|
|
|
__LINE__, meta->o_name); |
|
|
|
|
return ERROR; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return PX4_OK; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -345,8 +357,9 @@ uORB::DeviceNode::poll_state(device::file_t *filp)
@@ -345,8 +357,9 @@ uORB::DeviceNode::poll_state(device::file_t *filp)
|
|
|
|
|
/*
|
|
|
|
|
* If the topic appears updated to the subscriber, say so. |
|
|
|
|
*/ |
|
|
|
|
if (appears_updated(sd)) |
|
|
|
|
if (appears_updated(sd)) { |
|
|
|
|
return POLLIN; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
@ -360,9 +373,10 @@ uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
@@ -360,9 +373,10 @@ uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
|
|
|
|
|
/*
|
|
|
|
|
* If the topic looks updated to the subscriber, go ahead and notify them. |
|
|
|
|
*/ |
|
|
|
|
if (appears_updated(sd)) |
|
|
|
|
if (appears_updated(sd)) { |
|
|
|
|
VDev::poll_notify_one(fds, events); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool |
|
|
|
|
uORB::DeviceNode::appears_updated(SubscriberData *sd) |
|
|
|
@ -407,6 +421,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
@@ -407,6 +421,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
|
|
|
|
|
// FIXME - the calls to hrt_called and hrt_call_after seem not to work in the
|
|
|
|
|
// POSIX build
|
|
|
|
|
#ifndef __PX4_POSIX |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* If the interval timer is still running, the topic should not |
|
|
|
|
* appear updated, even though at this point we know that it has. |
|
|
|
@ -414,8 +429,9 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
@@ -414,8 +429,9 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd)
|
|
|
|
|
* must have collected the update we reported, otherwise |
|
|
|
|
* update_reported would still be true. |
|
|
|
|
*/ |
|
|
|
|
if (!hrt_called(&sd->update_call)) |
|
|
|
|
if (!hrt_called(&sd->update_call)) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
* Make sure that we don't consider the topic to be updated again |
|
|
|
@ -466,8 +482,8 @@ void uORB::DeviceNode::add_internal_subscriber()
@@ -466,8 +482,8 @@ void uORB::DeviceNode::add_internal_subscriber()
|
|
|
|
|
{ |
|
|
|
|
_subscriber_count++; |
|
|
|
|
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); |
|
|
|
|
if( ch != nullptr && _subscriber_count > 0 ) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
if (ch != nullptr && _subscriber_count > 0) { |
|
|
|
|
ch->add_subscription(_meta->o_name, 1); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -478,8 +494,8 @@ void uORB::DeviceNode::remove_internal_subscriber()
@@ -478,8 +494,8 @@ void uORB::DeviceNode::remove_internal_subscriber()
|
|
|
|
|
{ |
|
|
|
|
_subscriber_count--; |
|
|
|
|
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); |
|
|
|
|
if( ch != nullptr && _subscriber_count == 0 ) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
if (ch != nullptr && _subscriber_count == 0) { |
|
|
|
|
ch->remove_subscription(_meta->o_name); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -492,8 +508,8 @@ int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz )
@@ -492,8 +508,8 @@ int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz )
|
|
|
|
|
// the remote entity.
|
|
|
|
|
// send the data to the remote entity.
|
|
|
|
|
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); |
|
|
|
|
if( _data != nullptr && ch != nullptr ) // _data will not be null if there is a publisher.
|
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
|
|
|
|
|
ch->send_message(_meta->o_name, _meta->o_size, _data); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -512,8 +528,8 @@ int16_t uORB::DeviceNode::process_remove_subscription()
@@ -512,8 +528,8 @@ int16_t uORB::DeviceNode::process_remove_subscription()
|
|
|
|
|
int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data) |
|
|
|
|
{ |
|
|
|
|
int16_t ret = -1; |
|
|
|
|
if( length != (int32_t)(_meta->o_size) ) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
if (length != (int32_t)(_meta->o_size)) { |
|
|
|
|
warnx("[uORB::DeviceNode::process_received_message(%d)]Error:[%s] Received DataLength[%d] != ExpectedLen[%d]", |
|
|
|
|
__LINE__, _meta->o_name, (int)length, (int)_meta->o_size); |
|
|
|
|
return ERROR; |
|
|
|
@ -522,8 +538,9 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data
@@ -522,8 +538,9 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t* data
|
|
|
|
|
/* call the devnode write method with no file pointer */ |
|
|
|
|
ret = write(nullptr, (const char *)data, _meta->o_size); |
|
|
|
|
|
|
|
|
|
if (ret < 0) |
|
|
|
|
if (ret < 0) { |
|
|
|
|
return ERROR; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (ret != (int)_meta->o_size) { |
|
|
|
|
errno = EIO; |
|
|
|
@ -582,6 +599,7 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -582,6 +599,7 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
|
|
|
|
/* try for topic groups */ |
|
|
|
|
const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; |
|
|
|
|
unsigned group_tries = 0; |
|
|
|
|
|
|
|
|
|
do { |
|
|
|
|
/* if path is modifyable change try index */ |
|
|
|
|
if (adv->instance != nullptr) { |
|
|
|
@ -624,9 +642,8 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
@@ -624,9 +642,8 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
|
|
|
|
delete node; |
|
|
|
|
free((void *)objname); |
|
|
|
|
free((void *)devpath); |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
// add to the node map;.
|
|
|
|
|
_node_map[std::string(nodepath)] = node; |
|
|
|
|
} |
|
|
|
@ -656,9 +673,10 @@ uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath )
@@ -656,9 +673,10 @@ uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath )
|
|
|
|
|
{ |
|
|
|
|
uORB::DeviceNode *rc = nullptr; |
|
|
|
|
std::string np(nodepath); |
|
|
|
|
if( _node_map.find( np ) != _node_map.end() ) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
if (_node_map.find(np) != _node_map.end()) { |
|
|
|
|
rc = _node_map[np]; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return rc; |
|
|
|
|
} |
|
|
|
|