Trinity Devboard PCB V1.0 Firmware. FreeRTOS is setup and the MCU reads IMU data over SPI fand Magnetometer data over I2C, each with a seperate task. Sensordata is then run though MadgwickAHRS and send over USB as serial packet data to use in trinity visualizer. Bare minimum functionality works and is replicated from the first prototype.
This commit is contained in:
@@ -0,0 +1,383 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file stm_queue.c
|
||||
* @author MCD Application Team
|
||||
* @brief Queue management
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* Copyright (c) 2018-2021 STMicroelectronics.
|
||||
* All rights reserved.
|
||||
*
|
||||
* This software is licensed under terms that can be found in the LICENSE file
|
||||
* in the root directory of this software component.
|
||||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "utilities_common.h"
|
||||
|
||||
#include "stm_queue.h"
|
||||
|
||||
/* Private define ------------------------------------------------------------*/
|
||||
/* Private typedef -------------------------------------------------------------*/
|
||||
/* Private macro -------------------------------------------------------------*/
|
||||
#define MOD(X,Y) (((X) >= (Y)) ? ((X)-(Y)) : (X))
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
/* Global variables ----------------------------------------------------------*/
|
||||
/* Extern variables ----------------------------------------------------------*/
|
||||
/* Private function prototypes -----------------------------------------------*/
|
||||
/* Private functions ---------------------------------------------------------*/
|
||||
/* Public functions ----------------------------------------------------------*/
|
||||
|
||||
/**
|
||||
* @brief Initilaiilze queue structure .
|
||||
* @note This function is used to initialize the global queue structure.
|
||||
* @param q: pointer on queue structure to be initialised
|
||||
* @param queueBuffer: pointer on Queue Buffer
|
||||
* @param queueSize: Size of Queue Buffer
|
||||
* @param elementSize: Size of an element in the queue. if =0, the queue will manage variable sizze elements
|
||||
* @retval always 0
|
||||
*/
|
||||
int CircularQueue_Init(queue_t *q, uint8_t* queueBuffer, uint32_t queueSize, uint16_t elementSize, uint8_t optionFlags)
|
||||
{
|
||||
q->qBuff = queueBuffer;
|
||||
q->first = 0;
|
||||
q->last = 0; /* queueSize-1; */
|
||||
q->byteCount = 0;
|
||||
q->elementCount = 0;
|
||||
q->queueMaxSize = queueSize;
|
||||
q->elementSize = elementSize;
|
||||
q->optionFlags = optionFlags;
|
||||
|
||||
if ((optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG) && q-> elementSize)
|
||||
{
|
||||
/* can not deal with splitting at the end of buffer with fixed size element */
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Add element to the queue .
|
||||
* @note This function is used to add one or more element(s) to the Circular Queue .
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param X; pointer on element(s) to be added
|
||||
* @param elementSize: Size of element to be added to the queue. Only used if the queue manage variable size elements
|
||||
* @param nbElements: number of elements in the in buffer pointed by x
|
||||
* @retval pointer on last element just added to the queue, NULL if the element to be added do not fit in the queue (too big)
|
||||
*/
|
||||
uint8_t* CircularQueue_Add(queue_t *q, uint8_t* x, uint16_t elementSize, uint32_t nbElements)
|
||||
{
|
||||
|
||||
uint8_t* ptr = NULL; /* fct return ptr to the element freshly added, if no room fct return NULL */
|
||||
uint16_t curElementSize = 0; /* the size of the element currently stored at q->last position */
|
||||
uint8_t elemSizeStorageRoom = 0 ; /* Indicate the header (which contain only size) of element in case of varaibale size element (q->elementsize == 0) */
|
||||
uint32_t curBuffPosition; /* the current position in the queue buffer */
|
||||
uint32_t i; /* loop counter */
|
||||
uint32_t NbBytesToCopy = 0, NbCopiedBytes = 0 ; /* Indicators for copying bytes in queue */
|
||||
uint32_t eob_free_size; /* Eof End of Quque Buffer Free Size */
|
||||
uint8_t wrap_will_occur = 0; /* indicate if a wrap around will occurs */
|
||||
uint8_t wrapped_element_eob_size; /* In case of Wrap around, indicate size of parta of element that fit at thened of the queuue buffer */
|
||||
uint16_t overhead = 0; /* In case of CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG or CIRCULAR_QUEUE_NO_WRAP_FLAG options,
|
||||
indcate the size overhead that will be generated by adding the element with wrap management (split or no wrap ) */
|
||||
|
||||
|
||||
elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
|
||||
/* retrieve the size of last element sored: the value stored at the beginning of the queue element if element size is variable otherwise take it from fixed element Size member */
|
||||
if (q->byteCount)
|
||||
{
|
||||
curElementSize = (q->elementSize == 0) ? q->qBuff[q->last] + ((q->qBuff[MOD((q->last+1), q->queueMaxSize)])<<8) + 2 : q->elementSize;
|
||||
}
|
||||
/* if queue element have fixed size , reset the elementSize arg with fixed element size value */
|
||||
if (q->elementSize > 0)
|
||||
{
|
||||
elementSize = q->elementSize;
|
||||
}
|
||||
|
||||
eob_free_size = (q->last >= q->first) ? q->queueMaxSize - (q->last + curElementSize) : 0;
|
||||
|
||||
/* check how many bytes of wrapped element (if anay) are at end of buffer */
|
||||
wrapped_element_eob_size = (((elementSize + elemSizeStorageRoom )*nbElements) < eob_free_size) ? 0 : (eob_free_size % (elementSize + elemSizeStorageRoom));
|
||||
wrap_will_occur = wrapped_element_eob_size > elemSizeStorageRoom;
|
||||
|
||||
overhead = (wrap_will_occur && (q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG)) ? wrapped_element_eob_size : overhead;
|
||||
overhead = (wrap_will_occur && (q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG)) ? elemSizeStorageRoom : overhead;
|
||||
|
||||
|
||||
/* Store now the elements if ennough room for all elements */
|
||||
if (elementSize && ((q->byteCount + ((elementSize + elemSizeStorageRoom )*nbElements) + overhead) <= q->queueMaxSize))
|
||||
{
|
||||
/* loop to add all elements */
|
||||
for (i=0; i < nbElements; i++)
|
||||
{
|
||||
q->last = MOD ((q->last + curElementSize),q->queueMaxSize);
|
||||
curBuffPosition = q->last;
|
||||
|
||||
/* store the element */
|
||||
/* store first the element size if element size is variable */
|
||||
if (q->elementSize == 0)
|
||||
{
|
||||
q->qBuff[curBuffPosition++]= elementSize & 0xFF;
|
||||
curBuffPosition = MOD(curBuffPosition, q->queueMaxSize);
|
||||
q->qBuff[curBuffPosition++]= (elementSize & 0xFF00) >> 8 ;
|
||||
curBuffPosition = MOD(curBuffPosition, q->queueMaxSize);
|
||||
q->byteCount += 2;
|
||||
}
|
||||
|
||||
/* Identify number of bytes of copy takeing account possible wrap, in this case NbBytesToCopy will contains size that fit at end of the queue buffer */
|
||||
NbBytesToCopy = MIN((q->queueMaxSize-curBuffPosition),elementSize);
|
||||
/* check if no wrap (NbBytesToCopy == elementSize) or if Wrap and no spsicf option;
|
||||
In this case part of data will copied at the end of the buffer and the rest a the beginning */
|
||||
if ((NbBytesToCopy == elementSize) || ((NbBytesToCopy < elementSize) && (q->optionFlags == CIRCULAR_QUEUE_NO_FLAG)))
|
||||
{
|
||||
/* Copy First part (or emtire buffer ) from current position up to the end of the buffer queue (or before if enough room) */
|
||||
memcpy(&q->qBuff[curBuffPosition],&x[i*elementSize],NbBytesToCopy);
|
||||
/* Adjust bytes count */
|
||||
q->byteCount += NbBytesToCopy;
|
||||
/* Wrap */
|
||||
curBuffPosition = 0;
|
||||
/* set NbCopiedBytes bytes with ampount copied */
|
||||
NbCopiedBytes = NbBytesToCopy;
|
||||
/* set the rest to copy if wrao , if no wrap will be 0 */
|
||||
NbBytesToCopy = elementSize - NbBytesToCopy;
|
||||
/* set the current element Size, will be used to calaculate next last position at beginning of loop */
|
||||
curElementSize = (elementSize) + elemSizeStorageRoom ;
|
||||
}
|
||||
else if (NbBytesToCopy) /* We have a wrap to manage */
|
||||
{
|
||||
/* case of CIRCULAR_QUEUE_NO_WRAP_FLAG option */
|
||||
if (q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG)
|
||||
{
|
||||
/* if element size are variable and NO_WRAP option, Invalidate end of buffer setting 0xFFFF size*/
|
||||
if (q->elementSize == 0)
|
||||
{
|
||||
q->qBuff[curBuffPosition-2] = 0xFF;
|
||||
q->qBuff[curBuffPosition-1] = 0xFF;
|
||||
}
|
||||
q->byteCount += NbBytesToCopy; /* invalid data at the end of buffer are take into account in byteCount */
|
||||
/* No bytes coped a the end of buffer */
|
||||
NbCopiedBytes = 0;
|
||||
/* all element to be copied at the begnning of buffer */
|
||||
NbBytesToCopy = elementSize;
|
||||
/* Wrap */
|
||||
curBuffPosition = 0;
|
||||
/* if variable size element, invalidate end of buffer setting OxFFFF in element header (size) */
|
||||
if (q->elementSize == 0)
|
||||
{
|
||||
q->qBuff[curBuffPosition++] = NbBytesToCopy & 0xFF;
|
||||
q->qBuff[curBuffPosition++] = (NbBytesToCopy & 0xFF00) >> 8 ;
|
||||
q->byteCount += 2;
|
||||
}
|
||||
|
||||
}
|
||||
/* case of CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG option */
|
||||
else if (q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG)
|
||||
{
|
||||
if (q->elementSize == 0)
|
||||
{
|
||||
/* reset the size of current element to the nb bytes fitting at the end of buffer */
|
||||
q->qBuff[curBuffPosition-2] = NbBytesToCopy & 0xFF;
|
||||
q->qBuff[curBuffPosition-1] = (NbBytesToCopy & 0xFF00) >> 8 ;
|
||||
/* copy the bytes */
|
||||
memcpy(&q->qBuff[curBuffPosition],&x[i*elementSize],NbBytesToCopy);
|
||||
q->byteCount += NbBytesToCopy;
|
||||
/* set the number of copied bytes */
|
||||
NbCopiedBytes = NbBytesToCopy;
|
||||
/* set rest of data to be copied to begnning of buffer */
|
||||
NbBytesToCopy = elementSize - NbBytesToCopy;
|
||||
/* one element more dur to split in 2 elements */
|
||||
q->elementCount++;
|
||||
/* Wrap */
|
||||
curBuffPosition = 0;
|
||||
/* Set new size for rest of data */
|
||||
q->qBuff[curBuffPosition++] = NbBytesToCopy & 0xFF;
|
||||
q->qBuff[curBuffPosition++] = (NbBytesToCopy & 0xFF00) >> 8 ;
|
||||
q->byteCount += 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Should not occur */
|
||||
/* can not manage split Flag on Fixed size element */
|
||||
/* Buffer is corrupted */
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
curElementSize = (NbBytesToCopy) + elemSizeStorageRoom ;
|
||||
q->last = 0;
|
||||
}
|
||||
|
||||
/* some remaining byte to copy */
|
||||
if (NbBytesToCopy)
|
||||
{
|
||||
memcpy(&q->qBuff[curBuffPosition],&x[(i*elementSize)+NbCopiedBytes],NbBytesToCopy);
|
||||
q->byteCount += NbBytesToCopy;
|
||||
}
|
||||
|
||||
/* One more element */
|
||||
q->elementCount++;
|
||||
}
|
||||
|
||||
ptr = q->qBuff + (MOD((q->last+elemSizeStorageRoom ),q->queueMaxSize));
|
||||
}
|
||||
/* for Breakpoint only...to remove */
|
||||
else
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return ptr;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Remove element from the queue and copy it in provided buffer
|
||||
* @note This function is used to remove and element from the Circular Queue .
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param elementSize: Pointer to return Size of element to be removed
|
||||
* @param buffer: destination buffer where to copy element
|
||||
* @retval Pointer on removed element. NULL if queue was empty
|
||||
*/
|
||||
uint8_t* CircularQueue_Remove_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Remove element from the queue.
|
||||
* @note This function is used to remove and element from the Circular Queue .
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param elementSize: Pointer to return Size of element to be removed (ignored if NULL)
|
||||
* @retval Pointer on removed element. NULL if queue was empty
|
||||
*/
|
||||
uint8_t* CircularQueue_Remove(queue_t *q, uint16_t* elementSize)
|
||||
{
|
||||
uint8_t elemSizeStorageRoom = 0;
|
||||
uint8_t* ptr= NULL;
|
||||
elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
|
||||
uint16_t eltSize = 0;
|
||||
if (q->byteCount > 0)
|
||||
{
|
||||
/* retrieve element Size */
|
||||
eltSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
|
||||
|
||||
if ((q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG) && !(q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG))
|
||||
{
|
||||
if (((eltSize == 0xFFFF) && q->elementSize == 0 ) ||
|
||||
((q->first > q->last) && q->elementSize && ((q->queueMaxSize - q->first) < q->elementSize)))
|
||||
{
|
||||
/* all data from current position up to the end of buffer are invalid */
|
||||
q->byteCount -= (q->queueMaxSize - q->first);
|
||||
/* Adjust first element pos */
|
||||
q->first = 0;
|
||||
/* retrieve the right size after the wrap [if variable size element] */
|
||||
eltSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
|
||||
}
|
||||
}
|
||||
|
||||
/* retrieve element */
|
||||
ptr = q->qBuff + (MOD((q->first + elemSizeStorageRoom), q->queueMaxSize));
|
||||
|
||||
/* adjust byte count */
|
||||
q->byteCount -= (eltSize + elemSizeStorageRoom) ;
|
||||
|
||||
/* Adjust q->first */
|
||||
if (q->byteCount > 0)
|
||||
{
|
||||
q->first = MOD((q->first+ eltSize + elemSizeStorageRoom ), q->queueMaxSize);
|
||||
}
|
||||
/* adjust element count */
|
||||
--q->elementCount;
|
||||
}
|
||||
if (elementSize != NULL)
|
||||
{
|
||||
*elementSize = eltSize;
|
||||
}
|
||||
return ptr;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief "Sense" first element of the queue, without removing it and copy it in provided buffer
|
||||
* @note This function is used to return a pointer on the first element of the queue without removing it.
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param elementSize: Pointer to return Size of element to be removed
|
||||
* @param buffer: destination buffer where to copy element
|
||||
* @retval Pointer on sensed element. NULL if queue was empty
|
||||
*/
|
||||
|
||||
uint8_t* CircularQueue_Sense_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief "Sense" first element of the queue, without removing it.
|
||||
* @note This function is used to return a pointer on the first element of the queue without removing it.
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @param elementSize: Pointer to return Size of element to be removed (ignored if NULL)
|
||||
* @retval Pointer on sensed element. NULL if queue was empty
|
||||
*/
|
||||
uint8_t* CircularQueue_Sense(queue_t *q, uint16_t* elementSize)
|
||||
{
|
||||
uint8_t elemSizeStorageRoom = 0;
|
||||
uint8_t* x= NULL;
|
||||
elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
|
||||
uint16_t eltSize = 0;
|
||||
uint32_t FirstElemetPos = 0;
|
||||
|
||||
if (q->byteCount > 0)
|
||||
{
|
||||
FirstElemetPos = q->first;
|
||||
eltSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
|
||||
|
||||
if ((q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG) && !(q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG))
|
||||
{
|
||||
if (((eltSize == 0xFFFF) && q->elementSize == 0 ) ||
|
||||
((q->first > q->last) && q->elementSize && ((q->queueMaxSize - q->first) < q->elementSize)))
|
||||
|
||||
{
|
||||
/* all data from current position up to the end of buffer are invalid */
|
||||
FirstElemetPos = 0; /* wrap to the begiining of buffer */
|
||||
|
||||
/* retrieve the right size after the wrap [if variable size element] */
|
||||
eltSize = (q->elementSize == 0) ? q->qBuff[FirstElemetPos]+ ((q->qBuff[MOD((FirstElemetPos+1), q->queueMaxSize)])<<8) : q->elementSize;
|
||||
}
|
||||
}
|
||||
/* retrieve element */
|
||||
x = q->qBuff + (MOD((FirstElemetPos + elemSizeStorageRoom), q->queueMaxSize));
|
||||
}
|
||||
if (elementSize != NULL)
|
||||
{
|
||||
*elementSize = eltSize;
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if queue is empty.
|
||||
* @note This function is used to to check if the queue is empty.
|
||||
* @param q: pointer on queue structure to be handled
|
||||
* @retval TRUE (!0) if the queue is empyu otherwise FALSE (0)
|
||||
*/
|
||||
int CircularQueue_Empty(queue_t *q)
|
||||
{
|
||||
int ret=FALSE;
|
||||
if (q->byteCount <= 0)
|
||||
{
|
||||
ret=TRUE;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
int CircularQueue_NbElement(queue_t *q)
|
||||
{
|
||||
return q->elementCount;
|
||||
}
|
||||
Reference in New Issue
Block a user