Diff: source/ServiceProvider.cpp

Revision:

5:22a337cdc0e3

Parent:

4:2034b04c86d2

--- 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 */
 
-
-
-
-
-
-