Index: App/Services/MsgQueues.c
===================================================================
diff -u -ree310a2e5262c05bf0dc0eb0d84da0ee50bac7fe -ra87b6b9e253c6c0fcc84bca6f5de71959ce18bcc
--- App/Services/MsgQueues.c	(.../MsgQueues.c)	(revision ee310a2e5262c05bf0dc0eb0d84da0ee50bac7fe)
+++ App/Services/MsgQueues.c	(.../MsgQueues.c)	(revision a87b6b9e253c6c0fcc84bca6f5de71959ce18bcc)
@@ -10,7 +10,10 @@
  *  @date 08-Oct-2019
  *  @author S. Nash
  *
- *  @brief MsgQueues service module.  Provides message queue functionality.
+ *  @brief MsgQueues service module.  Provides message queue functionality. \n
+ *  These queues are NOT thread safe.  However, these queue functions are \n
+ *  intended to be called from the General Task thread so no thread safety \n
+ *  is required.
  *
  **************************************************************************/
 
@@ -19,8 +22,17 @@
 
 // ********** private definitions **********
 
+#define MAX_MSG_QUEUE_SIZE          100      // messages
+
 // ********** private data **********
 
+static U32 msgQueueCounts[NUM_OF_MSG_QUEUES];
+static U32 msgQueueStarts[NUM_OF_MSG_QUEUES];
+static U32 msgQueueNexts[NUM_OF_MSG_QUEUES];
+static MESSAGE_WRAPPER_T msgQueues[NUM_OF_MSG_QUEUES][MAX_MSG_QUEUE_SIZE];
+
+// ********** private function prototypes **********
+
 /*************************************************************************
  * @brief initMsgQueues
  * The initMsgQueues function initializes the MsgQueues module.
@@ -32,41 +44,101 @@
  *************************************************************************/
 void initMsgQueues( void )
 {
+    U32 q, m;
 
+    // reset message queues
+    for ( q = 0; q < NUM_OF_MSG_QUEUES; q++ )
+    {
+        msgQueueCounts[q] = 0;
+        msgQueueStarts[q] = 0;
+        msgQueueNexts[q] = 0;
+        for ( m = 0; m < MAX_MSG_QUEUE_SIZE; m++ )
+        {
+            blankMessageInWrapper( &msgQueues[q][m] );
+        }
+    }
 }
 
 /*************************************************************************
  * @brief addToMsgQueue
- * The addToMsgQueue function adds a message to a given message queue.
+ * The addToMsgQueue function adds a message to a given message queue. \n
+ * This function should only be called from the General Task.
  * @details
  * Inputs : none
- * Outputs : none
+ * Outputs : message added to queue
  * @param queue : the message queue to add to
  * @param msg : a pointer to a message structure to add to the queue
  * @return TRUE if message added to queue, FALSE if could not
  *************************************************************************/
-BOOL addToMsgQueue( MSG_QUEUE_T queue, MESSAGE_T *msg )
+BOOL addToMsgQueue( MSG_QUEUE_T queue, MESSAGE_WRAPPER_T *msg )
 {
     BOOL result = FALSE;
 
+    // verify given message queue
+    if ( queue < NUM_OF_MSG_QUEUES )
+    {
+        if ( FALSE == isMsgQueueFull( queue ) )
+        {
+            result = TRUE;
+            // add message to queue
+            msgQueues[queue][msgQueueNexts[queue]] = *msg;
+            // increment next index to add to
+            msgQueueNexts[queue] = INC_WRAP(msgQueueNexts[queue],0,MAX_MSG_QUEUE_SIZE);
+            // increment queue count
+            msgQueueCounts[queue]++;
+        }
+        else // msg queue is full
+        {
+            // TODO - s/w fault?
+        }
+    }
+    else // invalid message queue
+    {
+        // TODO - s/w fault
+    }
+
     return result;
 }
 
 /*************************************************************************
  * @brief getFromMsgQueue
  * The getFromMsgQueue function retrieves the next message from a given \n
- * message queue.
+ * message queue.  This function should only be called from the General Task.
  * @details
- * Inputs : none
- * Outputs : none
+ * Inputs : queue
+ * Outputs : message retrieved from the queue
  * @param queue : the message queue to retrieve from
- * @param msg : a pointer to a message structure to stuff
+ * @param msg : a pointer to a message structure to populate with the retrieved \n
+ * message.
  * @return TRUE if a message was found to retrieve, FALSE if not
  *************************************************************************/
