Revision 5:22a337cdc0e3, committed 2016-02-13

Comitter:

ptpaterson

Date:

Sat Feb 13 20:22:59 2016 +0000

Parent:

4:2034b04c86d2

Commit message:

PDO receive complete

Changed in this revision

diff -r 2034b04c86d2 -r 22a337cdc0e3 drivers/mbed-Nucleo-F091RC/canopen_api.cpp
--- a/drivers/mbed-Nucleo-F091RC/canopen_api.cpp	Sat Jan 09 17:15:29 2016 +0000
+++ b/drivers/mbed-Nucleo-F091RC/canopen_api.cpp	Sat Feb 13 20:22:59 2016 +0000
@@ -28,44 +28,78 @@
 
 #include "canopen_api.h"
 #include "canopen_config.h"
-#include "CanOpenHandle.h"
 
-#include "mbed.h"
-
-#include "CanOpenHandle.h"
 #include "CanOpenMessage.h"
 
-int CanOpenApiInit (CanOpenHandle *hCanOpen)
+#include "mbed.h" 
+#include "CAN.h"  /* use with CANNucleo library */
+
+#include "stdio.h"
+
+
+void *pServiceObject = 0;
+
+/* Can declarations ---------------------------------------------------------*/
+CAN * can; //(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
+ServiceProviderRxInterruptCallback  serviceRxCallback = 0; 
+void CanReadInterruptCallback (void) {
+    if (serviceRxCallback && pServiceObject) {
+        serviceRxCallback(pServiceObject);   
+    }
+}
+
+/* Ticker declarations ---------------------------------------------------------*/
+Ticker updateTicker;
+ServiceProviderTickCallback serviceTickCallback = 0;
+void UpdateTickerCallback(void) {
+    if (serviceTickCallback && pServiceObject) {
+        serviceTickCallback(pServiceObject);   
+    }   
+}
+
+int CanOpenApiInit  (void *object, ServiceProviderRxInterruptCallback rxCallback, ServiceProviderTickCallback tickCallback)
 {
     printf ("----------- CANOPEN API: API INIT\r\n");
     
-    /** CAN instance to do all of the communication
-      *
-      * @note For some reason, I cannot use "static CAN can".  It will compile,
-      * but code get's stuck during initialization, and will not even enter
-      * main(). Works great like this though! Only one ServiceProvider, though,
-      * so mem leaks should not be an issue if we don't delete it???
-      */
+    /* CAN instance to do all of the communication
+     *
+     * For some reason, I cannot use "static CAN can".  It will compile,
+     * but code get's stuck during initialization, and will not even enter
+     * main(). Works great like this though! Only one ServiceProvider, though,
+     * so mem leaks should not be an issue if we don't delete it???
+     */
     //static CAN can(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
-    CAN * can = new CAN(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
+    //can = new CAN(CANOPEN_PIN_RX, CANOPEN_PIN_TX); 
+    can = new CAN(CANOPEN_PIN_RX, CANOPEN_PIN_TX);
+    //can.reset();
     
-    hCanOpen->can = can;
+    /* init the callback system ---------------------------------------------*/
+    pServiceObject = object;
+    
+    serviceRxCallback = rxCallback;
+    serviceTickCallback = tickCallback;
+    
+    can->attach(CanReadInterruptCallback);
+    updateTicker.attach(UpdateTickerCallback, .0005);
     
     return 1;
 }
 
-int CanOpenApiRead (CanOpenHandle *hCanOpen, CanOpenMessage *canOpenMsg)
+int CanOpenApiRead (CanOpenMessage *canOpenMsg)
 {
     int result = 0;
     
-    printf("api: can_read: Anything to read?\r\n");
+    /* CAUTON ********************************
+     * Could be used in interrupt!!!
+     * ***************************************
+     */
+    //printf("canopen_api: CanOpenApiRead()\r\n");
     
-    /*
-    if (hCanOpen->can) {
+    if (can) {
         
         CANMessage canMsg;
         
-        result = hCanOpen->can->read (canMsg);
+        result = can->read (canMsg);
         if (result) {
             canOpenMsg->id          = canMsg.id;
             canOpenMsg->dataCount   = canMsg.len, 
@@ -82,42 +116,53 @@
             canOpenMsg->data[5] = canMsg.data[5];
             canOpenMsg->data[6] = canMsg.data[6];
             canOpenMsg->data[7] = canMsg.data[7];
+            
+            // DEBUG
+            //printf("   ID:      %d\r\n", (int)canOpenMsg->id);
+            //printf("   Count:   %d\r\n", (int)canOpenMsg->dataCount);
+            //printf("   Type:    %d\r\n", (int)canOpenMsg->type);
+            //printf("   Format:  %d\r\n", (int)canOpenMsg->format);
+            //printf("   Data[0]: %d\r\n", (int)canOpenMsg->data[0]);
         }
         
     }
-    */
     return result;
 }
 
-int CanOpenApiWrite (CanOpenHandle *hCanOpen, CanOpenMessage *canOpenMsg)
+int CanOpenApiWrite (CanOpenMessage *canOpenMsg)
 {
     int result = 0;
     
-    if (hCanOpen->can) {    
+    CANMessage canMsg;
+    canMsg.id       = canOpenMsg->id;
+    canMsg.len      = canOpenMsg->dataCount, 
+    canMsg.type     = canOpenMsg->type   == CANOPEN_TYPE_DATA       ? CANData     : CANRemote;
+    canMsg.format   = canOpenMsg->format == CANOPEN_FORMAT_STANDARD ? CANStandard : CANExtended;
     
-        CANMessage canMsg;
-        canMsg.id       = canOpenMsg->id;
-        canMsg.len      = canOpenMsg->dataCount, 
-        canMsg.type     = canOpenMsg->type   == CANOPEN_TYPE_DATA       ? CANData     : CANRemote;
-        canMsg.format   = canOpenMsg->format == CANOPEN_FORMAT_STANDARD ? CANStandard : CANExtended;
-        
-        // NOTE: memcpy is freezing execution
-        //memcpy(canMsg.data, canOpenMsg->data, canOpenMsg->dataCount); 
-        canMsg.data[0] = canOpenMsg->data[0];
-        canMsg.data[1] = canOpenMsg->data[1];
-        canMsg.data[2] = canOpenMsg->data[2];
-        canMsg.data[3] = canOpenMsg->data[3];
-        canMsg.data[4] = canOpenMsg->data[4];
-        canMsg.data[5] = canOpenMsg->data[5];
-        canMsg.data[6] = canOpenMsg->data[6];
-        canMsg.data[7] = canOpenMsg->data[7];
-                        
-        result = hCanOpen->can->write (canMsg);
-    }
+    // NOTE: memcpy is freezing execution
+    //memcpy(canMsg.data, canOpenMsg->data, canOpenMsg->dataCount); 
+    canMsg.data[0] = canOpenMsg->data[0];
+    canMsg.data[1] = canOpenMsg->data[1];
+    canMsg.data[2] = canOpenMsg->data[2];
+    canMsg.data[3] = canOpenMsg->data[3];
+    canMsg.data[4] = canOpenMsg->data[4];
+    canMsg.data[5] = canOpenMsg->data[5];
+    canMsg.data[6] = canOpenMsg->data[6];
+    canMsg.data[7] = canOpenMsg->data[7];
+                    
+    result = can->write (canMsg);
+    
     
     return result;
 }
 
+static int timeSyncOffset = 0;
+
+uint32_t CanOpenApiGetHardwareTime (void)
+{
+    return HAL_GetTick();
+}
+
 
 
 
@@ -127,4 +172,3 @@
 
 
 
-

diff -r 2034b04c86d2 -r 22a337cdc0e3 include/CanOpenMessage.h
--- a/include/CanOpenMessage.h	Sat Jan 09 17:15:29 2016 +0000
+++ b/include/CanOpenMessage.h	Sat Feb 13 20:22:59 2016 +0000
@@ -29,6 +29,8 @@
 #ifndef PPCAN_CANOPEN_MESSAGE_H
 #define PPCAN_CANOPEN_MESSAGE_H
 
+#include "stdint.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -54,13 +56,91 @@
 
 /** CANOpen Message */
 typedef struct CanOpenMessage {
-    unsigned int    id;
-    char   data[8];
+    uint32_t        id;
+    uint8_t         data[8];
     CanOpenFormat   format;
     CanOpenType     type;
-    unsigned char   dataCount;
+    uint8_t         dataCount;
 } CanOpenMessage;
 
+
+/*=========================================================================
+ * CANOpen Function Codes
+ *=========================================================================
+ */
+ 
+/** 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_PDO1R      = 0x04,
+    CANOPEN_FUNCTION_CODE_PDO2T      = 0x05,
+    CANOPEN_FUNCTION_CODE_PDO2R      = 0x06,
+    CANOPEN_FUNCTION_CODE_PDO3T      = 0x07,
+    CANOPEN_FUNCTION_CODE_PDO3R      = 0x08,
+    CANOPEN_FUNCTION_CODE_PDO4T      = 0x09,
+    CANOPEN_FUNCTION_CODE_PDO4R      = 0x0A,
+    CANOPEN_FUNCTION_CODE_SDOT       = 0x0B,
+    CANOPEN_FUNCTION_CODE_SDOR       = 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;
+
 #ifdef __cplusplus
 };
 #endif

diff -r 2034b04c86d2 -r 22a337cdc0e3 include/Node.h
--- a/include/Node.h	Sat Jan 09 17:15:29 2016 +0000
+++ b/include/Node.h	Sat Feb 13 20:22:59 2016 +0000
@@ -29,12 +29,14 @@
 #ifndef PPCAN_NODE_H
 #define PPCAN_NODE_H
 
+#include "ObjectDictionary.h"
 
 /*=========================================================================
  * Forward declarations
  *=========================================================================
  */
 struct CanOpenMessage;
+//struct ObjectData;
 
 /* NMT Constants --------------------------------------------------------*/
 #define NMT_STATE_TOGGLE_BIT      0x80
@@ -47,8 +49,6 @@
 
 /* Avoid circular reference */
 class ServiceProvider;
-class ObjectDictionary;
-
 
 
 /** Node Class to implement feature of a CANOpen NMT node
@@ -57,6 +57,11 @@
 {
 public:
 
+    /* ========================================================================
+     * Internal structures and constants
+     * ========================================================================
+     */
+     
     /** Node network management state */
     struct State {
         static const int INITIALIZED     = 0x00;
@@ -80,45 +85,60 @@
         int bLifeGuardToggle;
     };
 
+    /* ========================================================================
+     * Construction
+     * ========================================================================
+     */
+     
     /** Maintain the multitude of states for a node */
-
-    Node (ServiceProvider * provider);    
+    Node (int id, ServiceProvider * provider, int bLoop = 0);    
 
     /* ========================================================================
-     * Methods to handle operation of node device
+     * Public Methods
      * ========================================================================
      */
     
-    // TODO: pass elapsed time to Update method
-    virtual void Update (void);
+    /** Call from ServiceProvider on an interrupt */
+    void FixedUpdate (uint32_t time);
+    
+    /** Call from ServiceProvider every loop of Run() */
+    void Update (void);
     
+    /** Handle message given by the ServiceProvider*/
+     int DispatchMessage(CanOpenMessage *canOpenMsg);
+     
     /* ========================================================================
-     * Message indication
-     * Called by a ServiceProvider when a message is received.
+     * Public Member Variables
      * ========================================================================
      */
      
-    /**
-      * @note
+    /* Common Node properties -----------------------------------------------*/
+    /** Network id for the node
+      * @note Ref is held in object dictionary and upon setting, may need to
+      * update some other objects.
       */
-     int DispatchMessage(CanOpenMessage *canOpenMsg);
+    int nodeId;
 
+    /** Loopback Mode.  Default off (0), if on (1) then will hear it's own 
+      * messages
+      */
+    int bLoopbackOn;
 
 protected:
 
     /* ========================================================================
-     * Methods to handle various messages
-     * Called by a ServiceProvider when a message is received.
+     * Protected? Methods to handle various messages
+     * Used to deal with everything from inside of Dispatch Message
      * ========================================================================
      */
     
     /* PDO (7.2.2), MPDO (7.2.3) --------------------------------------------*/
     
-    int ConsumePdo (int pdoNum, char * dataIn);
-    int HandlePdoReadRequest (int pdoNum);
+    int HandlePdo (CanOpenMessage *canOpenMsg);
     
     
     /* SDO (7.2.4) ----------------------------------------------------------*/
+    int HandleSdo (CanOpenMessage *canOpenMsg);
     
     int HandleExpeditedDownload (int sdoNum, int index, int subIndex, int dataCount, char * data);
     int HandleInitiateDownloadRequest (int sdoNum, int index, int subIndex, int dataCount, char * data);
@@ -129,12 +149,10 @@
     // TODO: express and not express
     
     /* SYNC object (7.2.5) --------------------------------------------------*/
-    
-    // TODO: SYNC consumer 
+    int HandleSync (CanOpenMessage *canOpenMsg);
     
     /* Time Stamp object (7.2.6) --------------------------------------------*/
-    
-    // TODO: time consumer
+    /* Handled all inside of the ServiceProvider */
     
     /* Emergency object (7.2.7) ---------------------------------------------*/
     
@@ -143,7 +161,7 @@
     
     /* Network Management (7.2.8) -------------------------------------------*/
     /* ---- Node Control (7.2.8.2.1) ----------------------------------------*/
-    int HandleNodeControl (int commandSpecifier);
+    int HandleNodeControl (CanOpenMessage *canOpenMsg);
 
 
     /* ---- Error Control (7.2.8.2.2) ---------------------------------------*/
@@ -151,17 +169,34 @@
     int HandleNodeGuardRequest (int masterId);
     int ConsumeHeartbeat (int producerId);
     
-    
+protected:
+
     /* ========================================================================
-     * Methods to implement node control in derived classes
+     * Protected Methods to implement node application in derived classes
      * ========================================================================
      */
      
-     /** Perform actions every cycle
+     /** Perform actions every cycle (when in operational mode)
+       * @note Override to implement user application
+       * @note The OnTick function should avoid modifying data in the object
+       * dictionary unless necessary, because there will almost certainly be 
+       * a race condition.
+       */
+     virtual void OnFixedUpdate(void) = 0;
+     
+     
+     
+     virtual void OnUpdate(void) = 0;
+    
+    
+    /* SYNC -----------------------------------------------------------------*/
+    /** Perform actions when state changed to stop
       * @note Override to implement user application
       */
-     virtual void OnUpdate(void) = 0;
+    virtual void OnSync (uint8_t counter) = 0;
     
+    
+    /* NMT Node Control -----------------------------------------------------*/
     /** Perform actions when node reset
       * @note Override to implement user application
       */
@@ -182,36 +217,61 @@
       */
     virtual void OnStopped (void) = 0;
     
+    /* Object Dictionary Handling -------------------------------------------*/
+    /** Abstract method to give access to the object entries of derived
+      * classes
+      */
+    virtual ObjectData * ScanIndex(IndexSize index) = 0;
+
+
     /* ========================================================================
-     * Protected Members
+     * Protected Methods to Post Messages
      * ========================================================================
      */
+     
+    int PostTPDO (int cobId);
 
-    /** object dictionary */
-    ObjectDictionary * pMyDictionary;
 
-private:
+    /* ========================================================================
+     * Protected Member Variables
+     * ========================================================================
+     */
+     
+    /* Object Dictionary Handling -------------------------------------------*/
+    /** Array of ObjectData to contain all of the object dictionary data */
+    ObjectData *dictionary;
+    
+    /* Elapsed Time ---------------------------------------------------------*/
+    /** Millisecond time stamp at this OnTick() */
+    uint32_t timeCurrentTick;
+    
+    /** Millisecond difference between current OnTick() and the previous */
+    uint32_t timeSinceLastTick;
+
+// should be private
+// TODO: implement functions in Node class to post all types of messages so
+//       derived classes do not have to consider message structure.
+protected:
 
     /* ========================================================================
      * Private members
      * ========================================================================
      */
      
-     
-    /** 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 ------------------------------------------------------*/
+    /** Reference to Service Provider that this node is attached to */
     ServiceProvider * pMyProvider;
-     
-    /** Network id for the node
-      * @note May need to be a pointer, because node id is held in object dictionary
-      */
-    int nodeId;
+
+private:  
+    /* Common Node Methods --------------------------------------------------*/
+    /** Change state of the node */
+    void ChangeState(int newState);
     
-    /** Network and communication state of the node
-      * @note
-      */
-    State state;     
+    /* Common Node properties -----------------------------------------------*/    
+    /** Network and communication state of the node */
+    State state;
+    
+    
 };
 
 } /* namespace ppCANOpen */

