AMiRo-OS

/*

µRtWare is a lightweight publish/subscribe middleware for real-time

applications. It was developed as part of the software habitat for the

Autonomous Mini Robot [1] (AMiRo) but can be used for other purposes as well.

Copyright (C) 2018..2020  Thomas Schöpping et al.

This program is free software: you can redistribute it and/or modify

it under the terms of the GNU General Public License as published by

the Free Software Foundation, either version 3 of the License, or

(at your option) any later version.

This program is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

GNU General Public License for more details.

You should have received a copy of the GNU General Public License

along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <urtware.h>

#include <stdio.h>

/******************************************************************************/

/* LOCAL DEFINITIONS                                                          */

/******************************************************************************/

/******************************************************************************/

/* EXPORTED VARIABLES                                                         */

/******************************************************************************/

/******************************************************************************/

/* LOCAL TYPES                                                                */

/******************************************************************************/

/******************************************************************************/

/* LOCAL VARIABLES                                                            */

/******************************************************************************/

/******************************************************************************/

/* LOCAL FUNCTIONS                                                            */

/******************************************************************************/

urt_message_t* urtFindOldestMessage(urt_message_t* oldestMessage)

{

  while (oldestMessage->next->originTime < oldestMessage->originTime) {

    oldestMessage = oldestMessage->next;

  }

  return oldestMessage;

}

urt_message_t* urtFindLatestMessage(urt_message_t* latestMessage)

{

  urt_message_t* lastMessage = latestMessage;

  while (lastMessage->next->originTime < lastMessage->originTime) {

    lastMessage = lastMessage->next;

#if (URT_CFG_PUBSUB_PROFILING == true)

    subscriber->base.lastMessage->numConsumersLeft--;

    subscriber->base->numMessagesReceived++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  }

  return latestMessage;

}

void urtContributeMessages(urt_message_t* messages, urt_topic_t* topic)

{

  urt_message_t* lastMessageContribute = messages;

  while (lastMessageContribute->next) {

    lastMessageContribute = lastMessageContribute->next;

  }

  lastMessageContribute->next = topic->latestMessage->next;

  topic->latestMessage->next = messages;

}

/******************************************************************************/

/* EXPORTED FUNCTIONS                                                         */

/******************************************************************************/

/**

 * @brief   Initialize the nrt Subscriber.

 *

 * @param[in] subscriber  The NRT subscriber to initialize. Must not be NULL.

 */

void urtNrtSubscriberInit (urt_nrtsubscriber_t* subscriber)

{

  urtDebugAssert(subscriber);

  subscriber->base.topic = NULL;

  urtEventListenerInit(subscriber->base.evtListener);

  subscriber->base.lastMessage = NULL;

  subscriber->base.lastMessageTime = 0;

#if (URT_CFG_PUBSUB_PROFILING == true)

    subscriber->minLatency = URT_DELAY_INFINITE;

    subscriber->maxLatency = URT_DELAY_IMMEDIATE;

#endif /* URT_CFG_PUBSUB_PROFILING */

  return;

}

/**

 * @brief  Subscribes the subscriber to a topic.

 *

 * @param[in] subscriber  The NRT subscriber which shall subscribe to a topic. Must not be NULL.

 * @param[in] topic  The topic to subscribe to. Must not be NULL.

 * @param[in] messages  NULL terminated list of messages to contribute to the topic.

 *                      Messages must not be associated to another topic.

 *                      Once a message has been contributed, it cannot be removed later.

 *                      May be NULL(no messages to contribute).

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_SUBSCRIBE_TOPICSET if the subscriber is already associated to a topic.

 */

urt_status_t urtNrtSubscriberSubscribe (urt_nrtsubscriber_t* subscriber, urt_topic_t* topic, urt_message_t* messages)

{

  urtDebugAssert(subscriber);

  urtDebugAssert(topic);

  if (!subscriber->base.topic) {

    return URT_STATUS_SUBSCRIBE_TOPICSET;

  }

  subscriber->base.topic = topic;

  urtMutexLock(&topic->lock);

  if (messages) {

    urtContributeMessages(messages, topic);

  }

  subscriber->base.lastMessage = topic->latestMessage;

  subscriber->base.lastMessageTime = topic->latestMessage->originTime;

  urtEventRegister(&topic->evtSource, &subscriber->base.evtListener, subscriber->base.evtListener.events, subscriber->base.evtListener.flags);

#if (URT_CFG_PUBSUB_PROFILING == true)

    topic->numHrtSubscribers--;

#endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(&topic->lock);

  return URT_STATUS_OK;

}

/**

 * @brief  Fetches the next message.

 *

 * @param[in] subscriber  The NRT subscriber that shall fetch the message. Must not be NULL.

 * @param[in] payload  Pointer where to copy the payload to. May be NULL for messages without payload.

 * @param[in] bytes  Payload size in bytes.

 * @param[in] latency  The latency can be returned by reference. May be NULL.

 *

 * @return  Returns URT_STATUS_OK on success.

 *          Returns URT_STATUS_FETCH_NOTOPIC if the subscriber is not associated to a topic.

 *          Retruns URT_STATUS_FETCH_NOMESSAGE if there is no new message to fetch.

 */

urt_status_t urtNrtSubscriberFetchNextMessage (urt_nrtsubscriber_t* subscriber, void* const payload, size_t bytes, urt_delay_t* latency)

{   

  urtDebugAssert(subscriber);

  if (!subscriber->base.topic) {

    return URT_STATUS_FETCH_NOTOPIC;

  }

  urtMutexLock(&subscriber->base.topic->lock);

  urt_message_t* oldestMessage = subscriber->base.lastMessage;

  if(oldestMessage->originTime == subscriber->base.lastMessageTime) {

    if (subscriber->base.lastMessage->next->originTime > subscriber->base.lastMessageTime) {

      urtMutexUnlock(&subscriber->base.topic->lock);

      return URT_STATUS_FETCH_NOMESSAGE;

    }

    oldestMessage = oldestMessage->next;

  }

  else {

    oldestMessage = urtFindOldestMessage(oldestMessage->next);

  }

  subscriber->base.lastMessage = oldestMessage;

  subscriber->base.lastMessageTime = oldestMessage->originTime;

  memcpy(oldestMessage->payload, payload, bytes);

  if (URT_CFG_PUBSUB_PROFILING == true || latency) {

    uint64_t calculatedLatency = urtTimeNow() - oldestMessage->originTime;

#if(URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.sumLatencies += calculatedLatency;

  if (calculatedLatency < subscriber->minLatency) {

    subscriber->minLatency = calculatedLatency;

  }

  else if (calculatedLatency > subscriber->maxLatency) {

    subscriber->maxLatency = calculatedLatency;

  }

#endif /* URT_CFG_PUBSUB_PROFILING */

    if (latency) {

      *latency = calculatedLatency;

    }

  }

#if (URT_CFG_PUBSUB_PROFILING == true)

  subscriber->base.lastMessage->numConsumersLeft--;

  subscriber->base->numMessagesReceived++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  urtMutexUnlock(&subscriber->base.topic->lock);

  return URT_STATUS_OK;

}