-BOOL getFromMsgQueue( MSG_QUEUE_T queue, MESSAGE_T *msg )
+BOOL getFromMsgQueue( MSG_QUEUE_T queue, MESSAGE_WRAPPER_T *msg )
 {
     BOOL result = FALSE;
 
+    // verify given message queue
+    if ( queue < NUM_OF_MSG_QUEUES )
+    {
+        if ( FALSE == isMsgQueueEmpty( queue ) )
+        {
+            result = TRUE;
+            // get message from queue
+            *msg = msgQueues[queue][msgQueueStarts[queue]];
+            // increment queue next index to get from
+            msgQueueStarts[queue] = INC_WRAP(msgQueueStarts[queue],0,MAX_MSG_QUEUE_SIZE);
+            // decrement queue count
+            msgQueueCounts[queue]--;
+        }
+        else // message queue is empty
+        {
+            // result already set to FALSE
+        }
+    }
+    else // invalid message queue
+    {
+        // TODO - s/w fault
+    }
+
     return result;
 }
 
@@ -83,6 +155,19 @@
 {
     BOOL result = FALSE;
 
+    // verify given message queue
+    if ( queue < NUM_OF_MSG_QUEUES )
+    {
+        if ( msgQueueCounts[queue] == 0 )
+        {
+            result = TRUE;
+        }
+    }
+    else // invalid message queue
+    {
+        // TODO - s/w fault
+    }
+
     return result;
 }
 
@@ -97,7 +182,73 @@
  *************************************************************************/
 BOOL isMsgQueueFull( MSG_QUEUE_T queue )
 {
-    BOOL result = FALSE;
+    BOOL result = TRUE;
 
+    // verify given message queue
+    if ( queue < NUM_OF_MSG_QUEUES )
+    {
+        if ( msgQueueCounts[queue] < MAX_MSG_QUEUE_SIZE )
+        {
+            result = FALSE;
+        }
+    }
+    else // invalid message queue
+    {
+        // TODO - s/w fault
+    }
+
     return result;
 }
+
+/*************************************************************************
+ * @brief blankMessage
+ * The blankMessage function blanks a given message.
+ * @details
+ * Inputs : none
+ * Outputs : none
+ * @param message : Pointer to the message to blank.
+ * @return none
+ *************************************************************************/
+void blankMessage( MESSAGE_T *message )
+{
+    U32 i;
+    U32 msgSize = sizeof(MESSAGE_T);
+    U08 *msgContent = (U08*)message;
+
+    // zero out the message
+    for ( i = 0; i < msgSize; i++ )
+    {
+        *msgContent++ = 0x0;
+    }
+
+    // set msg ID out of bounds in case blank message goes somewhere
+    message->hdr.msgID = 0xFFFF;
+}
+
+/*************************************************************************
+ * @brief blankMessageInWrapper
+ * The blankMessageInWrapper function blanks a given message in a wrapper.
+ * @details
+ * Inputs : none
+ * Outputs : none
+ * @param message : Pointer to the message in a wrapper to blank.
+ * @return none
+ *************************************************************************/
+void blankMessageInWrapper( MESSAGE_WRAPPER_T *message )
+{
+    U32 i;
+    U32 msgSize = sizeof(MESSAGE_T);
+    U08 *msgContent = (U08*)message;
+
+    // zero out the message
+    for ( i = 0; i < msgSize; i++ )
+    {
+        *msgContent++ = 0x0;
+    }
+
+    // set msg ID out of bounds in case blank message goes somewhere
+    message->msg.hdr.msgID = 0xFFFF;
+}
+
+
+