diff -r 2034b04c86d2 -r 22a337cdc0e3 include/ObjectDictionary.h
--- a/include/ObjectDictionary.h	Sat Jan 09 17:15:29 2016 +0000
+++ b/include/ObjectDictionary.h	Sat Feb 13 20:22:59 2016 +0000
@@ -34,27 +34,21 @@
 namespace ppCANOpen
 {
 
-/** Abstract Object Dictionary class to provide constants and virtual scan
-  * function.
-  */
-class ObjectDictionary
+/** Data Type alias to provide context when defining the object library */
+typedef uint8_t DataType;
+
+/** Data Property alias to provide context when defining the object library */
+typedef uint8_t DataProperty;
+
+/** defines the data in a single entry of data (subindex)
+ */
+struct EntryData
 {
-    
-public:
-
-    /*=========================================================================
-     * Constants wrapped into the Object Dictionary to use with construction
-     *=========================================================================
-     */
-     
     /* Data value type constants ----------------------------------------------
      * Taken from the CANOpen Standard.  they are part of the object dictionary
      * at indices 0x0001 to 0x0023.  Given to every Sub Index.
      */
      
-    /** Data Type alias to provide context when defining the object library */
-    typedef uint8_t DataType;
-     
     static const DataType TYPE_BOOLEAN                     = 0x01;
     static const DataType TYPE_INT8                        = 0x02;
     static const DataType TYPE_INT16                       = 0x03;
@@ -94,69 +88,46 @@
      * Can be strung together into a mask.
      */
      
-    /** Data Property alias to provide context when defining the object library */
-    typedef uint8_t DataProperty;
-    
     static const DataProperty PROPERTY_READABLE         = 0x01;
     static const DataProperty PROPERTY_WRITEABLE        = 0x02;
     static const DataProperty PROPERTY_READ_WRITEABLE   = 0x03;
     /* static const uint8_t PROPERTY_STATIC                = 0x04 *//* possible to save setting after powerdown */
     
-    
-    /* Index Constants --------------------------------------------------------
+    /* EntryData data --------------------------------------------------------
      */
-     
-    /** Index alias to provide context when defining the object library */
-    typedef uint8_t Index;
+    
+    void           *pData;
+    uint16_t        size;
+    DataType        type;
+    DataProperty    properties;
+    
+    EntryData(){}
+    
+    EntryData(void *d, uint16_t s, DataType t, DataProperty p)
+    : pData(d), size(s), type(t), properties(p)
+    {}
+};
 
 
-    /*=========================================================================
-     * Internal structures
-     *=========================================================================
-     */
-     
-    /** defines the data in a single entry of data (subindex)
-     */
-    struct EntryData
-    {           
-        void           *pData;
-        uint16_t        size;
-        DataType        type;
-        DataProperty    properties;
-        
-        EntryData(){}
-        
-        EntryData(void *d, uint16_t s, DataType t, DataProperty p)
-        : pData(d), size(s), type(t), properties(p)
-        {}
-    };
+/** Index alias to provide context when defining the object library */
+typedef uint16_t IndexSize;
+
+/** Sub-Index alias to provide context when defining the object library */
+typedef uint8_t SubIndexSize;
+
+/** defines the data for a single index, including an array of subindices
+ */
+struct ObjectData
+{                
+    EntryData       *entries;
+    IndexSize        index;
+    SubIndexSize     entryCount;
     
-    /** defines the data for a single index, including an array of subindices
-     */
-    struct ObjectData
-    {        
-        EntryData  *entries;
-        uint16_t    index;
-        uint16_t    entryCount;
-        
-        ObjectData(){}
-        
-        ObjectData(EntryData *e, uint16_t i, uint16_t c)
-        : entries(e), index(i), entryCount(c)
-        {}
-    };
-
-
-    /*=========================================================================
-     * Public Methods
-     *=========================================================================
-     */
-     
-    /** Abstract method to give access to the object entries of derived
-      * classes
-      */
-    virtual ObjectData * ScanIndex(int index) = 0;
+    ObjectData(){}
     
+    ObjectData(EntryData *e, IndexSize i, SubIndexSize c)
+    : entries(e), index(i), entryCount(c)
+    {}
 };
 
 } /* namespace ppCANOpen */

