Statistics
| Branch: | Revision:

urtware / src / urt_publisher.c @ fb72e91b

History | View | Annotate | Download (6.44 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
  publisher->topic = topic;
64
#if (URT_CFG_PUBSUB_PROFILING == true)
65
  publisher->publishAttempts = 0;
66
  publisher->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
    lastMessage->next = topic->latestMessage->next;
75
    topic->latestMessage->next = messages;
76
    urtMutexUnlock(&topic->lock);
77
  }
78

    
79
  return;
80
}
81

    
82

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

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

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

    
122
#if (URT_CFG_PUBSUB_PROFILING == true)
123
  if (latestMessage->numConsumersLeft > 0) {
124
    publisher->topic->numMessagesDiscarded++;
125
  }
126
#endif /* URT_CFG_PUBSUB_PROFILING */
127

    
128
  publisher->topic->latestMessage = latestMessage;
129
  memcpy(latestMessage->payload, payload, bytes);
130
  latestMessage->originTime = t;
131
  publisher->topic->numHrtSubscribers = latestMessage->numHrtConsumersLeft;
132
#if (URT_CFG_PUBSUB_PROFILING == true)
133
  publisher->topic->numSubscribers = latestMessage->numConsumersLeft;
134
#endif /* URT_CFG_PUBSUB_PROFILING */
135

    
136
#if (URT_CFG_PUBSUB_QOS_RATECHECKS == true)
137
  publisher->topic->qosRateTimer =  publisher->topic->hrtSubscribers;
138
  //TODO: Set QoS rate timer (@topic) wrt. most critical HRT Subscriber (here different types?)
139
#endif /* URT_CFG_PUBSUB_QOS_RATECHECKS */
140

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

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

    
155
  urtEventSourceBroadcast(&publisher->topic->evtSource, URT_EVENTFLAG_PROCEED);
156
  urtMutexUnlock(&publisher->topic->lock);
157
  return URT_STATUS_OK;
158
}