Statistics
| Branch: | Revision:

urtware / src / urt_publisher.c @ 33aa05c5

History | View | Annotate | Download (6.41 KB)

1
/*
2
µRtWare is a lightweight publish/subscribe middleware for real-time
3
applications. It was developed as part of the software habitat for the
4
Autonomous Mini Robot [1] (AMiRo) but can be used for other purposes as well.
5

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

8
This program is free software: you can redistribute it and/or modify
9
it under the terms of the GNU General Public License as published by
10
the Free Software Foundation, either version 3 of the License, or
11
(at your option) any later version.
12

13
This program is distributed in the hope that it will be useful,
14
but WITHOUT ANY WARRANTY; without even the implied warranty of
15
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16
GNU General Public License for more details.
17

18
You should have received a copy of the GNU General Public License
19
along with this program.  If not, see <http://www.gnu.org/licenses/>.
20
*/
21

    
22
#include <urtware.h>
23

    
24
/******************************************************************************/
25
/* LOCAL DEFINITIONS                                                          */
26
/******************************************************************************/
27

    
28
/******************************************************************************/
29
/* EXPORTED VARIABLES                                                         */
30
/******************************************************************************/
31

    
32
/******************************************************************************/
33
/* LOCAL TYPES                                                                */
34
/******************************************************************************/
35

    
36
/******************************************************************************/
37
/* LOCAL VARIABLES                                                            */
38
/******************************************************************************/
39

    
40
/******************************************************************************/
41
/* LOCAL FUNCTIONS                                                            */
42
/******************************************************************************/
43

    
44
/******************************************************************************/
45
/* EXPORTED FUNCTIONS                                                         */
46
/******************************************************************************/
47

    
48
/**
49
 * @brief   Initialize the publisher.
50
 *
51
 * @param[in] publisher  The publisher to initialize. Must not be NULL.
52
 * @param[in] topic  The topic, this publisher is associated to. Must not be NULL.
53
 * @param[in] message  NULL terminated list of messages to contribute to the topic.
54
 *                     Messages must not be associated to another topic.
55
 *                     Once a message has been contributed, it cannot be removed later.
56
 *                     May be NULL (no messages to contribute).
57
 */
58
void urtPublisherInit(urt_publisher_t* publisher, urt_topic_t* topic, urt_message_t* messages)
59
{
60
  urtDebugAssert(publisher);
61
  urtDebugAssert(topic);
62

    
63
  urt_publisher_t.topic = topic;
64
#if (URT_CFG_PUBSUB_PROFILING == true)
65
  urt_publisher_t.publishAttempts = 0;
66
  urt_publisher_t.publishFails = 0;
67
#endif /* URT_CFG_PUBSUB_PROFILING */
68
  if (messages != NULL) {
69
    urtMutexLock(topic->lock);
70
    urt_message_t* lastMessage = messages;
71
    while (lastMessage->next != NULL) {
72
      lastMessage = lastMessage->next;
73
    }
74
    //TODO: setnextpointer of last message to contribute to the message after the topic's latest message
75
    topic->latestMessage->next = messages;
76
  }
77

    
78
  return;
79
}
80

    
81

    
82
/**
83
 * @brief   Publish data.
84
 *
85
 * @param[in] publisher  Pointer to the publisher to use. Must not be NULL.
86
 * @param[in] payload  Pointer to the data to be published. May be NULL for messages without payload.
87
 * @param[in] bytes  Size of the payload in bytes.
88
 * @param[in] t  Timestamp to be set for the message.
89
 * @param[in] timeout  Timeout delay in case the message cannot be published.
90
 *
91
 * @return  Returns URT_STATUS_OK on success. Returns URT_STATUS_PUBLISH_TIMEOUT on timeout.
92
 */
93
urt_status_t urtPublisherPublish(urt_publisher_t* publisher, void* payload, size_t bytes, urt_osTime_t t, urt_delay_t timeout)
94
{
95
  urtMutexLock(publisher->topic->lock);
96
  urt_message_t* message = publisher->topic->next->latestMessage;
97
  urt_osTime_t messageTime =  message->originTime;
98
#if (URT_CFG_PUBSUB_PROFILING == true)
99
  publisher->publishAttempts++;
100
#endif /* URT_CFG_PUBSUB_PROFILING */
101

    
102
  while (message->numHrtConsumersLeft > 0) {
103
    urt_osCondvarWaitStatus_t timeout = urtCondvarWait(publisher->topic->hrtReleased, publisher->topic->lock, URT_DELAY_INFINITE);
104
    if (timeout == URT_CONDVAR_WAITSTATUS_TIMEOUT) {
105
#if (URT_CFG_PUBSUB_PROFILING == true)
106
      publisher->publishFails++;
107
#endif /* URT_CFG_PUBSUB_PROFILING */
108
      urtMutexUnlock(publisher->topic->lock);
109
      return URT_STATUS_PUBLISH_TIMEOUT;
110
    }
111

    
112
    if (messageTime != message->originTime) {
113
      messageTime = message->originTime;
114
      message = message->next;
115
      while (message->originTime < messageTime) {
116
        message = message->next;
117
      }
118
    }
119
  }
120

    
121
#if (URT_CFG_PUBSUB_PROFILING == true)
122
  if (message->numHrtConsumersLeft > 0) { //TODO: never true because of while?
123
    publisher->topic->numMessagesDiscarded++;
124
  }
125
#endif /* URT_CFG_PUBSUB_PROFILING */
126

    
127
  publisher->topic->latestMessage = message; //TODO: Iterate topic's pointer to this message?
128
  memcpy(message->payload, payload, bytes); //TODO: copy payload to message?
129
  //TODO: Set origin time of message
130
  publisher->topic->numHrtSubscribers = message->numHrtConsumersLeft;
131
#if (URT_CFG_PUBSUB_PROFILING == true)
132
  publisher->topic->numSubscribers = message->numHrtConsumersLeft; //TODO: Set number of comsumers?
133
#endif /* URT_CFG_PUBSUB_PROFILING */
134

    
135
#if (URT_CFG_PUBSUB_QOS_RATECHECKS == true)
136
  publisher->topic->qosRateTimer =  NULL; //TODO: Set QoS rate timer wrt. most critical HRT Subscriber
137
#endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */
138

    
139
#if (URT_CFG_PUBSUB_QOS_DEADLINECHECKS == true)
140
  urt_hrtsubscriber_t* hrtSubscriber = publisher->topic->hrtSubscribers;
141
  while (hrtSubscriber != NULL) {
142
    if (!urtTimerIsArmed(hrtSubscriber->qosDeadlineTimer)) {
143
      *hrtSubscriber->qosDeadlineTimer = message->originTime;
144
    }
145
    hrtSubscriber = hrtSubscriber->next;
146
  }
147
#endif /* URT_CFG_PUBSUB_QOS_DEADLINECHECKS */
148

    
149
#if (URT_CFG_PUBSUB_PROFILING == true)
150
  publisher->topic->numMessagesPublished++;
151
#endif /* URT_CFG_PUBSUB_PROFILING */
152

    
153
  //TODO: Fire event
154
  urtMutexUnlock(publisher->topic->lock);
155
  return URT_STATUS_OK;
156
}