diff -r 2034b04c86d2 -r 22a337cdc0e3 include/ServiceProvider.h
--- a/include/ServiceProvider.h	Sat Jan 09 17:15:29 2016 +0000
+++ b/include/ServiceProvider.h	Sat Feb 13 20:22:59 2016 +0000
@@ -51,65 +51,117 @@
 {
 public:
 
-    
-
     ServiceProvider (void);
     ~ServiceProvider (void);
     
+    /* ========================================================================
+     * Public Methods
+     * ========================================================================
+     */
+    
+    /* Main application -----------------------------------------------------*/
     /** Main loop */
     void Run (void);
     
-    /** Register a node to get messages and get update calls
-      * @note
-      * @param
-      * @retval
-      */
+    /** Iterate through Nodes and update */
+    void UpdateNodes(void);
+    
+    /** C helper function to pass to API */
+    static void UpdateNodes_CWrapper(void *pServiceObject);
+    
+    /* Node Management ------------------------------------------------------*/
+    /** Register a node to get messages and get update calls */
     int AddNode (Node * node);
     
-    /** Register a node to get messages and get update calls
-      * @note
-      * @param
-      * @retval
-      */
+    // TODO: remove node (swap pointers to fill in nicely
+    
       
-    /** Add a message to the message queue outbox
-      * @note
-      * @param
+    /* CanMessage Transmission ----------------------------------------------*/
+    /** Add a message to the message queue outbox */
+    void PostMessage (int nodeId, CanOpenMessage * msg);
+    
+    /** quick way to create and send an NMT Control message
+      * @note Make posting a message easier! 
       */
-    void PostMessage (CanOpenMessage * msg);
+    void PostNmtControl (char targetNodeId, NmtCommandSpecifier cs);
+    
+    /* Elapsed Time ---------------------------------------------------------*/
+    /** A way for the the Nodes to get elapsed time when they want it */
+    uint32_t GetTime(void);
     
-    // TODO: remove node (swap pointers to fill in nicely
 private:
+    /* ========================================================================
+     * Private Constants
+     * ========================================================================
+     */
     
-    /** define size of arrays in one place */
+    /** Define size of arrays in one place */
     static const int SERVICE_MAX_NODES = 8;    
 
     /* ========================================================================
-     * Private Members
+     * Private Methods
      * ========================================================================
      */
 
-    /** 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.
+    /* CanMessage Reading ---------------------------------------------------*/
+    /** Function to be called on every CAN read interrupt */
+    void ReadIT(void);
+    
+    /** C helper function used to pass ReadIT() to API */
+    static void ReadIT_CWrapper(void *pServiceObject);
+    
+    /** Helper function that utilizes backup message inbox when the main one
+      * is locked by the application.
       */
-    std::queue<CanOpenMessage> outbox;
-    //CanOpenMessage outbox[SERVICE_MAX_NODES * 2];
-    //int messageCount;
+    void ReadIT_clearBuffer(void);
 
-    /** array of nodes to cycle through.
-     *  @note
+    /* Elapsed Time ---------------------------------------------------------*/
+    /** Synchronize elapsed time*/
+    void HandleTimeMessage(CanOpenMessage *canOpenMsg); 
+    
+    
+    /* ========================================================================
+     * Private Member Variables
+     * ========================================================================
      */
+     
+    /* Node Management -------------------------------------------------------*/
+    /** array of nodes to cycle through. */
     Node * nodes[SERVICE_MAX_NODES];
     
-    /** Keeps count of current number of nodes.
-      * @note
+    /** Keeps count of current number of nodes. */
+    int nodeCount;    
+    
+    /* CanMessage Reading ---------------------------------------------------*/
+    /** array of messages in a queue */
+    std::queue<CanOpenMessage> inbox;
+    
+    /** Extra buffer only handled by ISR. Used if inbox is locked */
+    std::queue<CanOpenMessage> inboxBuffer; 
+    
+    /** boolean to lock and unlock inbox  */
+    int bInboxUnlocked;
+    
+    /** Data to keep track of internal messages.  Node ID allows SP to avoid
+      * dispatching message to the one who sent it
       */
-    int nodeCount;
+    struct InternalMessage {
+        int             nodeId;
+        CanOpenMessage  message;
+        
+        InternalMessage(uint8_t n, CanOpenMessage m):
+            nodeId(n),
+            message(m)
+        {}
+    };
+
+    /** array of messages in a queue */
+    std::queue<InternalMessage> outbox; 
     
-    /** stores handle for can open api */
-    CanOpenHandle * hCanOpen;
-
+    /* Elapsed Time ---------------------------------------------------------*/   
+    /** Elapsed time offset used for synchronization*/
+    int32_t elapsedTimeOffset; 
+    
 };    
     
 } /* namspace ppCANOpen */

