bool
MavlinkOrbSubscription::update(uint64_t *time, void *data)
{
// TODO this is NOT atomic operation, we can get data newer than time
// if topic was published between orb_stat and orb_copy calls.
if (!is_published()) {
return false;
}
uint64_t time_topic;
if (orb_stat(_fd, &time_topic)) {
/* error getting last topic publication time */
time_topic = 0;
}
if (time_topic == 0 || (time_topic != *time)) {
if (orb_copy(_topic, _fd, data) == PX4_OK) {
/* data copied successfully */
*time = time_topic;
return true;
}
}
return false;
}
sequence_t wait_until_published(
sequence_t sequence,
sequence_t lastKnownPublished,
const std::chrono::time_point<Clock, Duration>& timeoutTime) const
{
assert(difference(sequence, lastKnownPublished) > 0);
for (sequence_t seq = lastKnownPublished + 1;
difference(seq, sequence) <= 0;
++seq)
{
if (!is_published(seq))
{
const std::atomic<sequence_t>* const sequences[1] =
{ &m_published[seq & m_indexMask] };
sequence_t result =
m_waitStrategy.wait_until_published(seq, 1, sequences);
if (difference(result, seq) < 0)
{
// Timeout. seq is the first non-published sequence
return seq - 1;
}
}
}
return last_published_after(sequence);
}
/// \brief
/// Return the highest sequence number published after the specified
/// last-known published sequence.
///
/// \param lastKnownPublished
/// This sequence number is assumed to have already been published.
/// The initial value passed in here on first call should be sequence_t(-1).
///
/// \return
/// The last-published sequence number.
/// This will be equal to \p lastKnownPublished if no additional sequences
/// have been published.
sequence_t last_published_after(sequence_t lastKnownPublished) const
{
sequence_t seq = lastKnownPublished + 1;
while (is_published(seq))
{
lastKnownPublished = seq;
++seq;
}
return lastKnownPublished;
}
bool
MavlinkOrbSubscription::update(void *data)
{
if (!is_published()) {
return false;
}
if (orb_copy(_topic, _fd, data) != PX4_OK) {
return false;
}
return true;
}
bool
MavlinkOrbSubscription::update_if_changed(void *data)
{
if (!is_published()) {
return false;
}
bool updated;
if (orb_check(_fd, &updated) || !updated) {
return false;
}
return update(data);
}
bool
MavlinkOrbSubscription::update(void *data)
{
if (!is_published()) {
return false;
}
if (orb_copy(_topic, _fd, data)) {
if (data != nullptr) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
}
return false;
}
return true;
}
/// \brief
/// Block the caller until the specified sequence number has been
/// published.
///
/// This method is called by reader threads waiting to consume
/// items written to the ring buffer.
///
/// \param sequence
/// The sequence number to wait for.
///
/// \param lastKnownPublished
/// This sequence number is assumed to have already been published.
/// The initial value passed in here on first call should be sequence_t(-1).
///
/// \return
/// Returns the sequence number of the latest available published
/// sequence, guaranteed to be equal to or later than the specified
/// \p sequence parameter.
///
/// \throw std::exception
/// Throws any exception thrown by \c WaitStrategy::wait_until_published().
sequence_t wait_until_published(
sequence_t sequence,
sequence_t lastKnownPublished) const
{
assert(difference(sequence, lastKnownPublished) > 0);
for (sequence_t seq = lastKnownPublished + 1;
difference(seq, sequence) <= 0;
++seq)
{
if (!is_published(seq))
{
const std::atomic<sequence_t>* const sequences[1] =
{ &m_published[seq & m_indexMask] };
m_waitStrategy.wait_until_published(seq, 1, sequences);
}
}
return last_published_after(sequence);
}
bool
MavlinkOrbSubscription::update_if_changed(void *data)
{
bool prevpub = _published;
if (!is_published()) {
return false;
}
bool updated;
if (orb_check(_fd, &updated)) {
return false;
}
// If we didn't update and this topic did not change
// its publication status then nothing really changed
if (!updated && prevpub == _published) {
return false;
}
return update(data);
}