ServiceProvider.cpp

00001 /**
00002  ******************************************************************************
00003  * @file
00004  * @author  Paul Paterson
00005  * @version
00006  * @date    2015-12-22
00007  * @brief   CANOpen implementation library
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2015 Paul Paterson
00012  *
00013  * All rights reserved.
00014 
00015  This program is free software: you can redistribute it and/or modify
00016  it under the terms of the GNU General Public License as published by
00017  the Free Software Foundation, either version 3 of the License, or
00018  (at your option) any later version.
00019 
00020  This program is distributed in the hope that it will be useful,
00021  but WITHOUT ANY WARRANTY; without even the implied warranty of
00022  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023  GNU General Public License for more details.
00024 
00025  You should have received a copy of the GNU General Public License
00026  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00027  */
00028 #include "mbed.h"
00029 
00030 #include "ServiceProvider.h"
00031 #include "Node.h"
00032 
00033 #include "canopen_api.h"
00034 
00035 #include <string.h>
00036 #include <stdio.h>
00037 
00038 namespace ppCANOpen
00039 {
00040 
00041 
00042 
00043 ServiceProvider::ServiceProvider (void) : nodeCount(0)
00044 {
00045 
00046     bInboxUnlocked = 1;
00047 
00048     //memset (nodes, 0, sizeof(Node) * SERVICE_MAX_NODES ); // TODO: fix incorrect <--
00049     for (int i = 0; i < SERVICE_MAX_NODES; i++) {
00050         nodes[i] = 0;
00051     }
00052 
00053     CanOpenApiInit((void *)this, &ReadIT_CWrapper, &UpdateNodes_CWrapper);
00054 }
00055 
00056 ServiceProvider::~ServiceProvider (void)
00057 {
00058 }
00059 
00060 
00061 /* ============================================================================
00062  * Public Methods
00063  * ============================================================================
00064  */
00065 
00066 
00067 /* Main application ---------------------------------------------------------*/
00068 
00069 void ServiceProvider::Run (void)
00070 {
00071     //while (1) {
00072 
00073     
00074     /* Check for new messages.  
00075      * Dispatch one at a time in order for nodes to handle incoming data 
00076      */
00077     bInboxUnlocked = 0;
00078     if (! inbox.empty()) {
00079 
00080         /* Handle things inside of Service Provider first if necessary */
00081         if (CANOPEN_FUNCTION_CODE_TIME == MESSAGE_GET_COMMAND(inbox.front().id)) {
00082            
00083            HandleTimeMessage(&inbox.front());
00084            
00085         } else {
00086         /* Send out remaining messages to the Nodes */
00087         
00088             // TODO if time stamp message, then the SP can sync the clock for all nodes
00089     
00090             /* if new message exists, give it to the nodes*/
00091             for (int i=0; i < nodeCount; i++) {
00092                 nodes[i]->DispatchMessage(&inbox.front());
00093             }
00094         }
00095 
00096         inbox.pop();
00097     }
00098     bInboxUnlocked = 1;
00099 
00100     /* Update all of the nodes */
00101     for (int i=0; i < nodeCount; i++) {
00102         nodes[i]->Update();
00103     }
00104 
00105     /* send out the responses */
00106     while (! outbox.empty()) {
00107         if (CanOpenApiWrite(&outbox.front().message)) {
00108             /* send message externally first.
00109              * Since we want to keep the message alive and try to  resend, we
00110              * do not want to keep resending the message internally over and
00111              * over again.*/
00112              
00113             /* dispatch to the internal nodes */
00114             for (int i=0; i < nodeCount; i++) {
00115                 if ((nodes[i]->bLoopbackOn) || 
00116                     (nodes[i]->nodeId != outbox.front().nodeId)) {
00117                     
00118                     nodes[i]->DispatchMessage(&outbox.front().message);
00119                 }
00120             }
00121             
00122             outbox.pop();
00123         } else {
00124             printf("Could not send last message.  Trying again\r\n");
00125         }
00126     }
00127 
00128     wait (.005);
00129 
00130     //}
00131 }
00132 
00133 void ServiceProvider::UpdateNodes(void)
00134 {
00135     uint32_t time = GetTime();
00136     
00137     //printf("S::Run() update nodes\r\n");
00138     /* update all of the nodes */
00139     for (int i=0; i < nodeCount; i++) {
00140         nodes[i]->FixedUpdate(time);
00141     }
00142 }
00143 
00144 void ServiceProvider::UpdateNodes_CWrapper(void *pServiceObject)
00145 {
00146     ServiceProvider * pTempProvider = static_cast <ServiceProvider *> (pServiceObject);
00147     pTempProvider->UpdateNodes();
00148 }
00149 
00150 
00151 /* Node Management ----------------------------------------------------------*/
00152 
00153 int ServiceProvider::AddNode (Node * node)
00154 {
00155     int result = 1;
00156 
00157     if (nodeCount < SERVICE_MAX_NODES) {
00158         nodes[nodeCount++] = node;
00159     } else {
00160         result = 0;
00161     }
00162 
00163     return result;
00164 }
00165 
00166 
00167 /* CanMessage Transmission --------------------------------------------------*/
00168 
00169 void ServiceProvider::PostMessage (int nodeId, CanOpenMessage * msg)
00170 {
00171     outbox.push(InternalMessage(nodeId, *msg));
00172 }
00173 
00174 void ServiceProvider::PostNmtControl (char targetNodeId, NmtCommandSpecifier cs)
00175 {
00176     CanOpenMessage msg;
00177 
00178     msg.id          = 0;
00179     msg.dataCount   = 2;
00180     msg.type        = CANOPEN_TYPE_DATA;
00181     msg.format      = CANOPEN_FORMAT_STANDARD;
00182 
00183     // NOTE: memcpy is freezing execution
00184     //memcpy(msg.data, canMsg.data, canMsg.len);
00185     msg.data[0] = targetNodeId;
00186     msg.data[1] = (char) cs;
00187     //msg.data[2] =
00188     //msg.data[3] =
00189     //msg.data[4] =
00190     //msg.data[5] =
00191     //msg.data[6] =
00192     //msg.data[7] =
00193 
00194     PostMessage(0, &msg);
00195 }
00196 
00197 
00198 /* ============================================================================
00199  * Private Methods
00200  * ============================================================================
00201  */
00202 
00203 
00204 /* CanMessage Reading -------------------------------------------------------*/
00205 
00206 void ServiceProvider::ReadIT(void)
00207 {
00208 
00209     //printf("S::ReadIT()\r\n");
00210 
00211     CanOpenMessage msg;
00212     if(CanOpenApiRead (&msg)) {
00213         if (bInboxUnlocked) {
00214             ReadIT_clearBuffer();
00215             inbox.push(msg);
00216         } else {
00217             inboxBuffer.push(msg);
00218             printf("!!!!!!!!!!!!!!!!!!!!!!!INBOX LOCKED!!!!!!!!!!!!!!!!!!!!!!!!!!!\r\n");
00219         }
00220     }
00221 }
00222 
00223 void ServiceProvider::ReadIT_clearBuffer(void)
00224 {
00225     while (! inboxBuffer.empty()) {
00226         inbox.push (inboxBuffer.front());
00227         inboxBuffer.pop();
00228     }
00229 }
00230 
00231 void ServiceProvider::ReadIT_CWrapper(void *pServiceObject)
00232 {
00233     ServiceProvider * pTempProvider = static_cast <ServiceProvider *> (pServiceObject);
00234     pTempProvider->ReadIT();
00235 }
00236 
00237 
00238 /* Elapsed Time -------------------------------------------------------------*/
00239 
00240 uint32_t ServiceProvider::GetTime(void)
00241 {
00242     return (CanOpenApiGetHardwareTime() + elapsedTimeOffset) & 0x0FFFFFFF;
00243 }
00244 
00245 void ServiceProvider::HandleTimeMessage(CanOpenMessage *canOpenMsg)
00246 {
00247     if (6 == canOpenMsg->dataCount) {        
00248         
00249         uint32_t timeStamp_ms = (uint32_t)canOpenMsg->data[0] +
00250                                 (uint32_t)canOpenMsg->data[1] << 8 +
00251                                 (uint32_t)canOpenMsg->data[2] << 16 +
00252                                 (uint32_t)canOpenMsg->data[3] << 24;
00253         
00254         // Nothing uses Days yet, going to ignore for now.                        
00255         // uint32_t timeStamp_days = (uint32_t)canOpenMsg->data[4] +
00256         //                           (uint32_t)canOpenMsg->data[5] << 8;
00257         
00258         uint32_t hwTime = CanOpenApiGetHardwareTime() & 0x0FFFFFFF;
00259     
00260         elapsedTimeOffset = ((int32_t)timeStamp_ms) - ((int32_t)hwTime);
00261         
00262     // Else TODO: send error
00263     }
00264 }
00265 
00266 
00267 
00268 } /* namspace ppCANOpen */
00269