diff -r 2034b04c86d2 -r 22a337cdc0e3 include/canopen_api.h
--- a/include/canopen_api.h	Sat Jan 09 17:15:29 2016 +0000
+++ b/include/canopen_api.h	Sat Feb 13 20:22:59 2016 +0000
@@ -29,6 +29,8 @@
 #ifndef PPCAN_CANOPEN_API_H
 #define PPCAN_CANOPEN_API_H
 
+#include "stdint.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -36,8 +38,7 @@
     /*=========================================================================
      * Forward declarations
      *=========================================================================
-     */  
-    struct CanOpenHandle;
+     */
     struct CanOpenMessage;
 
     /*=========================================================================
@@ -45,10 +46,15 @@
      *=========================================================================
      */    
     
-    int CanOpenApiInit  (CanOpenHandle * hCanOpen);
+    typedef void (* ServiceProviderRxInterruptCallback)(void *pServiceObject);
+    typedef void (* ServiceProviderTickCallback)       (void *pServiceObject);
+    
+    int CanOpenApiInit  (void *object, ServiceProviderRxInterruptCallback rxCallback, ServiceProviderTickCallback tickCallback);
     
-    int CanOpenApiRead  (CanOpenHandle * hCanOpen, CanOpenMessage * canOpenMsg);
-    int CanOpenApiWrite (CanOpenHandle * hCanOpen, CanOpenMessage * canOpenMsg);
+    int CanOpenApiRead  (CanOpenMessage * canOpenMsg);
+    int CanOpenApiWrite (CanOpenMessage * canOpenMsg);
+    
+    uint32_t CanOpenApiGetHardwareTime  (void);
 
 #ifdef __cplusplus
 };

