File content as of revision 5:22a337cdc0e3:

/**
 ******************************************************************************
 * @file
 * @author  Paul Paterson
 * @version
 * @date    2015-12-22
 * @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/>.
 */
#include "mbed.h"

#include "ServiceProvider.h"
#include "Node.h"

#include "canopen_api.h"

#include <string.h>
#include <stdio.h>

namespace ppCANOpen
{



ServiceProvider::ServiceProvider (void) : nodeCount(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;
    }

    CanOpenApiInit((void *)this, &ReadIT_CWrapper, &UpdateNodes_CWrapper);
}

ServiceProvider::~ServiceProvider (void)
{
}


/* ============================================================================
 * Public Methods
 * ============================================================================
 */


/* Main application ---------------------------------------------------------*/

void ServiceProvider::Run (void)
{
    //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 */
        
            // 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);

    //}
}

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);
    }
}

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;
}


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


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