File content as of revision 4:2034b04c86d2:

/**
 ******************************************************************************
 * @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 "ServiceProvider.h"
#include "Node.h"

#include "CanOpenHandle.h"
#include "canopen_api.h"

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

namespace ppCANOpen
{
    
/*=============================================================================
 * Construction
 *=============================================================================
 */ 

ServiceProvider::ServiceProvider (void) : nodeCount(0) 
{
    
    //messageCount = 0;
    
    //memset (nodes, 0, sizeof(Node) * SERVICE_MAX_NODES ); // TODO: fix incorrect <--
    for (int i = 0; i < SERVICE_MAX_NODES; i++) {
        nodes[i] = 0;   
    }
    
    hCanOpen = new CanOpenHandle;
    CanOpenApiInit(hCanOpen);
}

ServiceProvider::~ServiceProvider (void)
{
    delete hCanOpen;   
}

/*=============================================================================
 * main loop
 *=============================================================================
 */ 

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

    }    
}

/*=============================================================================
 * Register a node to get messages and get update calls
 *=============================================================================
 */ 

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

void ServiceProvider::PostMessage (CanOpenMessage * msg)
{
    outbox.push(*msg);
}

} /* namspace ppCANOpen */