diff -r 2034b04c86d2 -r 22a337cdc0e3 source/Node.cpp
--- a/source/Node.cpp	Sat Jan 09 17:15:29 2016 +0000
+++ b/source/Node.cpp	Sat Feb 13 20:22:59 2016 +0000
@@ -36,84 +36,17 @@
 
 namespace ppCANOpen
 {
-    
 
-    /** 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)
+Node::Node (int id, ServiceProvider * pProvider, int bLoop)
+{
+    nodeId = id;
 
-    /*=========================================================================
-     * 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
-     *=========================================================================
-     */
+    state.nmtState = State::INITIALIZED;
 
-    /** 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;
+    bLoopbackOn = bLoop;
 
-Node::Node (ServiceProvider * pProvider)
-{
     pMyProvider = pProvider;
-    pProvider->AddNode(this);
+    pProvider->AddNode(this);    
 }
 
 /*=============================================================================
@@ -121,29 +54,41 @@
  *=============================================================================
  */
 
-int Node::DispatchMessage(CanOpenMessage *msg)
+int Node::DispatchMessage(CanOpenMessage *canOpenMsg)
 {
-    int command = MESSAGE_GET_COMMAND(msg->id);
-    int nodeId  = MESSAGE_GET_NODEID(msg->id);
+    int command = MESSAGE_GET_COMMAND(canOpenMsg->id);
 
-    //printf("*** N.Dispatch: got com and id\r\n");
-    
     switch (command) {
         case CANOPEN_FUNCTION_CODE_NMT:
-            printf("*** N.Dispatch: it's an NMT Control!!!\r\n");
-            if (msg->data[0] == nodeId) {
-                HandleNodeControl ((int)msg->data[1]);
+            printf("   NMT Control:   \r\n");
+            HandleNodeControl(canOpenMsg);
+            break;
+
+        case CANOPEN_FUNCTION_CODE_PDO1T:
+        case CANOPEN_FUNCTION_CODE_PDO1R:
+        case CANOPEN_FUNCTION_CODE_PDO2T:
+        case CANOPEN_FUNCTION_CODE_PDO2R:
+        case CANOPEN_FUNCTION_CODE_PDO3T:
+        case CANOPEN_FUNCTION_CODE_PDO3R:
+        case CANOPEN_FUNCTION_CODE_PDO4T:
+        case CANOPEN_FUNCTION_CODE_PDO4R:
+            if (state.bPDO) {
+                HandlePdo(canOpenMsg);
             }
             break;
+
+        case CANOPEN_FUNCTION_CODE_SDOT:
+        case CANOPEN_FUNCTION_CODE_SDOR:
+            if (state.bSDO) {
+                HandleSdo(canOpenMsg);
+            }
+            break;
+
         default:
-            printf("*** N.Dispatch: some random message\r\n");
+            printf("   some random message\r\n");
             break;
     }
 
-    // ECHO ***********************
-    pMyProvider->PostMessage(msg);
-    // END ECHO *******************
-
     return 1;
 }
 
@@ -152,14 +97,191 @@
  *=============================================================================
  */
 
-int Node::ConsumePdo (const int pdoNum, char *const data)
+static void CopyBits(uint8_t *sourceData, 
+                uint8_t *destData,
+                uint8_t mappedBits,
+                uint8_t &sourceBitNum, 
+                uint8_t &destBitNum);
+
+int Node::HandlePdo (CanOpenMessage *canOpenMsg)
 {
-    return 0;
+    printf("   RPDO:\r\n");
+    
+    int result = 0;
+
+    if (CANOPEN_TYPE_REMOTE == canOpenMsg->type) {
+
+    } else {
+        int pdoNum = 0;
+        int bSearching = 1;
+
+        ObjectData * parameterObject;
+
+        /* search through PDO parameters until we find a match */
+        while (bSearching) {
+
+            //printf("      PDO %d?", pdoNum);
+            parameterObject = ScanIndex (0x1400 + pdoNum);
+            if (parameterObject) {
+
+                uint32_t *cobId = (uint32_t*) parameterObject->entries[1].pData;
+                if (canOpenMsg->id == *cobId) {
+                    bSearching = 0;
+                    
+                    //printf(" ... match!!!\r\n");
+                } else {
+                    pdoNum++;
+                    //printf(" ... nope\r\n");
+                }
+
+            } else {
+                bSearching = 0;
+                printf("\r\n      ERROR: No PDO Parameter match found\r\n");
+            }
+            
+            
+        }
+
+        /* if a matching parameter object was found then get mapping */
+        if (parameterObject) {
+            
+            ObjectData  *mappingObject = ScanIndex (0x1600 + pdoNum);
+            
+            if (mappingObject) {
+                SubIndexSize *numMappedVariables = (SubIndexSize*) mappingObject->entries[0].pData;
+                
+                /* variable to track position in data copy */
+                uint8_t sourceBitNum  = 0;
+                
+                int bError = 0;
+                
+                /* for each mapped variable, write in data from message */
+                for (SubIndexSize subIndexIterator = 1; subIndexIterator <= *numMappedVariables && !bError; subIndexIterator++) {
+                
+                    uint32_t *map = (uint32_t*)mappingObject->entries[subIndexIterator].pData;
+                    
+                    uint8_t         mappedBits     = (uint8_t)      (*map); 
+                    uint8_t         mappedBytes    = mappedBits / 8;
+                    SubIndexSize    mappedSubIndex = (SubIndexSize) (*map >> 8);
+                    IndexSize       mappedIndex    = (IndexSize)    (*map >> 16);
+                    
+                    printf("      mapped: %#06x, %#04x, %d\r\n",
+                            mappedIndex, mappedSubIndex, mappedBits);
+                    
+                    /* if less than 0x1000 then it is a dummy value */
+                    if (mappedIndex < 0x1000) {
+                        sourceBitNum += mappedBits;
+                    } else {
+                
+                        /* get the index object */
+                        ObjectData  *mappedObject = ScanIndex (mappedIndex);
+                        if (mappedObject) {
+                            
+                            /* get the subindex object */
+                            if (mappedSubIndex <= *(SubIndexSize*) mappedObject->entries[0].pData) {
+                                
+                                EntryData *destinationEntry = &mappedObject->entries[mappedSubIndex];                            
+                                
+                                /* can we write to it? */
+                                if (!(destinationEntry->properties & EntryData::PROPERTY_WRITEABLE)) {
+                                    printf("      ERROR: Mapped SubIndex is not writeable!\r\n"); 
+                                    bError = 1;
+                                }
+                                
+                                uint8_t sourceByteNum = sourceBitNum / 8;
+                                if (sourceByteNum + mappedBytes > canOpenMsg->dataCount) {
+                                    printf("      ERROR: Insufficient mapped data remaining!\r\n"); 
+                                    bError = 1;
+                                }
+                                
+                                if (destinationEntry->size < mappedBytes) {
+                                    printf("      ERROR: Too much data to pack into destination!\r\n"); 
+                                    bError = 1;
+                                }
+                                
+                                if (!bError) {
+                                    
+                                    //printf("      No Errors, copying data...\r\n");
+                                    
+                                    uint8_t* destData = (uint8_t*) destinationEntry->pData;
+                                    uint8_t* sourceData = canOpenMsg->data;
+                                    
+                                    uint8_t destBitNum = 0;
+                                    CopyBits(sourceData, destData, mappedBits, sourceBitNum, destBitNum);
+                                }                         
+                                
+                            } else {
+                                printf("      ERROR: Mapped SubIndex does not exist!\r\n"); 
+                                    bError = 1;
+                            }
+                            
+                        } else {
+                            printf("      ERROR: Mapped Index does not exist!\r\n"); 
+                                    bError = 1;
+                        }
+                    
+                    } /* if mappedIndex < 0x1000 */
+                    
+                               
+                }   /* for each mapping */
+                
+                if (!bError) {
+                    result = 1;
+                }
+                
+            } else {
+                printf("      ERROR: No PDO Mapping match found\r\n");
+            }
+            
+        } /* if parameter exists */
+        
+    } /* if remote message */
+
+    return result;
 }
 
-int Node::HandlePdoReadRequest (const int pdoNum)
+
+int Node::HandleSdo (CanOpenMessage *canOpenMsg)
 {
-    return 0;
+    
+    printf("   SDO:\r\n");
+    
+    int result = 0;
+    
+    int bSearching = 1;
+    
+    int sdoNum = 0;
+    int sdoType = 0;  /* 0=unknown, 1=receive/server, 2=transmit/client */
+    ObjectData * sdoObject;
+    
+    /* search through PDO parameters until we find a match */
+    while (bSearching) {
+
+        sdoObject = ScanIndex (0x1200 + sdoNum);
+        if (sdoObject) {
+            
+            uint32_t *receiveCobId  = (uint32_t*) sdoObject->entries[1].pData;
+            uint32_t *transmitCobId = (uint32_t*) sdoObject->entries[2].pData;
+            
+            if (canOpenMsg->id == *receiveCobId) {
+                bSearching = 0;
+                sdoType = 1;
+                printf("      Receive SDO\r\n");
+            } else if (canOpenMsg->id == *transmitCobId) {
+                bSearching = 0;
+                sdoType = 2;
+                printf("      Transmit SDO\r\n");
+            }
+            
+        } else {
+            printf("      ERROR: No SDO parameters found");
+            bSearching = 0;
+        }
+        
+        sdoNum++;
+    }
+    
+    return result;
 }
 
 int Node::ConsumeEmergency (void)
@@ -167,84 +289,109 @@
     return 0;
 }
 
-int Node::HandleNodeControl (int commandSpecifier)
+int Node::HandleNodeControl (CanOpenMessage *canOpenMsg)
 {
     int result = 0;
 
-    switch (commandSpecifier) {
-        case NMT_CS_START:
-            if (State::INITIALIZED != state.nmtState) {
-                state.nmtState = State::OPERATIONAL;
+    if (canOpenMsg->data[0] == nodeId) {
+
+        int commandSpecifier = (int)canOpenMsg->data[1];
+
+        switch (commandSpecifier) {
+            case NMT_CS_START:
+                printf("      NMT_CS_START\r\n");
+                if ((State::INITIALIZED    == state.nmtState) || 
+                    (State::PREOPERATIONAL == state.nmtState) || 
+                    (State::STOPPED        == state.nmtState)) 
+                {
+
+                    state.nmtState = State::OPERATIONAL;
+                    state.bBoot         = 0;
+                    state.bSDO          = 1;
+                    state.bEmergency    = 1;
+                    state.bSYNC         = 1;
+                    state.bLifeGuard    = 1;
+                    state.bPDO          = 1;
+                    state.bLSS          = 0;
+                    OnOperational();
+                    result = 1;
+                }
+                break;
+
+            case NMT_CS_STOP:
+                if (State::INITIALIZED != state.nmtState) {
+                    state.nmtState = State::STOPPED;
+                    state.bBoot         = 0;
+                    state.bSDO          = 0;
+                    state.bEmergency    = 0;
+                    state.bSYNC         = 0;
+                    state.bLifeGuard    = 1;
+                    state.bPDO          = 0;
+                    state.bLSS          = 1;
+                }
+                OnStopped();
+                result = 1;
+                break;
+
+            case NMT_CS_ENTER_PREOP:
+                state.nmtState = State::PREOPERATIONAL;
                 state.bBoot         = 0;
                 state.bSDO          = 1;
                 state.bEmergency    = 1;
                 state.bSYNC         = 1;
                 state.bLifeGuard    = 1;
-                state.bPDO          = 1;
-                state.bLSS          = 0;
-                OnOperational();
+                state.bPDO          = 0;
+                state.bLSS          = 1;
+                OnPreoperational();
                 result = 1;
-            }
-            break;
+                break;
 
-        case NMT_CS_STOP:
-            if (State::INITIALIZED != state.nmtState) {
-                state.nmtState = State::STOPPED;
-                state.bBoot         = 0;
+            case NMT_CS_RESET_NODE:
+            case NMT_CS_RESET_COM:
+                printf("      NMT_CS_RESET\r\n");
+
+                state.nmtState      = State::INITIALIZED;
+                state.bBoot         = 1;
                 state.bSDO          = 0;
                 state.bEmergency    = 0;
                 state.bSYNC         = 0;
+                state.bLifeGuard    = 0;
+                state.bPDO          = 0;
+                state.bLSS          = 0;
+
+                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(nodeId, &msgBoot);
+                
+                OnInitialize();
+
+                state.nmtState = State::PREOPERATIONAL;
+                state.bBoot         = 0;
+                state.bSDO          = 1;
+                state.bEmergency    = 1;
+                state.bSYNC         = 1;
                 state.bLifeGuard    = 1;
                 state.bPDO          = 0;
                 state.bLSS          = 1;
-            }
-            OnStopped();
-            result = 1;
-            break;
-
-        case NMT_CS_ENTER_PREOP:
-            state.nmtState = State::PREOPERATIONAL;
-            state.bBoot         = 0;
-            state.bSDO          = 1;
-            state.bEmergency    = 1;
-            state.bSYNC         = 1;
-            state.bLifeGuard    = 1;
-            state.bPDO          = 0;
-            state.bLSS          = 1;
-            OnPreoperational();
-            result = 1;
-            break;
+                
+                OnPreoperational();
 
-        case NMT_CS_RESET_NODE:
-        case NMT_CS_RESET_COM:
-            
-            state.nmtState      = State::INITIALIZED;
-            state.bBoot         = 1;
-            state.bSDO          = 0;
-            state.bEmergency    = 0;
-            state.bSYNC         = 0;
-            state.bLifeGuard    = 0;
-            state.bPDO          = 0;
-            state.bLSS          = 0;
-            
-            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;
+                result = 1;
+                break;
 
-        default:
-            break;
+            default:
+                break;
+        }
+
     }
 
     return result;
@@ -265,29 +412,256 @@
  *=============================================================================
  */
 
+void Node::FixedUpdate (uint32_t time)
+{
+    timeSinceLastTick = time - timeCurrentTick;
+    timeCurrentTick = time; 
+    
+    if (State::OPERATIONAL == state.nmtState) {
+        OnFixedUpdate();
+    }
+       
+}
+
 void Node::Update (void)
 {
-    if (State::OPERATIONAL == state.nmtState) {
+     if (State::OPERATIONAL == state.nmtState) {
         OnUpdate();
     }
-    
-    // TODO: check elapsed time to see if it is time for fixed update
 }
 
 /*=============================================================================
- * Methods to implement node control in derived classes
+ * Other Member Functions
  *=============================================================================
  */
 
+int Node::PostTPDO (int specifiedCobId)
+{
+    printf("   TPDO:\r\n");
+    
+    int result = 0;
+    
+    /* initialize a blank message -------------------------------------------*/
+    CanOpenMessage msg;
+
+    msg.id          = specifiedCobId;
+    msg.dataCount   = 0;
+    msg.type        = CANOPEN_TYPE_DATA;
+    msg.format      = CANOPEN_FORMAT_STANDARD;
+
+    msg.data[0] = 0;
+    msg.data[1] = 0;
+    msg.data[2] = 0;
+    msg.data[3] = 0;
+    msg.data[4] = 0;
+    msg.data[5] = 0;
+    msg.data[6] = 0;
+    msg.data[7] = 0;
+    
+    int pdoNum = 0;
+    int bSearching = 1;
+    
+    /* Find the mapped data and send ----------------------------------------*/
+    
+    ObjectData * parameterObject;
+    
+    /* search through PDO parameters until we find a match */
+    while (bSearching) {
+    
+        //printf("      PDO %d?", pdoNum);
+        parameterObject = ScanIndex (0x1800 + pdoNum);
+        if (parameterObject) {
+    
+            uint32_t *cobId = (uint32_t*) parameterObject->entries[1].pData;
+            if (specifiedCobId == *cobId) {
+                bSearching = 0;
+                
+                //printf(" ... match!!!\r\n");
+            } else {
+                pdoNum++;
+                //printf(" ... nope\r\n");
+            }
+    
+        } else {
+            bSearching = 0;
+            printf("\r\n      ERROR: No PDO Parameter match found\r\n");
+        }
+    }
+    
+    /* if a matching parameter object was found then get mapping */
+    if (parameterObject) {
+        
+        ObjectData  *mappingObject = ScanIndex (0x1A00 + pdoNum);
+        
+        if (mappingObject) {
+            SubIndexSize *numMappedVariables = (SubIndexSize*) mappingObject->entries[0].pData;
+            
+            //printf("      numMappedVariables:    %d\r\n", *numMappedVariables);
+            
+            /* variable to track position in data copy */
+            uint8_t destBitNum  = 0;
+            
+            int bError = 0;
+            
+            /* for each mapped variable, write in data from message */
+            for (SubIndexSize subIndexIterator = 1; subIndexIterator <= *numMappedVariables && !bError; subIndexIterator++) {
+            
+                uint32_t *map = (uint32_t*)mappingObject->entries[subIndexIterator].pData;
+                
+                uint8_t         mappedBits     = (uint8_t)      (*map); 
+                uint8_t         mappedBytes    = mappedBits / 8;
+                SubIndexSize    mappedSubIndex = (SubIndexSize) (*map >> 8);
+                IndexSize       mappedIndex    = (IndexSize)    (*map >> 16);
+                
+                msg.dataCount += mappedBytes;
+                
+                //printf("      mapped: %#06x, %#04x, %d\r\n",
+                //        mappedIndex, mappedSubIndex, mappedBits);
+                
+                /* if less than 0x1000 then it is a dummy value */
+                if (mappedIndex < 0x1000) {
+                    destBitNum += mappedBits;
+                    //printf("      No Errors, skipping VOID data...\r\n");
+                } else {
+                
+                    /* push into the TPDO data */
+                    ObjectData  *mappedObject = ScanIndex (mappedIndex);
+                    if (mappedObject) {
+                        
+                        /* get the subindex object */
+                        if (mappedSubIndex <= *(SubIndexSize*) mappedObject->entries[0].pData) {
+                            
+                            EntryData *sourceEntry = &mappedObject->entries[mappedSubIndex];                            
+                            
+                            /* can we write to it? */
+                            if (!(sourceEntry->properties & EntryData::PROPERTY_READABLE)) {
+                                printf("      ERROR: Mapped SubIndex is not readable!\r\n"); 
+                                bError = 1;
+                            }
+                            
+                            uint8_t destByteNum = destBitNum / 8;
+                            if (destByteNum + mappedBytes > 8) {
+                                printf("      ERROR: Too much data to pack into destination!\r\n"); 
+                                bError = 1;
+                            }
+                            
+                            if (sourceEntry->size < mappedBytes) {
+                                printf("      ERROR: trying to grab too much information!\r\n"); 
+                                bError = 1;
+                            }
+                            
+                            if (!bError) {
+                                
+                                //printf("      No Errors, copying data...\r\n");
+                                
+                                uint8_t *destData   = msg.data;
+                                uint8_t *sourceData = (uint8_t*)sourceEntry->pData;
+                                
+                                uint8_t sourceBitNum = 0;
+                                CopyBits(sourceData, destData, mappedBits, sourceBitNum, destBitNum);
+                            }                         
+                            
+                        } else {
+                            printf("      ERROR: Mapped SubIndex does not exist!\r\n"); 
+                                bError = 1;
+                        }
+                        
+                    } else {
+                        printf("      ERROR: Mapped Index does not exist!\r\n"); 
+                                bError = 1;
+                    }
+                    
+                } /* if mappedIndex < 0x1000 */
+                
+                           
+            }   /* for each mapping */
+            
+            if (!bError) {
+                result = 1;
+                
+                /* Send the message we built up */
+                pMyProvider->PostMessage(nodeId, &msg);
+            }
+            
+        } else {
+            printf("      ERROR: No PDO Mapping match found\r\n");
+        } /* if mappingObject exists */
+        
+    } /* if parameter exists */
+
+    
+    return result;
+}
+
+/*=============================================================================
+ * Private functions
+ *=============================================================================
+ */
+
+void ChangeState(int newState) {
+    
+}
+
 /*=============================================================================
  * Local functions
  *=============================================================================
  */
 
+void CopyBits(uint8_t *sourceData, 
+                uint8_t *destData,
+                uint8_t mappedBits,
+                uint8_t &sourceBitNum, 
+                uint8_t &destBitNum)
+{    
+    uint8_t sourceByteNum;
+    uint8_t destByteNum;
 
+    uint8_t bitCounter = 0;
+
+    if ((mappedBits % 8) == 0 && (sourceBitNum % 8) == 0 && (destBitNum % 8) == 0) {
+                             
+        //printf("         Loading BYTEwise...\r\n");       
+        /* load in by bytes */
+        uint8_t destByteNum = 0;
+        while (bitCounter < mappedBits) {
+            
+            
+            destByteNum   = destBitNum   / 8;
+            sourceByteNum = sourceBitNum / 8; 
+            
+                destData[destByteNum] = sourceData[sourceByteNum];
+                
+            destBitNum   += 8;
+            sourceBitNum += 8;
+            bitCounter   += 8;
+        }
+        
+    } else {
+         
+        
+        //printf("         Loading BITwise..."); 
+        /* not a multiple of 8, so do bit by bit */
+        while (bitCounter < mappedBits) { 
+        
+            destByteNum   = destBitNum   / 8;
+            sourceByteNum = sourceBitNum / 8; 
+             
+                /* clear the destination bit */
+                destData[destByteNum] &= ~(1 << destBitNum); 
+                /* get source bit value */
+                uint8_t destValue = (sourceData[sourceByteNum] & (1 << (sourceBitNum % 8))) >> (sourceBitNum % 8) << destBitNum;
+                /* set dest bit */
+                destData[destByteNum] |= destValue;      
+            
+            destBitNum++;
+            sourceBitNum++;  
+            bitCounter++;                    
+        }
+    }        
+}
 
 /*=============================================================================
- *============================================================================= 
+ *=============================================================================
  * Methods to handle message requests and responses
  * Called by the node, usually during Update() or during handling of
  * incoming messages.
@@ -303,45 +677,45 @@
  *  @note
  *  @param
  */
-void RequestPdo (int pdoNum){}
+void RequestPdo (int pdoNum) {}
 
 /** Build and send a PDO
  *  @note
  *  @param
  */
-void ProducePdo (int pdoNum, char * data){}
+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   
+ *  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){}
+void DownloadSdo (int sdoNum, int index, int subindex, int size, char * data) {}
 
 /** initiate SDO upload
- *  @note 
+ *  @note
  *  @param
  */
-void UploadSdo (int sdoNum, int index, int subindex){}
+void UploadSdo (int sdoNum, int index, int subindex) {}
 
 /** Acknowledge that SDO was recieved properly
- *  @note 
+ *  @note
  *  @param
  */
-void ConfirmSdo (int sdoNum, int bSuccess){}
+void ConfirmSdo (int sdoNum, int bSuccess) {}
 
 /** Abort current SDO transfer
- *  @note 
+ *  @note
  *  @param
  */
-void AbortSdo (int sdoNum){}
+void AbortSdo (int sdoNum) {}
 
 
 /* Emergency object (7.2.7) ---------------------------------------------*/
@@ -356,7 +730,10 @@
  *  @note
  *  @param
  */
-int  SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+int  SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId)
+{
+    return 0;
+}
 
 
 /* ---- Error Control (7.2.8.2.2) ---------------------------------------*/
@@ -365,13 +742,19 @@
  *  @note
  *  @param
  */
-int  RequestErrorControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+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;}
+int  RespondErrorControl (NmtCommandSpecifier cs, unsigned int nodeId)
+{
+    return 0;
+}
 
 
 } /* namspace ppCANOpen */

