library for C++ CANOpen implementation. mbed independant, but is easy to attach into with mbed.
Dependents: ppCANOpen_Example DISCO-F746NG_rtos_test
Example:
Import programppCANOpen_Example
I am no longer actively working on the ppCANOpen library, however, I want to publish this project so that anyone who wants to pick up any of the pieces can have a good example. This is a a project I was working on using the ppCANOpen library. It has a pretty in deep use of the object dictionary structure. And a number of functions to control high voltage pinball drivers, if you're into that sort of thing.
source/Node.cpp
- Committer:
- ptpaterson
- Date:
- 2016-02-13
- Revision:
- 5:22a337cdc0e3
- Parent:
- 4:2034b04c86d2
File content as of revision 5:22a337cdc0e3:
/** ****************************************************************************** * @file * @author Paul Paterson * @version * @date 2015-12-14 * @brief CANOpen implementation library ****************************************************************************** * @attention * * <h2><center>© 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 */