diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 7ae8e3a1ce..93eeaa8d92 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -127,7 +127,7 @@ uORB::DeviceNode::open(struct file *filp) sd->generation = _generation; /* set priority */ - sd->priority = _priority; + sd->set_priority(_priority); filp->f_priv = (void *)sd; @@ -157,7 +157,10 @@ uORB::DeviceNode::close(struct file *filp) SubscriberData *sd = filp_to_sd(filp); if (sd != nullptr) { - hrt_cancel(&sd->update_call); + if (sd->update_interval) { + hrt_cancel(&sd->update_interval->update_call); + } + remove_internal_subscriber(); delete sd; sd = nullptr; @@ -209,13 +212,13 @@ uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen) } /* set priority */ - sd->priority = _priority; + sd->set_priority(_priority); /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. */ - sd->update_reported = false; + sd->set_update_reported(false); px4_leave_critical_section(flags); @@ -294,16 +297,43 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg) *(bool *)arg = appears_updated(sd); return OK; - case ORBIOCSETINTERVAL: - sd->update_interval = arg; - return OK; + case ORBIOCSETINTERVAL: { + int ret = PX4_OK; + lock(); + + if (arg == 0) { + if (sd->update_interval) { + delete(sd->update_interval); + sd->update_interval = nullptr; + } + + } else { + if (sd->update_interval) { + sd->update_interval->interval = arg; + + } else { + sd->update_interval = new UpdateIntervalData(); + + if (sd->update_interval) { + memset(&sd->update_interval->update_call, 0, sizeof(hrt_call)); + sd->update_interval->interval = arg; + + } else { + ret = -ENOMEM; + } + } + } + + unlock(); + return ret; + } case ORBIOCGADVERTISER: *(uintptr_t *)arg = (uintptr_t)this; return OK; case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; + *(int *)arg = sd->priority(); return OK; case ORBIOCSETQUEUESIZE: @@ -312,7 +342,13 @@ uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg) return update_queue_size(arg); case ORBIOCGETINTERVAL: - *(unsigned *)arg = sd->update_interval; + if (sd->update_interval) { + *(unsigned *)arg = sd->update_interval->interval; + + } else { + *(unsigned *)arg = 0; + } + return OK; default: @@ -443,7 +479,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) /* * Handle non-rate-limited subscribers. */ - if (sd->update_interval == 0) { + if (sd->update_interval == nullptr) { ret = true; break; } @@ -455,7 +491,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) * behaviour where checking / polling continues to report an update * until the topic is read. */ - if (sd->update_reported) { + if (sd->update_reported()) { ret = true; break; } @@ -467,7 +503,7 @@ 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_interval->update_call)) { break; } @@ -476,15 +512,15 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) * until the interval has passed once more by restarting the interval * timer and thereby re-scheduling a poll notification at that time. */ - hrt_call_after(&sd->update_call, - sd->update_interval, + hrt_call_after(&sd->update_interval->update_call, + sd->update_interval->interval, &uORB::DeviceNode::update_deferred_trampoline, (void *)this); /* * Remember that we have told the subscriber that there is data. */ - sd->update_reported = true; + sd->set_update_reported(true); ret = true; break; diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index e4eedd6003..2ad16ee3c7 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -183,12 +183,22 @@ protected: virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct UpdateIntervalData { + unsigned interval; /**< if nonzero minimum interval between updates */ struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - bool update_reported; /**< true if we have reported the update via poll/check */ - int priority; /**< priority of publisher */ + }; + struct SubscriberData { + ~SubscriberData() { if (update_interval) { delete(update_interval); } } + + unsigned generation; /**< last generation the subscriber has seen */ + int flags; /**< lowest 8 bits: priority of publisher, 9. bit: update_reported bit */ + UpdateIntervalData *update_interval; /**< if null, no update interval */ + + int priority() const { return flags & 0xff; } + void set_priority(uint8_t prio) { flags = (flags & ~0xff) | prio; } + + bool update_reported() const { return flags & (1 << 8); } + void set_update_reported(bool update_reported_flag) { flags = (flags & ~(1 << 8)) | (((int)update_reported_flag) << 8); } }; const struct orb_metadata *_meta; /**< object metadata information */ diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index ac936e45e9..a7c7d3cfe6 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -136,7 +136,7 @@ uORB::DeviceNode::open(device::file_t *filp) sd->generation = _generation; /* set priority */ - sd->priority = _priority; + sd->set_priority(_priority); filp->priv = (void *)sd; @@ -169,7 +169,10 @@ uORB::DeviceNode::close(device::file_t *filp) SubscriberData *sd = filp_to_sd(filp); if (sd != nullptr) { - hrt_cancel(&sd->update_call); + if (sd->update_interval) { + hrt_cancel(&sd->update_interval->update_call); + } + remove_internal_subscriber(); delete sd; sd = nullptr; @@ -222,13 +225,13 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen) } /* set priority */ - sd->priority = _priority; + sd->set_priority(_priority); /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. */ - sd->update_reported = false; + sd->set_update_reported(false); unlock(); @@ -306,19 +309,45 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) unlock(); return PX4_OK; - case ORBIOCSETINTERVAL: - lock(); - sd->update_interval = arg; - sd->last_update = hrt_absolute_time(); - unlock(); - return PX4_OK; + case ORBIOCSETINTERVAL: { + int ret = PX4_OK; + lock(); + + if (arg == 0) { + if (sd->update_interval) { + delete(sd->update_interval); + sd->update_interval = nullptr; + } + + } else { + if (sd->update_interval) { + sd->update_interval->interval = arg; + sd->update_interval->last_update = hrt_absolute_time(); + + } else { + sd->update_interval = new UpdateIntervalData(); + + if (sd->update_interval) { + memset(&sd->update_interval->update_call, 0, sizeof(hrt_call)); + sd->update_interval->interval = arg; + sd->update_interval->last_update = hrt_absolute_time(); + + } else { + ret = -ENOMEM; + } + } + } + + unlock(); + return ret; + } case ORBIOCGADVERTISER: *(uintptr_t *)arg = (uintptr_t)this; return PX4_OK; case ORBIOCGPRIORITY: - *(int *)arg = sd->priority; + *(int *)arg = sd->priority(); return PX4_OK; case ORBIOCSETQUEUESIZE: @@ -327,7 +356,13 @@ uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) return update_queue_size(arg); case ORBIOCGETINTERVAL: - *(unsigned *)arg = sd->update_interval; + if (sd->update_interval) { + *(unsigned *)arg = sd->update_interval->interval; + + } else { + *(unsigned *)arg = 0; + } + return OK; default: @@ -466,7 +501,7 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) /* * Handle non-rate-limited subscribers. */ - if (sd->update_interval == 0) { + if (sd->update_interval == nullptr) { ret = true; break; } @@ -478,22 +513,22 @@ uORB::DeviceNode::appears_updated(SubscriberData *sd) * behaviour where checking / polling continues to report an update * until the topic is read. */ - if (sd->update_reported) { + if (sd->update_reported()) { ret = true; break; } // If we have not yet reached the deadline, then assume that we can ignore any // newly received data. - if (sd->last_update + sd->update_interval > hrt_absolute_time()) { + if (sd->update_interval->last_update + sd->update_interval->interval > hrt_absolute_time()) { break; } /* * Remember that we have told the subscriber that there is data. */ - sd->update_reported = true; - sd->last_update = hrt_absolute_time(); + sd->set_update_reported(true); + sd->update_interval->last_update = hrt_absolute_time(); ret = true; break; diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index 25f241ff58..7fad8b5e7f 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -120,13 +120,23 @@ protected: virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); private: - struct SubscriberData { - unsigned generation; /**< last generation the subscriber has seen */ - unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct UpdateIntervalData { + unsigned interval; /**< if nonzero minimum interval between updates */ uint64_t last_update; /**< time at which the last update was provided, used when update_interval is nonzero */ struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ - bool update_reported; /**< true if we have reported the update via poll/check */ - int priority; /**< priority of publisher */ + }; + struct SubscriberData { + ~SubscriberData() { if (update_interval) { delete(update_interval); } } + + unsigned generation; /**< last generation the subscriber has seen */ + int flags; /**< lowest 8 bits: priority of publisher, 9. bit: update_reported bit */ + UpdateIntervalData *update_interval; /**< if null, no update interval */ + + int priority() const { return flags & 0xff; } + void set_priority(uint8_t prio) { flags = (flags & ~0xff) | prio; } + + bool update_reported() const { return flags & (1 << 8); } + void set_update_reported(bool update_reported_flag) { flags = (flags & ~(1 << 8)) | (((int)update_reported_flag) << 8); } }; const struct orb_metadata *_meta; /**< object metadata information */