diff -r 2034b04c86d2 -r 22a337cdc0e3 source/ServiceProvider.cpp
--- a/source/ServiceProvider.cpp	Sat Jan 09 17:15:29 2016 +0000
+++ b/source/ServiceProvider.cpp	Sat Feb 13 20:22:59 2016 +0000
@@ -25,11 +25,11 @@
  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
+#include "mbed.h"
 
 #include "ServiceProvider.h"
 #include "Node.h"
 
-#include "CanOpenHandle.h"
 #include "canopen_api.h"
 
 #include <string.h>
@@ -37,107 +37,233 @@
 
 namespace ppCANOpen
 {
-    
-/*=============================================================================
- * Construction
- *=============================================================================
- */ 
+
+
 
-ServiceProvider::ServiceProvider (void) : nodeCount(0) 
+ServiceProvider::ServiceProvider (void) : nodeCount(0)
 {
-    
-    //messageCount = 0;
-    
+
+    bInboxUnlocked = 1;
+
     //memset (nodes, 0, sizeof(Node) * SERVICE_MAX_NODES ); // TODO: fix incorrect <--
     for (int i = 0; i < SERVICE_MAX_NODES; i++) {
-        nodes[i] = 0;   
+        nodes[i] = 0;
     }
-    
-    hCanOpen = new CanOpenHandle;
-    CanOpenApiInit(hCanOpen);
+
+    CanOpenApiInit((void *)this, &ReadIT_CWrapper, &UpdateNodes_CWrapper);
 }
 
 ServiceProvider::~ServiceProvider (void)
 {
-    delete hCanOpen;   
 }
 
-/*=============================================================================
- * main loop
- *=============================================================================
- */ 
+
+/* ============================================================================
+ * Public Methods
+ * ============================================================================
+ */
+
+
+/* Main application ---------------------------------------------------------*/
 
 void ServiceProvider::Run (void)
 {
-    while (1) {
+    //while (1) {
+
+    
+    /* Check for new messages.  
+     * Dispatch one at a time in order for nodes to handle incoming data 
+     */
+    bInboxUnlocked = 0;
+    if (! inbox.empty()) {
+
+        /* Handle things inside of Service Provider first if necessary */
+        if (CANOPEN_FUNCTION_CODE_TIME == MESSAGE_GET_COMMAND(inbox.front().id)) {
+           
+           HandleTimeMessage(&inbox.front());
+           
+        } else {
+        /* Send out remaining messages to the Nodes */
         
-        /* Check for new messages */
-        CanOpenMessage msg;
-        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);                 
-                }
-                //printf("*** SP.Run: dispatched\r\n");
-            }
-        }    
-   
-        /* update all of the nodes */
-        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");
+            // TODO if time stamp message, then the SP can sync the clock for all nodes
+    
+            /* if new message exists, give it to the nodes*/
+            for (int i=0; i < nodeCount; i++) {
+                nodes[i]->DispatchMessage(&inbox.front());
             }
         }
 
