Revision 4:2034b04c86d2, committed 2016-01-09

Comitter:

ptpaterson

Date:

Sat Jan 09 17:15:29 2016 +0000

Parent:

3:12b3c25bdeba

Child:

5:22a337cdc0e3

Commit message:

echo ability

Changed in this revision

--- a/drivers/mbed-Nucleo-F091RC/canopen_api.cpp	Mon Jan 04 06:10:49 2016 +0000
+++ b/drivers/mbed-Nucleo-F091RC/canopen_api.cpp	Sat Jan 09 17:15:29 2016 +0000
@@ -28,9 +28,13 @@
 
 #include "canopen_api.h"
 #include "canopen_config.h"
+#include "CanOpenHandle.h"
 
 #include "mbed.h"
 
+#include "CanOpenHandle.h"
+#include "CanOpenMessage.h"
+
 int CanOpenApiInit (CanOpenHandle *hCanOpen)
 {
     printf ("----------- CANOPEN API: API INIT\r\n");
@@ -54,6 +58,9 @@
 {
     int result = 0;
     
+    printf("api: can_read: Anything to read?\r\n");
+    
+    /*
     if (hCanOpen->can) {
         
         CANMessage canMsg;
@@ -78,7 +85,7 @@
         }
         
     }
-    
+    */
     return result;
 }
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/CanOpenMessage.h	Sat Jan 09 17:15:29 2016 +0000
@@ -0,0 +1,68 @@
+/**
+ ******************************************************************************
+ * @file
+ * @author  Paul Paterson
+ * @version
+ * @date    2015-12-14
+ * @brief   CANOpen implementation library
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2015 Paul Paterson
+ *
+ * All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef PPCAN_CANOPEN_MESSAGE_H
+#define PPCAN_CANOPEN_MESSAGE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*=========================================================================
+ * CANOpen Message Structure
+ *=========================================================================
+ */
+
+/** CANOpen Message Format */
+typedef enum {
+    CANOPEN_FORMAT_STANDARD = 0,
+    CANOPEN_FORMAT_EXTENDED,
+    CANOPEN_FORMAT_ANY
+} CanOpenFormat;
+
+/** CANOpen Message Data Type */
+typedef enum {
+    CANOPEN_TYPE_DATA = 0,
+    CANOPEN_TYPE_REMOTE
+} CanOpenType;
+
+
+/** CANOpen Message */
+typedef struct CanOpenMessage {
+    unsigned int    id;
+    char   data[8];
+    CanOpenFormat   format;
+    CanOpenType     type;
+    unsigned char   dataCount;
+} CanOpenMessage;
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif /* PPCAN_CANOPEN_MESSAGE_H */

--- a/include/Node.h	Mon Jan 04 06:10:49 2016 +0000
+++ b/include/Node.h	Sat Jan 09 17:15:29 2016 +0000
@@ -29,7 +29,18 @@
 #ifndef PPCAN_NODE_H
 #define PPCAN_NODE_H
 
-#include "canopen_protocol.h"
+
+/*=========================================================================
+ * Forward declarations
+ *=========================================================================
+ */
+struct CanOpenMessage;
+
+/* NMT Constants --------------------------------------------------------*/
+#define NMT_STATE_TOGGLE_BIT      0x80
+
+/* NMT Macros -----------------------------------------------------------*/
+#define NMT_SET_STATE(curState, newState) (curState) = (((curState) & NMT_STATE_TOGGLE_BIT) | ((newState) & (~NMT_STATE_TOGGLE_BIT))))
 
 namespace ppCANOpen
 {
@@ -38,15 +49,26 @@
 class ServiceProvider;
 class ObjectDictionary;
 
+
+
 /** Node Class to implement feature of a CANOpen NMT node
  */
 class Node
 {
 public:
 
-    /** Maintain the multitude of states for a node */
-    typedef struct {
-        NmtState nmtState;
+    /** Node network management state */
+    struct State {
+        static const int INITIALIZED     = 0x00;
+        static const int DISCONNECTED    = 0x01;
+        static const int CONNECTING      = 0x02;
+        static const int PREPARING       = 0x02;
+        static const int STOPPED         = 0x04;
+        static const int OPERATIONAL     = 0x05;
+        static const int PREOPERATIONAL  = 0x7F;
+        static const int UNKNOWN         = 0x0F;
+        
+        int nmtState;
         int bBoot;
         int bSDO;
         int bEmergency;
@@ -54,7 +76,11 @@
         int bLifeGuard;
         int bPDO;
         int bLSS;
-    } State;
+        
+        int bLifeGuardToggle;
+    };
+
+    /** Maintain the multitude of states for a node */
 
     Node (ServiceProvider * provider);    
 
@@ -161,8 +187,6 @@
      * ========================================================================
      */
 
-    ServiceProvider * pMyProvider;
-
     /** object dictionary */
     ObjectDictionary * pMyDictionary;
 
@@ -173,6 +197,12 @@
      * ========================================================================
      */
      
+     
+    /** Reference to Service Provider that this node is attached to.
+      * @note May need to be a pointer, because node id is held in object dictionary
+      */
+    ServiceProvider * pMyProvider;
+     
     /** Network id for the node
       * @note May need to be a pointer, because node id is held in object dictionary
       */

--- a/include/ObjectDictionary.h	Mon Jan 04 06:10:49 2016 +0000
+++ b/include/ObjectDictionary.h	Sat Jan 09 17:15:29 2016 +0000
@@ -29,7 +29,6 @@
 #ifndef PPCAN_OBJECT_DICTIONARY_H
 #define PPCAN_OBJECT_DICTIONARY_H
 
-#include "canopen_protocol.h"
 #include <stdint.h>
 
 namespace ppCANOpen

--- a/include/ProcessDataObject.h	Mon Jan 04 06:10:49 2016 +0000
+++ b/include/ProcessDataObject.h	Sat Jan 09 17:15:29 2016 +0000
@@ -29,8 +29,6 @@
 #ifndef PPCAN_PDO_H
 #define PPCAN_PDO_H
 
-#include "canopen_protocol.h"
-
 namespace ppCANOpen
 {
 

--- a/include/ServiceDataObject.h	Mon Jan 04 06:10:49 2016 +0000
+++ b/include/ServiceDataObject.h	Sat Jan 09 17:15:29 2016 +0000
@@ -29,8 +29,6 @@
 #ifndef PPCAN_SDO_H
 #define PPCAN_SDO_H
 
-#include "canopen_protocol.h"
-
 namespace ppCANOpen
 {
 

--- a/include/ServiceProvider.h	Mon Jan 04 06:10:49 2016 +0000
+++ b/include/ServiceProvider.h	Sat Jan 09 17:15:29 2016 +0000
@@ -29,7 +29,15 @@
 #ifndef PPCAN_SERVICE_PROVIDER_H
 #define PPCAN_SERVICE_PROVIDER_H
 
-#include "canopen_protocol.h"
+#include "CanOpenMessage.h"
+#include <queue>
+
+/*=========================================================================
+ * Forward declarations
+ *=========================================================================
+ */  
+ 
+struct CanOpenHandle;
 
 namespace ppCANOpen
 {
@@ -43,117 +51,52 @@
 {
 public:
 
+    
+
     ServiceProvider (void);
     ~ServiceProvider (void);
     
-    /** Main loop
-     *  @note
-     *  @param
-     *  @retval
-     */
+    /** Main loop */
     void Run (void);
     
-    /**
-     *  @note
-     *  @param
-     *  @retval
-     */
+    /** Register a node to get messages and get update calls
+      * @note
+      * @param
+      * @retval
+      */
     int AddNode (Node * node);
     
+    /** Register a node to get messages and get update calls
+      * @note
+      * @param
+      * @retval
+      */
+      
+    /** Add a message to the message queue outbox
+      * @note
+      * @param
+      */
+    void PostMessage (CanOpenMessage * msg);
+    
     // TODO: remove node (swap pointers to fill in nicely
 private:
     
-    static const int SERVICE_MAX_NODES = 8;
-
-    /* ========================================================================
-     * Methods to handle message requests and responses
-     * Called by the node, usually during Update() or during handling of
-     * incoming messages.
-     * ========================================================================
-     */
-    
-    /* PDO (7.2.2), MPDO (7.2.3) --------------------------------------------*/
-    
-    /** Build and send a PDO request
-     *  @note
-     *  @param
-     */
-    void RequestPdo (int pdoNum);
-    
-    /** Build and send a PDO
-     *  @note
-     *  @param
-     */
-    void ProducePdo (int pdoNum, char * data);
-    
-    
-    /* SDO (7.2.4) ----------------------------------------------------------*/
-    
-    /** initiate SDO download
-     *  @note Handles automatically whether it will be a expedited transfer or
-     *  or if message will be split into   
-     *  @param
-     *
-     *  Node will create a big data array and Service provide will have to
-     *  iterate through and send all of the data.  ServiceProvider will pass
-     *  the confirmation to the node, and the node will free up it's buffer.
-     */
-    void DownloadSdo (int sdoNum, int index, int subindex, int size, char * data);
-    
-    /** initiate SDO upload
-     *  @note 
-     *  @param
-     */
-    void UploadSdo (int sdoNum, int index, int subindex);
-    
-    /** Acknowledge that SDO was recieved properly
-     *  @note 
-     *  @param
-     */
-    void ConfirmSdo (int sdoNum, int bSuccess);
-    
-    /** Abort current SDO transfer
-     *  @note 
-     *  @param
-     */
-    void AbortSdo (int sdoNum);
-    
-    
-    /* Emergency object (7.2.7) ---------------------------------------------*/
-    
-    // TODO: emergency producer
-    
-    
-    /* Network Management (7.2.8) -------------------------------------------*/
-    /* ---- Node Control (7.2.8.2.1) ----------------------------------------*/
-
-    /** Build a CANOpen nmt control message to a node
-     *  @note
-     *  @param
-     */
-    int  SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId);
-
-
-    /* ---- Error Control (7.2.8.2.2) ---------------------------------------*/
-
-    /** Build a CANOpen error control request to a node
-     *  @note
-     *  @param
-     */
-    int  RequestErrorControl (NmtCommandSpecifier cs, unsigned int nodeId);
-
-    /** Build a CANOpen error control response
-     *  @note
-     *  @param
-     */
-    int  RespondErrorControl (NmtCommandSpecifier cs, unsigned int nodeId);
-
+    /** define size of arrays in one place */
+    static const int SERVICE_MAX_NODES = 8;    
 
     /* ========================================================================
      * Private Members
      * ========================================================================
      */
 
+    /** array of messages in a queue
+      * @note twice number of max nodes is a bit arbitrary.  Will need at
+      * least max nodes, but not sure how much more to be safe.
+      */
+    std::queue<CanOpenMessage> outbox;
+    //CanOpenMessage outbox[SERVICE_MAX_NODES * 2];
+    //int messageCount;
+
     /** array of nodes to cycle through.
      *  @note
      */

--- a/include/canopen_api.h	Mon Jan 04 06:10:49 2016 +0000
+++ b/include/canopen_api.h	Sat Jan 09 17:15:29 2016 +0000
@@ -29,16 +29,26 @@
 #ifndef PPCAN_CANOPEN_API_H
 #define PPCAN_CANOPEN_API_H
 
-#include "canopen_protocol.h"
-
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+    /*=========================================================================
+     * Forward declarations
+     *=========================================================================
+     */  
+    struct CanOpenHandle;
+    struct CanOpenMessage;
+
+    /*=========================================================================
+     * API functions
+     *=========================================================================
+     */    
+    
     int CanOpenApiInit  (CanOpenHandle * hCanOpen);
     
-    int CanOpenApiRead  (CanOpenHandle * hCanOpen, CanOpenMessage *canOpenMsg);
-    int CanOpenApiWrite (CanOpenHandle * hCanOpen, CanOpenMessage *canOpenMsg);
+    int CanOpenApiRead  (CanOpenHandle * hCanOpen, CanOpenMessage * canOpenMsg);
+    int CanOpenApiWrite (CanOpenHandle * hCanOpen, CanOpenMessage * canOpenMsg);
 
 #ifdef __cplusplus
 };

--- a/include/canopen_protocol.h	Mon Jan 04 06:10:49 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,161 +0,0 @@
-/**
- ******************************************************************************
- * @file
- * @author  Paul Paterson
- * @version
- * @date    2015-12-17
- * @brief   CANOpen implementation library
- ******************************************************************************
- * @attention
- *
- * <h2><center>&copy; COPYRIGHT(c) 2015 Paul Paterson
- *
- * All rights reserved.
-
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef PPCAN_CANOPEN_PROTOCOL_H
-#define PPCAN_CANOPEN_PROTOCOL_H
-
-#include "CanOpenHandle.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-    /*=========================================================================
-     * MESSAGES
-     *=========================================================================
-     */
-
-    /** CANOpen Message Format */
-    typedef enum {
-        CANOPEN_FORMAT_STANDARD = 0,
-        CANOPEN_FORMAT_EXTENDED,
-        CANOPEN_FORMAT_ANY
-    } CanOpenFormat;
-
-    /** CANOpen Message Data Type */
-    typedef enum {
-        CANOPEN_TYPE_DATA = 0,
-        CANOPEN_TYPE_REMOTE
-    } CanOpenType;
-
-    /** CANOpen Function Codes */
-    typedef enum {
-        CANOPEN_FUNCTION_CODE_NMT        = 0x00,
-        CANOPEN_FUNCTION_CODE_SYNC       = 0x01,
-        CANOPEN_FUNCTION_CODE_TIME       = 0x02,
-        CANOPEN_FUNCTION_CODE_PDO1T      = 0x03,
-        CANOPEN_FUNCTION_CODE_PD01R      = 0x04,
-        CANOPEN_FUNCTION_CODE_PD02T      = 0x05,
-        CANOPEN_FUNCTION_CODE_PD02R      = 0x06,
-        CANOPEN_FUNCTION_CODE_PD03T      = 0x07,
-        CANOPEN_FUNCTION_CODE_PD03R      = 0x08,
-        CANOPEN_FUNCTION_CODE_PD04T      = 0x09,
-        CANOPEN_FUNCTION_CODE_PD04R      = 0x0A,
-        CANOPEN_FUNCTION_CODE_SD0T       = 0x0B,
-        CANOPEN_FUNCTION_CODE_SD0R       = 0x0C,
-        CANOPEN_FUNCTION_CODE_NODE_GUARD = 0x0E,
-        CANOPEN_FUNCTION_CODE_LSS        = 0x0F
-    } CanOpenFunctionCodes;
-
-    /** CANOpen Message */
-    typedef struct {
-        unsigned int    id;
-        char   data[8];
-        CanOpenFormat   format;
-        CanOpenType     type;
-        unsigned char   dataCount;
-    } CanOpenMessage;
-
-    
-
-    /* Message Constants */
-    #define MESSAGE_NODEID_BITS     0b00001111111
-    #define MESSAGE_COMMAND_BITS    0b11110000000
-
-    /* Message Macros -----------------------------------------------------------*/ 
-    #define MESSAGE_GET_NODEID(cobId)   (cobId & MESSAGE_NODEID_BITS)
-    #define MESSAGE_GET_COMMAND(cobId)  ((cobId & MESSAGE_COMMAND_BITS) >> 7)
-
-    /*=========================================================================
-     * SDO MESSAGE PARAMETERS
-     *=========================================================================
-     */
-    
-    /** SDO initiate protocol command specifiers */
-    typedef enum {
-         SDO_CCS_DOWNLOAD_SEGMENT_REQUEST   = 0x00, 
-         SDO_CCS_INITIATE_DOWNLOAD_REQUEST  = 0x01,
-         SDO_CCS_INITIATE_UPLOAD_REQUEST    = 0x02,
-    } SdoClientCommandSpecifier;
-    
-    /** SDO segment protocol command specifiers */
-    typedef enum {
-         SDO_SCS_DOWNLOAD_SEGMENT_RESPONSE  = 0x01,
-         SDO_SCS_INITIATE_UPLOAD_RESPONSE   = 0x02,
-         SDO_SCS_INITIATE_DOWNLOAD_RESPONSE = 0x03,
-    } SdoServerCommandSpecifier;
-    
-    /* SDO constants --------------------------------------------------------*/
-    #define SDO_SIZE_INDICATOR_BIT  0b00000001
-    #define SDO_TRANSFER_TYPE_BIT   0b00000010
-    #define SDO_DATA_COUNT_BITS     0b00001100
-    #define SDO_TOGGLE_BIT          0b00010000
-    #define SDO_CS_BITS             0b11100000
-    
-    /* SDO macros -----------------------------------------------------------*/ 
-    #define SDO_GET_CS(data0)           ((data0 & SDO_CS_BITS) >> 5)
-    #define SDO_GET_DATA_COUNT(data0)   ((data0 & SDO_DATA_COUNT_BITS) >> 2)
-    
-    
-    /*=========================================================================
-     * NMT MESSAGE PARAMETERS
-     *=========================================================================
-     */
-
-    /** Node network management state */
-    typedef enum NmtState {
-        NMT_STATE_INITIALIZED     = 0x00,
-        NMT_STATE_DISCONNECTED    = 0x01,
-        NMT_STATE_CONNECTING      = 0x02,
-        NMT_STATE_PREPARING       = 0x02,
-        NMT_STATE_STOPPED         = 0x04,
-        NMT_STATE_OPERATIONAL     = 0x05,
-        NMT_STATE_PREOPERATIONAL  = 0x7F,
-        NMT_STATE_UNKNOWN         = 0x0F
-    } NmtState;
-    
-    /** NMT node control protocol command specifiers */
-    typedef enum {
-         NMT_CS_START       = 0x01,
-         NMT_CS_STOP        = 0x02,
-         NMT_CS_ENTER_PREOP = 0x80,
-         NMT_CS_RESET_NODE  = 0x81,
-         NMT_CS_RESET_COM   = 0x82
-    } NmtCommandSpecifier;
-    
-    /* NMT Constants --------------------------------------------------------*/
-    #define NMT_STATE_TOGGLE_BIT      0x80
-    
-    /* NMT Macros -----------------------------------------------------------*/
-    #define NMT_SET_STATE(curState, newState) (curState) = (((curState) & NMT_STATE_TOGGLE_BIT) | ((newState) & (~NMT_STATE_TOGGLE_BIT))))
-    
-#ifdef __cplusplus
-};
-#endif
-
-#endif /* PPCAN_CANOPEN_PROTOCOL */

--- a/source/Node.cpp	Mon Jan 04 06:10:49 2016 +0000
+++ b/source/Node.cpp	Sat Jan 09 17:15:29 2016 +0000
@@ -30,12 +30,90 @@
 #include "ServiceProvider.h"
 #include "ObjectDictionary.h"
 
+#include "CanOpenMessage.h"
+
+#include <stdio.h>
+
 namespace ppCANOpen
 {
+    
 
-Node::Node (ServiceProvider * provider) 
+    /** CANOpen Function Codes */
+    typedef enum {
+        CANOPEN_FUNCTION_CODE_NMT        = 0x00,
+        CANOPEN_FUNCTION_CODE_SYNC       = 0x01,
+        CANOPEN_FUNCTION_CODE_TIME       = 0x02,
+        CANOPEN_FUNCTION_CODE_PDO1T      = 0x03,
+        CANOPEN_FUNCTION_CODE_PD01R      = 0x04,
+        CANOPEN_FUNCTION_CODE_PD02T      = 0x05,
+        CANOPEN_FUNCTION_CODE_PD02R      = 0x06,
+        CANOPEN_FUNCTION_CODE_PD03T      = 0x07,
+        CANOPEN_FUNCTION_CODE_PD03R      = 0x08,
+        CANOPEN_FUNCTION_CODE_PD04T      = 0x09,
+        CANOPEN_FUNCTION_CODE_PD04R      = 0x0A,
+        CANOPEN_FUNCTION_CODE_SD0T       = 0x0B,
+        CANOPEN_FUNCTION_CODE_SD0R       = 0x0C,
+        CANOPEN_FUNCTION_CODE_NODE_GUARD = 0x0E,
+        CANOPEN_FUNCTION_CODE_LSS        = 0x0F
+    } CanOpenFunctionCodes;
+
+    /* Message Constants */
+    #define MESSAGE_NODEID_BITS     0b00001111111
+    #define MESSAGE_COMMAND_BITS    0b11110000000
+
+    /* Message Macros -----------------------------------------------------------*/ 
+    #define MESSAGE_GET_NODEID(cobId)   (cobId & MESSAGE_NODEID_BITS)
+    #define MESSAGE_GET_COMMAND(cobId)  ((cobId & MESSAGE_COMMAND_BITS) >> 7)
+
+    /*=========================================================================
+     * SDO MESSAGE PARAMETERS
+     *=========================================================================
+     */
+    
+    /** SDO initiate protocol command specifiers */
+    typedef enum {
+         SDO_CCS_DOWNLOAD_SEGMENT_REQUEST   = 0x00, 
+         SDO_CCS_INITIATE_DOWNLOAD_REQUEST  = 0x01,
+         SDO_CCS_INITIATE_UPLOAD_REQUEST    = 0x02,
+    } SdoClientCommandSpecifier;
+    
+    /** SDO segment protocol command specifiers */
+    typedef enum {
+         SDO_SCS_DOWNLOAD_SEGMENT_RESPONSE  = 0x01,
+         SDO_SCS_INITIATE_UPLOAD_RESPONSE   = 0x02,
+         SDO_SCS_INITIATE_DOWNLOAD_RESPONSE = 0x03,
+    } SdoServerCommandSpecifier;
+    
+    /* SDO constants --------------------------------------------------------*/
+    #define SDO_SIZE_INDICATOR_BIT  0b00000001
+    #define SDO_TRANSFER_TYPE_BIT   0b00000010
+    #define SDO_DATA_COUNT_BITS     0b00001100
+    #define SDO_TOGGLE_BIT          0b00010000
+    #define SDO_CS_BITS             0b11100000
+    
+    /* SDO macros -----------------------------------------------------------*/ 
+    #define SDO_GET_CS(data0)           ((data0 & SDO_CS_BITS) >> 5)
+    #define SDO_GET_DATA_COUNT(data0)   ((data0 & SDO_DATA_COUNT_BITS) >> 2)
+    
+    
+    /*=========================================================================
+     * NMT MESSAGE PARAMETERS
+     *=========================================================================
+     */
+
+    /** NMT node control protocol command specifiers */
+    typedef enum {
+         NMT_CS_START       = 0x01,
+         NMT_CS_STOP        = 0x02,
+         NMT_CS_ENTER_PREOP = 0x80,
+         NMT_CS_RESET_NODE  = 0x81,
+         NMT_CS_RESET_COM   = 0x82
+    } NmtCommandSpecifier;
+
+Node::Node (ServiceProvider * pProvider)
 {
-    provider->AddNode(this);
+    pMyProvider = pProvider;
+    pProvider->AddNode(this);
 }
 
 /*=============================================================================
@@ -43,21 +121,29 @@
  *=============================================================================
  */
 
-int Node::DispatchMessage(CanOpenMessage *canOpenMsg)
+int Node::DispatchMessage(CanOpenMessage *msg)
 {
-    int command = MESSAGE_GET_COMMAND(canOpenMsg->id);
-    int nodeId = MESSAGE_GET_NODEID(canOpenMsg->id);
+    int command = MESSAGE_GET_COMMAND(msg->id);
+    int nodeId  = MESSAGE_GET_NODEID(msg->id);
+
+    //printf("*** N.Dispatch: got com and id\r\n");
     
     switch (command) {
         case CANOPEN_FUNCTION_CODE_NMT:
-            if (canOpenMsg->data[0] == nodeId) {
-                HandleNodeControl ((int)canOpenMsg->data[1]);
+            printf("*** N.Dispatch: it's an NMT Control!!!\r\n");
+            if (msg->data[0] == nodeId) {
+                HandleNodeControl ((int)msg->data[1]);
             }
             break;
         default:
+            printf("*** N.Dispatch: some random message\r\n");
             break;
     }
-    
+
+    // ECHO ***********************
+    pMyProvider->PostMessage(msg);
+    // END ECHO *******************
+
     return 1;
 }
 
@@ -84,11 +170,11 @@
 int Node::HandleNodeControl (int commandSpecifier)
 {
     int result = 0;
-    
+
     switch (commandSpecifier) {
         case NMT_CS_START:
-            if (state.nmtState != NMT_STATE_INITIALIZED) {
-                state.nmtState = NMT_STATE_OPERATIONAL;
+            if (State::INITIALIZED != state.nmtState) {
+                state.nmtState = State::OPERATIONAL;
                 state.bBoot         = 0;
                 state.bSDO          = 1;
                 state.bEmergency    = 1;
@@ -100,10 +186,10 @@
                 result = 1;
             }
             break;
-            
+
         case NMT_CS_STOP:
-            if (state.nmtState != NMT_STATE_INITIALIZED) {
-                state.nmtState = NMT_STATE_STOPPED;
+            if (State::INITIALIZED != state.nmtState) {
+                state.nmtState = State::STOPPED;
                 state.bBoot         = 0;
                 state.bSDO          = 0;
                 state.bEmergency    = 0;
@@ -115,9 +201,9 @@
             OnStopped();
             result = 1;
             break;
-            
+
         case NMT_CS_ENTER_PREOP:
-            state.nmtState = NMT_STATE_PREOPERATIONAL;
+            state.nmtState = State::PREOPERATIONAL;
             state.bBoot         = 0;
             state.bSDO          = 1;
             state.bEmergency    = 1;
@@ -128,10 +214,11 @@
             OnPreoperational();
             result = 1;
             break;
-            
+
         case NMT_CS_RESET_NODE:
         case NMT_CS_RESET_COM:
-            state.nmtState      = NMT_STATE_INITIALIZED;
+            
+            state.nmtState      = State::INITIALIZED;
             state.bBoot         = 1;
             state.bSDO          = 0;
             state.bEmergency    = 0;
@@ -139,14 +226,27 @@
             state.bLifeGuard    = 0;
             state.bPDO          = 0;
             state.bLSS          = 0;
-            OnInitialize();
+            
+            state.bLifeGuardToggle = 0;
+            
+            /* boot message is actually just the first node guard/ heart beat message */
+            // TODO: wrap up into heartbeat/lifeguard message             
+            CanOpenMessage msgBoot;
+            msgBoot.id      = CANOPEN_FUNCTION_CODE_NODE_GUARD | 5;
+            msgBoot.format  = CANOPEN_FORMAT_STANDARD;
+            msgBoot.type    = CANOPEN_TYPE_DATA;
+            msgBoot.dataCount = 1;
+            msgBoot.data[0] = 0;
+            
+            pMyProvider->PostMessage(&msgBoot);
+            
             result = 1;
             break;
-            
+
         default:
             break;
     }
-    
+
     return result;
 }
 
@@ -164,12 +264,14 @@
  * Methods to handle operation of node device
  *=============================================================================
  */
- 
+
 void Node::Update (void)
 {
-     if (NMT_STATE_OPERATIONAL == state.nmtState) {
+    if (State::OPERATIONAL == state.nmtState) {
         OnUpdate();
-     }
+    }
+    
+    // TODO: check elapsed time to see if it is time for fixed update
 }
 
 /*=============================================================================
@@ -181,8 +283,96 @@
  * Local functions
  *=============================================================================
  */
- 
- 
+
+
+
+/*=============================================================================
+ *============================================================================= 
+ * Methods to handle message requests and responses
+ * Called by the node, usually during Update() or during handling of
+ * incoming messages.
+ *
+ * Removed from Service ProviderClass.  Not sure if we will need these in the future
+ *=============================================================================
+ *=============================================================================
+ */
+
+/* PDO (7.2.2), MPDO (7.2.3) --------------------------------------------*/
+
+/** Build and send a PDO request
+ *  @note
+ *  @param
+ */
+void RequestPdo (int pdoNum){}
+
+/** Build and send a PDO
+ *  @note
+ *  @param
+ */
+void ProducePdo (int pdoNum, char * data){}
+
+
+/* SDO (7.2.4) ----------------------------------------------------------*/
+
+/** initiate SDO download
+ *  @note Handles automatically whether it will be a expedited transfer or
+ *  or if message will be split into   
+ *  @param
+ *
+ *  Node will create a big data array and Service provide will have to
+ *  iterate through and send all of the data.  ServiceProvider will pass
+ *  the confirmation to the node, and the node will free up it's buffer.
+ */
+void DownloadSdo (int sdoNum, int index, int subindex, int size, char * data){}
+
+/** initiate SDO upload
+ *  @note 
+ *  @param
+ */
+void UploadSdo (int sdoNum, int index, int subindex){}
+
+/** Acknowledge that SDO was recieved properly
+ *  @note 
+ *  @param
+ */
+void ConfirmSdo (int sdoNum, int bSuccess){}
+
+/** Abort current SDO transfer
+ *  @note 
+ *  @param
+ */
+void AbortSdo (int sdoNum){}
+
+
+/* Emergency object (7.2.7) ---------------------------------------------*/
+
+// TODO: emergency producer
+
+
+/* Network Management (7.2.8) -------------------------------------------*/
+/* ---- Node Control (7.2.8.2.1) ----------------------------------------*/
+
+/** Build a CANOpen nmt control message to a node
+ *  @note
+ *  @param
+ */
+int  SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+
+
+/* ---- Error Control (7.2.8.2.2) ---------------------------------------*/
+
+/** Build a CANOpen error control request to a node
+ *  @note
+ *  @param
+ */
+int  RequestErrorControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+
+/** Build a CANOpen error control response
+ *  @note
+ *  @param
+ */
+int  RespondErrorControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+
 
 } /* namspace ppCANOpen */
 

--- a/source/ServiceProvider.cpp	Mon Jan 04 06:10:49 2016 +0000
+++ b/source/ServiceProvider.cpp	Sat Jan 09 17:15:29 2016 +0000
@@ -29,6 +29,7 @@
 #include "ServiceProvider.h"
 #include "Node.h"
 
+#include "CanOpenHandle.h"
 #include "canopen_api.h"
 
 #include <string.h>
@@ -37,15 +38,20 @@
 namespace ppCANOpen
 {
     
+/*=============================================================================
+ * Construction
+ *=============================================================================
+ */ 
+
 ServiceProvider::ServiceProvider (void) : nodeCount(0) 
 {
-    printf ("-------- SERVICE PROVIDER: CONSTRUCTOR\r\n");
+    
+    //messageCount = 0;
     
     //memset (nodes, 0, sizeof(Node) * SERVICE_MAX_NODES ); // TODO: fix incorrect <--
     for (int i = 0; i < SERVICE_MAX_NODES; i++) {
         nodes[i] = 0;   
     }
-    printf ("-------- SERVICE PROVIDER: MEMSET GOOD\r\n");
     
     hCanOpen = new CanOpenHandle;
     CanOpenApiInit(hCanOpen);
@@ -63,8 +69,6 @@
 
 void ServiceProvider::Run (void)
 {
-    printf ("-------- SERVICE PROVIDER: RUN\r\n");
-    
     while (1) {
         
         /* Check for new messages */
@@ -72,18 +76,13 @@
         if (hCanOpen->can) {
             if (CanOpenApiRead (hCanOpen, &msg)) {
                 
+                //printf("*** SP.Run: About to dispatch\r\n");
                 /* if new message exists, give it to the nodes*/
                 /* update all of the nodes */
                 for (int i=0; i < nodeCount; i++) {
                     nodes[i]->DispatchMessage(&msg);                 
                 }
-                
-                //Node::DispatchMessage(&msg);
-                
-                //**ECHO**
-                if (CanOpenApiWrite(hCanOpen, &msg)) {
-                }
-                //**END ECHO
+                //printf("*** SP.Run: dispatched\r\n");
             }
         }    
    
@@ -91,11 +90,23 @@
         for (int i=0; i < nodeCount; i++) {
             nodes[i]->Update();            
         }
+        
+        /* send out the responses */
+        while (! outbox.empty()) {
+            
+            printf("msg id: %d\r\n", outbox.front().id);
+            if (CanOpenApiWrite(hCanOpen, &outbox.front())) {
+                outbox.pop();
+            } else {
+                   printf("Could not send last message.  Trying again\r\n");
+            }
+        }
+
     }    
 }
 
 /*=============================================================================
- * Add Node to list
+ * Register a node to get messages and get update calls
  *=============================================================================
  */ 
 
@@ -113,44 +124,15 @@
 }
 
 /* ========================================================================
- * Methods to handle message requests and responses
+ * Adds a message to the message queue outbox
  * ========================================================================
  */
 
-void ServiceProvider::RequestPdo (const int pdoNum)
-{}
-
-void ServiceProvider::ProducePdo (const int pdoNum, char * data)
-{}
-
-int  ServiceProvider::SendNodeControl (NmtCommandSpecifier cs, unsigned int targetId)
+void ServiceProvider::PostMessage (CanOpenMessage * msg)
 {
-//    CanOpenMessage canOpenMsg; 
-//    
-//    canOpenMsg.data[0]     = (char)cs;
-//    canOpenMsg.data[1]     = (char)nodeId;
-//    canOpenMsg.dataCount   = 2;
-//    canOpenMsg.type        = CANOPEN_TYPE_DATA;
-//    canOpenMsg.format      = CANOPEN_FORMAT_STANDARD;
-//    
-//    return CanOpenApiWrite (&canOpenMsg);
-
-    return 0;
+    outbox.push(*msg);
 }
 
-int  ServiceProvider::RequestErrorControl (NmtCommandSpecifier cs, unsigned int targetId)
-{
-    //NMT_STATE_SET(Node::state.nmtState, NMT_STATE_UNKNOWN);
-    return 0;    
-}
-
-int  ServiceProvider::RespondErrorControl (NmtCommandSpecifier cs, unsigned int targetId)
-{
-    //NMT_STATE_SET(Node::state.nmtState, NMT_STATE_UNKNOWN);
-    return 0;    
-}
-
-
 } /* namspace ppCANOpen */