Paul Paterson
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.
Last commit 19 Mar 2016 by 
Paul Paterson
Diff: source/Node.cpp
- Revision:
- 5:22a337cdc0e3
- Parent:
- 4:2034b04c86d2
--- a/source/Node.cpp Sat Jan 09 17:15:29 2016 +0000
+++ b/source/Node.cpp Sat Feb 13 20:22:59 2016 +0000
@@ -36,84 +36,17 @@
namespace ppCANOpen
{
-
- /** CANOpen Function Codes */
- typedef enum {
- CANOPEN_FUNCTION_CODE_NMT = 0x00,
- CANOPEN_FUNCTION_CODE_SYNC = 0x01,
- CANOPEN_FUNCTION_CODE_TIME = 0x02,
- CANOPEN_FUNCTION_CODE_PDO1T = 0x03,
- CANOPEN_FUNCTION_CODE_PD01R = 0x04,
- CANOPEN_FUNCTION_CODE_PD02T = 0x05,
- CANOPEN_FUNCTION_CODE_PD02R = 0x06,
- CANOPEN_FUNCTION_CODE_PD03T = 0x07,
- CANOPEN_FUNCTION_CODE_PD03R = 0x08,
- CANOPEN_FUNCTION_CODE_PD04T = 0x09,
- CANOPEN_FUNCTION_CODE_PD04R = 0x0A,
- CANOPEN_FUNCTION_CODE_SD0T = 0x0B,
- CANOPEN_FUNCTION_CODE_SD0R = 0x0C,
- CANOPEN_FUNCTION_CODE_NODE_GUARD = 0x0E,
- CANOPEN_FUNCTION_CODE_LSS = 0x0F
- } CanOpenFunctionCodes;
-
- /* Message Constants */
- #define MESSAGE_NODEID_BITS 0b00001111111
- #define MESSAGE_COMMAND_BITS 0b11110000000
-
- /* Message Macros -----------------------------------------------------------*/
- #define MESSAGE_GET_NODEID(cobId) (cobId & MESSAGE_NODEID_BITS)
- #define MESSAGE_GET_COMMAND(cobId) ((cobId & MESSAGE_COMMAND_BITS) >> 7)
+Node::Node (int id, ServiceProvider * pProvider, int bLoop)
+{
+ nodeId = id;
- /*=========================================================================
- * SDO MESSAGE PARAMETERS
- *=========================================================================
- */
-
- /** SDO initiate protocol command specifiers */
- typedef enum {
- SDO_CCS_DOWNLOAD_SEGMENT_REQUEST = 0x00,
- SDO_CCS_INITIATE_DOWNLOAD_REQUEST = 0x01,
- SDO_CCS_INITIATE_UPLOAD_REQUEST = 0x02,
- } SdoClientCommandSpecifier;
-
- /** SDO segment protocol command specifiers */
- typedef enum {
- SDO_SCS_DOWNLOAD_SEGMENT_RESPONSE = 0x01,
- SDO_SCS_INITIATE_UPLOAD_RESPONSE = 0x02,
- SDO_SCS_INITIATE_DOWNLOAD_RESPONSE = 0x03,
- } SdoServerCommandSpecifier;
-
- /* SDO constants --------------------------------------------------------*/
- #define SDO_SIZE_INDICATOR_BIT 0b00000001
- #define SDO_TRANSFER_TYPE_BIT 0b00000010
- #define SDO_DATA_COUNT_BITS 0b00001100
- #define SDO_TOGGLE_BIT 0b00010000
- #define SDO_CS_BITS 0b11100000
-
- /* SDO macros -----------------------------------------------------------*/
- #define SDO_GET_CS(data0) ((data0 & SDO_CS_BITS) >> 5)
- #define SDO_GET_DATA_COUNT(data0) ((data0 & SDO_DATA_COUNT_BITS) >> 2)
-
-
- /*=========================================================================
- * NMT MESSAGE PARAMETERS
- *=========================================================================
- */
+ state.nmtState = State::INITIALIZED;
- /** NMT node control protocol command specifiers */
- typedef enum {
- NMT_CS_START = 0x01,
- NMT_CS_STOP = 0x02,
- NMT_CS_ENTER_PREOP = 0x80,
- NMT_CS_RESET_NODE = 0x81,
- NMT_CS_RESET_COM = 0x82
- } NmtCommandSpecifier;
+ bLoopbackOn = bLoop;
-Node::Node (ServiceProvider * pProvider)
-{
pMyProvider = pProvider;
- pProvider->AddNode(this);
+ pProvider->AddNode(this);
}
/*=============================================================================
@@ -121,29 +54,41 @@
*=============================================================================
*/
-int Node::DispatchMessage(CanOpenMessage *msg)
+int Node::DispatchMessage(CanOpenMessage *canOpenMsg)
{
- int command = MESSAGE_GET_COMMAND(msg->id);
- int nodeId = MESSAGE_GET_NODEID(msg->id);
+ int command = MESSAGE_GET_COMMAND(canOpenMsg->id);
- //printf("*** N.Dispatch: got com and id\r\n");
-
switch (command) {
case CANOPEN_FUNCTION_CODE_NMT:
- printf("*** N.Dispatch: it's an NMT Control!!!\r\n");
- if (msg->data[0] == nodeId) {
- HandleNodeControl ((int)msg->data[1]);
+ 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("*** N.Dispatch: some random message\r\n");
+ printf(" some random message\r\n");
break;
}
- // ECHO ***********************
- pMyProvider->PostMessage(msg);
- // END ECHO *******************
-
return 1;
}
@@ -152,14 +97,191 @@
*=============================================================================
*/
-int Node::ConsumePdo (const int pdoNum, char *const data)
+static void CopyBits(uint8_t *sourceData,
+ uint8_t *destData,
+ uint8_t mappedBits,
+ uint8_t &sourceBitNum,
+ uint8_t &destBitNum);
+
+int Node::HandlePdo (CanOpenMessage *canOpenMsg)
{
- return 0;
+ 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::HandlePdoReadRequest (const int pdoNum)
+
+int Node::HandleSdo (CanOpenMessage *canOpenMsg)
{
- return 0;
+
+ 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)
@@ -167,84 +289,109 @@
return 0;
}
-int Node::HandleNodeControl (int commandSpecifier)
+int Node::HandleNodeControl (CanOpenMessage *canOpenMsg)
{
int result = 0;
- switch (commandSpecifier) {
- case NMT_CS_START:
- if (State::INITIALIZED != state.nmtState) {
- state.nmtState = State::OPERATIONAL;
+ 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 = 1;
- state.bLSS = 0;
- OnOperational();
+ state.bPDO = 0;
+ state.bLSS = 1;
+ OnPreoperational();
result = 1;
- }
- break;
+ break;
- case NMT_CS_STOP:
- if (State::INITIALIZED != state.nmtState) {
- state.nmtState = State::STOPPED;
- state.bBoot = 0;
+ 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;
- }
- 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;
+
+ OnPreoperational();
- case NMT_CS_RESET_NODE:
- case NMT_CS_RESET_COM:
-
- 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(&msgBoot);
-
- result = 1;
- break;
+ result = 1;
+ break;
- default:
- break;
+ default:
+ break;
+ }
+
}
return result;
@@ -265,29 +412,256 @@
*=============================================================================
*/
+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) {
+ if (State::OPERATIONAL == state.nmtState) {
OnUpdate();
}
-
- // TODO: check elapsed time to see if it is time for fixed update
}
/*=============================================================================
- * Methods to implement node control in derived classes
+ * 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.
@@ -303,45 +677,45 @@
* @note
* @param
*/
-void RequestPdo (int pdoNum){}
+void RequestPdo (int pdoNum) {}
/** Build and send a PDO
* @note
* @param
*/
-void ProducePdo (int pdoNum, char * data){}
+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
+ * 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){}
+void DownloadSdo (int sdoNum, int index, int subindex, int size, char * data) {}
/** initiate SDO upload
- * @note
+ * @note
* @param
*/
-void UploadSdo (int sdoNum, int index, int subindex){}
+void UploadSdo (int sdoNum, int index, int subindex) {}
/** Acknowledge that SDO was recieved properly
- * @note
+ * @note
* @param
*/
-void ConfirmSdo (int sdoNum, int bSuccess){}
+void ConfirmSdo (int sdoNum, int bSuccess) {}
/** Abort current SDO transfer
- * @note
+ * @note
* @param
*/
-void AbortSdo (int sdoNum){}
+void AbortSdo (int sdoNum) {}
/* Emergency object (7.2.7) ---------------------------------------------*/
@@ -356,7 +730,10 @@
* @note
* @param
*/
-int SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+int SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId)
+{
+ return 0;
+}
/* ---- Error Control (7.2.8.2.2) ---------------------------------------*/
@@ -365,13 +742,19 @@
* @note
* @param
*/
-int RequestErrorControl (NmtCommandSpecifier cs, unsigned int nodeId){return 0;}
+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;}
+int RespondErrorControl (NmtCommandSpecifier cs, unsigned int nodeId)
+{
+ return 0;
+}
} /* namspace ppCANOpen */