File content as of revision 5:22a337cdc0e3:

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

#include "CanOpenMessage.h"

#include <stdio.h>

namespace ppCANOpen
{

Node::Node (int id, ServiceProvider * pProvider, int bLoop)
{
    nodeId = id;

    state.nmtState = State::INITIALIZED;

    bLoopbackOn = bLoop;

    pMyProvider = pProvider;
    pProvider->AddNode(this);    
}

/*=============================================================================
 * Methods to handle message indication and confirmation
 *=============================================================================
 */

int Node::DispatchMessage(CanOpenMessage *canOpenMsg)
{
    int command = MESSAGE_GET_COMMAND(canOpenMsg->id);

    switch (command) {
        case CANOPEN_FUNCTION_CODE_NMT:
            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("   some random message\r\n");
            break;
    }

    return 1;
}

/*=============================================================================
 * Methods to handle message indication and confirmation
 *=============================================================================
 */

static void CopyBits(uint8_t *sourceData, 
                uint8_t *destData,
                uint8_t mappedBits,
                uint8_t &sourceBitNum, 
                uint8_t &destBitNum);

int Node::HandlePdo (CanOpenMessage *canOpenMsg)
{
    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::HandleSdo (CanOpenMessage *canOpenMsg)
{
    
    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)
{
    return 0;
}

int Node::HandleNodeControl (CanOpenMessage *canOpenMsg)
{
    int result = 0;

    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          = 0;
                state.bLSS          = 1;
                OnPreoperational();
                result = 1;
                break;

            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;
                
                OnPreoperational();

                result = 1;
                break;

            default:
                break;
        }

    }

    return result;
}

int Node::HandleNodeGuardRequest (const int masterId)
{
    return 0;
}

int Node::ConsumeHeartbeat (const int producerId)
{
    return 0;
}

/*=============================================================================
 * Methods to handle operation of node device
 *=============================================================================
 */

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) {
        OnUpdate();
    }
}

/*=============================================================================
 * 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.
 *
 * Removed from Service ProviderClass.  Not sure if we will need these in the future
 *=============================================================================
 *=============================================================================
 */

/* PDO (7.2.2), MPDO (7.2.3) --------------------------------------------*/

/** Build and send a PDO request
 *  @note
 *  @param
 */
void RequestPdo (int pdoNum) {}

/** Build and send a PDO
 *  @note
 *  @param
 */
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
 *  @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) {}

/** initiate SDO upload
 *  @note
 *  @param
 */
void UploadSdo (int sdoNum, int index, int subindex) {}

/** Acknowledge that SDO was recieved properly
 *  @note
 *  @param
 */
void ConfirmSdo (int sdoNum, int bSuccess) {}

/** Abort current SDO transfer
 *  @note
 *  @param
 */
void AbortSdo (int sdoNum) {}


/* Emergency object (7.2.7) ---------------------------------------------*/

// TODO: emergency producer


/* Network Management (7.2.8) -------------------------------------------*/
/* ---- Node Control (7.2.8.2.1) ----------------------------------------*/

/** Build a CANOpen nmt control message to a node
 *  @note
 *  @param
 */
int  SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId)
{
    return 0;
}


/* ---- Error Control (7.2.8.2.2) ---------------------------------------*/

/** Build a CANOpen error control request to a node
 *  @note
 *  @param
 */
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;
}


} /* namspace ppCANOpen */