AMiRo-OS

 * @brief   Initialize the publisher.

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

  urtDebugAssert(publisher);

  urtDebugAssert(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;

  }

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;

}