1  /*!
  2   ********************************************************************
  3   * Description: tcq.c
  4   *\brief queue handling functions for trajectory planner
  5   * These following functions implement the motion queue that
  6   * is fed by tpAddLine/tpAddCircle and consumed by tpRunCycle.
  7   * They have been fully working for a long time and a wise programmer
  8   * won't mess with them.
  9   *
 10   *\author Derived from a work by Fred Proctor & Will Shackleford
 11   *\author rewritten by Chris Radek
 12   *
 13   * License: GPL Version 2
 14   * System: Linux
 15   *
 16   * Copyright (c) 2004 All rights reserved.
 17   *
 18   ********************************************************************/
 19  
 20  #include "tcq.h"
 21  #include <stddef.h>
 22  
 23  /** Return 0 if queue is valid, -1 if not */
 24  static inline int tcqCheck(TC_QUEUE_STRUCT const * const tcq)
 25  {
 26      if ((0 == tcq) || (0 == tcq->queue))
 27      {
 28          return -1;
 29      }
 30      return 0;
 31  }
 32  
 33  /*! tcqCreate() function
 34   *
 35   * \brief Creates a new queue for TC elements.
 36   *
 37   * This function creates a new queue for TC elements.
 38   * It gets called by tpCreate()
 39   *
 40   * @param    tcq       pointer to the new TC_QUEUE_STRUCT
 41   * @param	 _size	   size of the new queue
 42   * @param	 tcSpace   holds the space allocated for the new queue, allocated in motion.c
 43   *
 44   * @return	 int	   returns success or failure
 45   */
 46  int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size, TC_STRUCT * const tcSpace)
 47  {
 48      if (!tcq || !tcSpace || _size < 1) {
 49          return -1;
 50      }
 51  	tcq->queue = tcSpace;
 52  	tcq->size = _size;
 53      tcqInit(tcq);
 54  
 55  	return 0;
 56  }
 57  
 58  /*! tcqDelete() function
 59   *
 60   * \brief Deletes a queue holding TC elements.
 61   *
 62   * This function creates deletes a queue. It doesn't free the space
 63   * only throws the pointer away.
 64   * It gets called by tpDelete()
 65   * \todo FIXME, it seems tpDelete() is gone, and this function isn't used.
 66   *
 67   * @param    tcq       pointer to the TC_QUEUE_STRUCT
 68   *
 69   * @return	 int	   returns success
 70   */
 71  int tcqDelete(TC_QUEUE_STRUCT * const tcq)
 72  {
 73      if (!tcqCheck(tcq)) {
 74          /* free(tcq->queue); */
 75          tcq->queue = 0;
 76      }
 77  
 78      return 0;
 79  }
 80  
 81  /*! tcqInit() function
 82   *
 83   * \brief Initializes a queue with TC elements.
 84   *
 85   * This function initializes a queue with TC elements.
 86   * It gets called by tpClear() and
 87   * 	  	   		  by tpRunCycle() when we are aborting
 88   *
 89   * @param    tcq       pointer to the TC_QUEUE_STRUCT
 90   *
 91   * @return	 int	   returns success or failure (if no tcq found)
 92   */
 93  int tcqInit(TC_QUEUE_STRUCT * const tcq)
 94  {
 95      if (tcqCheck(tcq)) return -1;
 96  
 97      tcq->_len = 0;
 98      tcq->start = tcq->end = 0;
 99      tcq->rend = 0;
100      tcq->_rlen = 0;
101      tcq->allFull = 0;
102  
103      return 0;
104  }
105  
106  /*! tcqPut() function
107   *
108   * \brief puts a TC element at the end of the queue
109   *
110   * This function adds a tc element at the end of the queue.
111   * It gets called by tpAddLine() and tpAddCircle()
112   *
113   * @param    tcq       pointer to the new TC_QUEUE_STRUCT
114   * @param	 tc        the new TC element to be added
115   *
116   * @return	 int	   returns success or failure
117   */
118  int tcqPut(TC_QUEUE_STRUCT * const tcq, TC_STRUCT const * const tc)
119  {
120      /* check for initialized */
121      if (tcqCheck(tcq)) return -1;
122  
123      /* check for allFull, so we don't overflow the queue */
124      if (tcq->allFull) {
125  	    return -1;
126      }
127  
128      /* add it */
129      tcq->queue[tcq->end] = *tc;
130      tcq->_len++;
131  
132      /* update end ptr, modulo size of queue */
133      tcq->end = (tcq->end + 1) % tcq->size;
134  
135      /* set allFull flag if we're really full */
136      if (tcq->end == tcq->start) {
137  	tcq->allFull = 1;
138      }
139  
140      return 0;
141  }
142  
143  
144  /*! tcqPopBack() function
145   *
146   * \brief removes the newest TC element (converse of tcqRemove)
147   *
148   * @param    tcq       pointer to the TC_QUEUE_STRUCT
149   *
150   * @return	 int	   returns success or failure
151   */
152  int tcqPopBack(TC_QUEUE_STRUCT * const tcq)
153  {
154      /* check for initialized */
155      if (tcqCheck(tcq)) return -1;
156  
157      /* Too short to pop! */
158      if (tcq->_len < 1) {
159          return -1;
160      }
161  
162      int n = tcq->end - 1 + tcq->size;
163      tcq->end = n % tcq->size;
164      tcq->_len--;
165  
166      return 0;
167  }
168  
169  #define TCQ_REVERSE_MARGIN 200
170  
171  int tcqPop(TC_QUEUE_STRUCT * const tcq)
172  {
173  
174      if (tcqCheck(tcq)) {
175          return -1;
176      }
177  
178      if (tcq->_len < 1 && !tcq->allFull) {	
179          return -1;
180      }
181  
182      /* update start ptr and reset allFull flag and len */
183      tcq->start = (tcq->start + 1) % tcq->size;
184      tcq->allFull = 0;
185      tcq->_len--;
186  
187      if (tcq->_rlen < TCQ_REVERSE_MARGIN) {
188          //If we're not overwriting the history yet, then we have another segment added to the reverse history
189          tcq->_rlen++;
190      } else {
191          //If we're run out of spare reverse history, then advance rend
192          tcq->rend = (tcq->rend + 1) % tcq->size;
193      }
194  
195      return 0;
196  }
197  
198  /*! tcqRemove() function
199   *
200   * \brief removes n items from the queue
201   *
202   * This function removes the first n items from the queue,
203   * after checking that they can be removed
204   * (queue initialized, queue not empty, enough elements in it)
205   * Function gets called by tpRunCycle() with n=1
206   * \todo FIXME: Optimize the code to remove only 1 element, might speed it up
207   *
208   * @param    tcq       pointer to the new TC_QUEUE_STRUCT
209   * @param	 n         the number of TC elements to be removed
210   *
211   * @return	 int	   returns success or failure
212   */
213  int tcqRemove(TC_QUEUE_STRUCT * const tcq, int n)
214  {
215  
216      if (n <= 0) {
217  	    return 0;		/* okay to remove 0 or fewer */
218      }
219  
220      if (tcqCheck(tcq) || ((tcq->start == tcq->end) && !tcq->allFull) ||
221              (n > tcq->_len)) {	/* too many requested */
222  	    return -1;
223      }
224  
225      /* update start ptr and reset allFull flag and len */
226      tcq->start = (tcq->start + n) % tcq->size;
227      tcq->allFull = 0;
228      tcq->_len -= n;
229  
230      return 0;
231  }
232  
233  
234  /**
235   * Step backward into the reverse history.
236   */
237  int tcqBackStep(TC_QUEUE_STRUCT * const tcq)
238  {
239  
240      if (tcqCheck(tcq)) {
241          return -1;
242      }
243  
244      // start == end means that queue is empty
245  
246      if ( tcq->start == tcq->rend) {	
247          return -1;
248      }
249      /* update start ptr and reset allFull flag and len */
250      tcq->start = (tcq->start - 1 + tcq->size) % tcq->size;
251      tcq->_len++;
252      tcq->_rlen--;
253  
254      return 0;
255  }
256  
257  /*! tcqLen() function
258   *
259   * \brief returns the number of elements in the queue
260   *
261   * Function gets called by tpSetVScale(), tpAddLine(), tpAddCircle()
262   *
263   * @param    tcq       pointer to the TC_QUEUE_STRUCT
264   *
265   * @return	 int	   returns number of elements
266   */
267  int tcqLen(TC_QUEUE_STRUCT const * const tcq)
268  {
269      if (tcqCheck(tcq)) return -1;
270  
271      return tcq->_len;
272  }
273  
274  /*! tcqItem() function
275   *
276   * \brief gets the n-th TC element in the queue, without removing it
277   *
278   * Function gets called by tpSetVScale(), tpRunCycle(), tpIsPaused()
279   *
280   * @param    tcq       pointer to the TC_QUEUE_STRUCT
281   *
282   * @return	 TC_STRUCT returns the TC elements
283   */
284  TC_STRUCT * tcqItem(TC_QUEUE_STRUCT const * const tcq, int n)
285  {
286      if (tcqCheck(tcq) || (n < 0) || (n >= tcq->_len)) return NULL;
287  
288      return &(tcq->queue[(tcq->start + n) % tcq->size]);
289  }
290  
291  /*!
292   * \def TC_QUEUE_MARGIN
293   * sets up a margin at the end of the queue, to reduce effects of race conditions
294   */
295  #define TC_QUEUE_MARGIN (TCQ_REVERSE_MARGIN+20)
296  
297  /*! tcqFull() function
298   *
299   * \brief get the full status of the queue
300   * Function returns full if the count is closer to the end of the queue than TC_QUEUE_MARGIN
301   *
302   * Function called by update_status() in control.c
303   *
304   * @param    tcq       pointer to the TC_QUEUE_STRUCT
305   *
306   * @return	 int       returns status (0==not full, 1==full)
307   */
308  int tcqFull(TC_QUEUE_STRUCT const * const tcq)
309  {
310      if (tcqCheck(tcq)) {
311  	   return 1;		/* null queue is full, for safety */
312      }
313  
314      /* call the queue full if the length is into the margin, so reduce the
315         effect of a race condition where the appending process may not see the
316         full status immediately and send another motion */
317  
318      if (tcq->size <= TC_QUEUE_MARGIN) {
319  	/* no margin available, so full means really all full */
320  	    return tcq->allFull;
321      }
322  
323      if (tcq->_len >= tcq->size - TC_QUEUE_MARGIN) {
324  	/* we're into the margin, so call it full */
325  	    return 1;
326      }
327  
328      /* we're not into the margin */
329      return 0;
330  }
331  
332  /*! tcqLast() function
333   *
334   * \brief gets the last TC element in the queue, without removing it
335   *
336   *
337   * @param    tcq       pointer to the TC_QUEUE_STRUCT
338   *
339   * @return	 TC_STRUCT returns the TC element
340   */
341  TC_STRUCT *tcqLast(TC_QUEUE_STRUCT const * const tcq)
342  {
343      if (tcqCheck(tcq)) {
344          return NULL;
345      }
346      if (tcq->_len == 0) {
347          return NULL;
348      }
349      //Fix for negative modulus error
350      int n = tcq->end-1 + tcq->size;
351      return &(tcq->queue[n % tcq->size]);
352  
353  }
354