-    }    
+        inbox.pop();
+    }
+    bInboxUnlocked = 1;
+
+    /* Update all of the nodes */
+    for (int i=0; i < nodeCount; i++) {
+        nodes[i]->Update();
+    }
+
+    /* send out the responses */
+    while (! outbox.empty()) {
+        if (CanOpenApiWrite(&outbox.front().message)) {
+            /* send message externally first.
+             * Since we want to keep the message alive and try to  resend, we
+             * do not want to keep resending the message internally over and
+             * over again.*/
+             
+            /* dispatch to the internal nodes */
+            for (int i=0; i < nodeCount; i++) {
+                if ((nodes[i]->bLoopbackOn) || 
+                    (nodes[i]->nodeId != outbox.front().nodeId)) {
+                    
+                    nodes[i]->DispatchMessage(&outbox.front().message);
+                }
+            }
+            
+            outbox.pop();
+        } else {
+            printf("Could not send last message.  Trying again\r\n");
+        }
+    }
+
+    wait (.005);
+
+    //}
 }
 
-/*=============================================================================
- * Register a node to get messages and get update calls
- *=============================================================================
- */ 
+void ServiceProvider::UpdateNodes(void)
+{
+    uint32_t time = GetTime();
+    
+    //printf("S::Run() update nodes\r\n");
+    /* update all of the nodes */
+    for (int i=0; i < nodeCount; i++) {
+        nodes[i]->FixedUpdate(time);
+    }
+}
 
