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>

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

/* LOCAL DEFINITIONS                                                          */

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

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

/* EXPORTED VARIABLES                                                         */

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

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

/* LOCAL TYPES                                                                */

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

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

/* LOCAL VARIABLES                                                            */

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

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

/* LOCAL FUNCTIONS                                                            */

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

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

/* EXPORTED FUNCTIONS                                                         */

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

/**

 * @brief   Initialize the publisher.

 *

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

 * @param[in] topic  The topic, this publisher is associated to. Must not be NULL.

 * @param[in] message  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).

 */

void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_message_t* messages)

{

  urtDebugAssert(publisher);

  urtDebugAssert(topic);

  urt_publisher_t.topic = topic;

#if (URT_CFG_PUBSUB_PROFILING == true)

  urt_publisher_t.publishAttempts = 0;

  urt_publisher_t.publishFails = 0;

#endif /* URT_CFG_PUBSUB_PROFILING */

  if (messages != NULL) {

    urtMutexLock(topic->lock);

    urt_message_t* lastMessage = messages;

    while (lastMessage->next != NULL) {

      lastMessage = lastMessage->next;

    }

    //TODO: setnextpointer of last message to contribute to the message after the topic's latest message

    topic->latestMessage->next = messages;

  }

  return;

}

/**

 * @brief   Publish data.

 *

 * @param[in] publisher  Pointer to the publisher to use. Must not be NULL.

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

 * @param[in] bytes  Size of the payload in bytes.

 * @param[in] t  Timestamp to be set for the message.

 * @param[in] timeout  Timeout delay in case the message cannot be published.

 *

 * @return  Returns URT_STATUS_OK on success. Returns URT_STATUS_PUBLISH_TIMEOUT on timeout.

 */

urt_status_t urtPublisherPublish(urt_publisher_t* publisher, void* payload, size_t bytes, urt_osTime_t t, urt_delay_t timeout)

{

  urtMutexLock(publisher->topic->lock);

  urt_message_t* message = publisher->topic->next->latestMessage;

  urt_osTime_t messageTime =  message->originTime;

#if (URT_CFG_PUBSUB_PROFILING == true)

  publisher->publishAttempts++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  while (message->numHrtConsumersLeft > 0) {

    urt_osCondvarWaitStatus_t timeout = urtCondvarWait(publisher->topic->hrtReleased, publisher->topic->lock, URT_DELAY_INFINITE);

    if (timeout == URT_CONDVAR_WAITSTATUS_TIMEOUT) {

#if (URT_CFG_PUBSUB_PROFILING == true)

      publisher->publishFails++;

#endif /* URT_CFG_PUBSUB_PROFILING */

      urtMutexUnlock(publisher->topic->lock);

      return URT_STATUS_PUBLISH_TIMEOUT;

    }

    if (messageTime != message->originTime) {

      messageTime = message->originTime;

      message = message->next;

      while (message->originTime < messageTime) {

        message = message->next;

      }

    }

  }

#if (URT_CFG_PUBSUB_PROFILING == true)

  if (message->numHrtConsumersLeft > 0) { //TODO: never true because of while?

    publisher->topic->numMessagesDiscarded++;

  }

#endif /* URT_CFG_PUBSUB_PROFILING */

  publisher->topic->latestMessage = message; //TODO: Iterate topic's pointer to this message?

  memcpy(message->payload, payload, bytes); //TODO: copy payload to message?

  //TODO: Set origin time of message

  publisher->topic->numHrtSubscribers = message->numHrtConsumersLeft;

#if (URT_CFG_PUBSUB_PROFILING == true)

  publisher->topic->numSubscribers = message->numHrtConsumersLeft; //TODO: Set number of comsumers?

#endif /* URT_CFG_PUBSUB_PROFILING */

#if (URT_CFG_PUBSUB_QOS_RATECHECKS == true)

  publisher->topic->qosRateTimer =  NULL; //TODO: Set QoS rate timer wrt. most critical HRT Subscriber

#endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */

#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)

  urt_hrtsubscriber_t* hrtSubscriber = publisher->topic->hrtSubscribers;

  while (hrtSubscriber != NULL) {

    if (!urtTimerIsArmed(hrtSubscriber->qosDeadlineTimer)) {

      *hrtSubscriber->qosDeadlineTimer = message->originTime;

    }

    hrtSubscriber = hrtSubscriber->next;

  }

#endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */

#if (URT_CFG_PUBSUB_PROFILING == true)

  publisher->topic->numMessagesPublished++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  //TODO: Fire event

  urtMutexUnlock(publisher->topic->lock);

  return URT_STATUS_OK;

}