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);

  publisher->topic = topic;

#if (URT_CFG_PUBSUB_PROFILING == true)

  publisher->publishAttempts = 0;

  publisher->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;

    }

    lastMessage->next = topic->latestMessage->next;

    topic->latestMessage->next = messages;

    urtMutexUnlock(&topic->lock);

  }

  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* latestMessage = publisher->topic->next->latestMessage;

  urt_osTime_t messageTime =  latestMessage->originTime;

#if (URT_CFG_PUBSUB_PROFILING == true)

  publisher->publishAttempts++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  while (latestMessage->numHrtConsumersLeft > 0) {

    //TODO: 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 != latestMessage->originTime) {

      messageTime = latestMessage->originTime;

      latestMessage = latestMessage->next;

      while (latestMessage->originTime < messageTime) {

        latestMessage = latestMessage->next;

      }

    }

  }

#if (URT_CFG_PUBSUB_PROFILING == true)

  if (latestMessage->numConsumersLeft > 0) {

    publisher->topic->numMessagesDiscarded++;

  }

#endif /* URT_CFG_PUBSUB_PROFILING */

  publisher->topic->latestMessage = latestMessage;

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

  latestMessage->originTime = t;

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

#if (URT_CFG_PUBSUB_PROFILING == true)

  publisher->topic->numSubscribers = latestMessage->numConsumersLeft;

#endif /* URT_CFG_PUBSUB_PROFILING */

#if (URT_CFG_PUBSUB_QOS_RATECHECKS == true)

  publisher->topic->qosRateTimer =  publisher->topic->hrtSubscribers;

  //TODO: Set QoS rate timer (@topic) wrt. most critical HRT Subscriber (here different types?)

#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 = latestMessage->originTime;

    }

    hrtSubscriber = hrtSubscriber->next;

  }

#endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */

#if (URT_CFG_PUBSUB_PROFILING == true)

  publisher->topic->numMessagesPublished++;

#endif /* URT_CFG_PUBSUB_PROFILING */

  urtEventSourceBroadcast(&publisher->topic->evtSource, URT_EVENTFLAG_PROCEED);

  urtMutexUnlock(&publisher->topic->lock);

  return URT_STATUS_OK;

}