-int ServiceProvider::AddNode (Node * node) 
+void ServiceProvider::UpdateNodes_CWrapper(void *pServiceObject)
+{
+    ServiceProvider * pTempProvider = static_cast <ServiceProvider *> (pServiceObject);
+    pTempProvider->UpdateNodes();
+}
+
+
+/* Node Management ----------------------------------------------------------*/
+
+int ServiceProvider::AddNode (Node * node)
 {
     int result = 1;
-    
+
     if (nodeCount < SERVICE_MAX_NODES) {
         nodes[nodeCount++] = node;
     } else {
         result = 0;
     }
-    
+
     return result;
 }
 
-/* ========================================================================
- * Adds a message to the message queue outbox
- * ========================================================================
+
+/* CanMessage Transmission --------------------------------------------------*/
+
+void ServiceProvider::PostMessage (int nodeId, CanOpenMessage * msg)
+{
+    outbox.push(InternalMessage(nodeId, *msg));
+}
+
+void ServiceProvider::PostNmtControl (char targetNodeId, NmtCommandSpecifier cs)
+{
+    CanOpenMessage msg;
+
+    msg.id          = 0;
+    msg.dataCount   = 2;
+    msg.type        = CANOPEN_TYPE_DATA;
+    msg.format      = CANOPEN_FORMAT_STANDARD;
+
+    // NOTE: memcpy is freezing execution
+    //memcpy(msg.data, canMsg.data, canMsg.len);
+    msg.data[0] = targetNodeId;
+    msg.data[1] = (char) cs;
+    //msg.data[2] =
+    //msg.data[3] =
+    //msg.data[4] =
+    //msg.data[5] =
+    //msg.data[6] =
+    //msg.data[7] =
+
+    PostMessage(0, &msg);
+}
+
+
+/* ============================================================================
+ * Private Methods
+ * ============================================================================
  */
 
-void ServiceProvider::PostMessage (CanOpenMessage * msg)
+
+/* CanMessage Reading -------------------------------------------------------*/
+
+void ServiceProvider::ReadIT(void)
+{
+
+    //printf("S::ReadIT()\r\n");
+
+    CanOpenMessage msg;
+    if(CanOpenApiRead (&msg)) {
+        if (bInboxUnlocked) {
+            ReadIT_clearBuffer();
+            inbox.push(msg);
+        } else {
+            inboxBuffer.push(msg);
+            printf("!!!!!!!!!!!!!!!!!!!!!!!INBOX LOCKED!!!!!!!!!!!!!!!!!!!!!!!!!!!\r\n");
+        }
+    }
+}
+
+void ServiceProvider::ReadIT_clearBuffer(void)
+{
+    while (! inboxBuffer.empty()) {
+        inbox.push (inboxBuffer.front());
+        inboxBuffer.pop();
+    }
+}
+
+void ServiceProvider::ReadIT_CWrapper(void *pServiceObject)
 {
-    outbox.push(*msg);
+    ServiceProvider * pTempProvider = static_cast <ServiceProvider *> (pServiceObject);
+    pTempProvider->ReadIT();
+}
+
+
+/* Elapsed Time -------------------------------------------------------------*/
+
+uint32_t ServiceProvider::GetTime(void)
+{
+    return (CanOpenApiGetHardwareTime() + elapsedTimeOffset) & 0x0FFFFFFF;
 }
 
+void ServiceProvider::HandleTimeMessage(CanOpenMessage *canOpenMsg)
+{
+    if (6 == canOpenMsg->dataCount) {        
+        
+        uint32_t timeStamp_ms = (uint32_t)canOpenMsg->data[0] +
+                                (uint32_t)canOpenMsg->data[1] << 8 +
+                                (uint32_t)canOpenMsg->data[2] << 16 +
+                                (uint32_t)canOpenMsg->data[3] << 24;
+        
+        // Nothing uses Days yet, going to ignore for now.                        
+        // uint32_t timeStamp_days = (uint32_t)canOpenMsg->data[4] +
+        //                           (uint32_t)canOpenMsg->data[5] << 8;
+        
+        uint32_t hwTime = CanOpenApiGetHardwareTime() & 0x0FFFFFFF;
+    
+        elapsedTimeOffset = ((int32_t)timeStamp_ms) - ((int32_t)hwTime);
+        
+    // Else TODO: send error
+    }
+}
+
+
+
 } /* namspace ppCANOpen */
 
-
-
-
-
-
-