library for C++ CANOpen implementation. mbed independant, but is easy to attach into with mbed.
Dependents: ppCANOpen_Example DISCO-F746NG_rtos_test
Node.cpp
00001 /** 00002 ****************************************************************************** 00003 * @file 00004 * @author Paul Paterson 00005 * @version 00006 * @date 2015-12-14 00007 * @brief CANOpen implementation library 00008 ****************************************************************************** 00009 * @attention 00010 * 00011 * <h2><center>© COPYRIGHT(c) 2015 Paul Paterson 00012 * 00013 * All rights reserved. 00014 00015 This program is free software: you can redistribute it and/or modify 00016 it under the terms of the GNU General Public License as published by 00017 the Free Software Foundation, either version 3 of the License, or 00018 (at your option) any later version. 00019 00020 This program is distributed in the hope that it will be useful, 00021 but WITHOUT ANY WARRANTY; without even the implied warranty of 00022 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00023 GNU General Public License for more details. 00024 00025 You should have received a copy of the GNU General Public License 00026 along with this program. If not, see <http://www.gnu.org/licenses/>. 00027 */ 00028 00029 #include "Node.h" 00030 #include "ServiceProvider.h" 00031 #include "ObjectDictionary.h" 00032 00033 #include "CanOpenMessage.h" 00034 00035 #include <stdio.h> 00036 00037 namespace ppCANOpen 00038 { 00039 00040 Node::Node (int id, ServiceProvider * pProvider, int bLoop) 00041 { 00042 nodeId = id; 00043 00044 state.nmtState = State::INITIALIZED; 00045 00046 bLoopbackOn = bLoop; 00047 00048 pMyProvider = pProvider; 00049 pProvider->AddNode(this); 00050 } 00051 00052 /*============================================================================= 00053 * Methods to handle message indication and confirmation 00054 *============================================================================= 00055 */ 00056 00057 int Node::DispatchMessage(CanOpenMessage *canOpenMsg) 00058 { 00059 int command = MESSAGE_GET_COMMAND(canOpenMsg->id); 00060 00061 switch (command) { 00062 case CANOPEN_FUNCTION_CODE_NMT: 00063 printf(" NMT Control: \r\n"); 00064 HandleNodeControl(canOpenMsg); 00065 break; 00066 00067 case CANOPEN_FUNCTION_CODE_PDO1T: 00068 case CANOPEN_FUNCTION_CODE_PDO1R: 00069 case CANOPEN_FUNCTION_CODE_PDO2T: 00070 case CANOPEN_FUNCTION_CODE_PDO2R: 00071 case CANOPEN_FUNCTION_CODE_PDO3T: 00072 case CANOPEN_FUNCTION_CODE_PDO3R: 00073 case CANOPEN_FUNCTION_CODE_PDO4T: 00074 case CANOPEN_FUNCTION_CODE_PDO4R: 00075 if (state.bPDO) { 00076 HandlePdo(canOpenMsg); 00077 } 00078 break; 00079 00080 case CANOPEN_FUNCTION_CODE_SDOT: 00081 case CANOPEN_FUNCTION_CODE_SDOR: 00082 if (state.bSDO) { 00083 HandleSdo(canOpenMsg); 00084 } 00085 break; 00086 00087 default: 00088 printf(" some random message\r\n"); 00089 break; 00090 } 00091 00092 return 1; 00093 } 00094 00095 /*============================================================================= 00096 * Methods to handle message indication and confirmation 00097 *============================================================================= 00098 */ 00099 00100 static void CopyBits(uint8_t *sourceData, 00101 uint8_t *destData, 00102 uint8_t mappedBits, 00103 uint8_t &sourceBitNum, 00104 uint8_t &destBitNum); 00105 00106 int Node::HandlePdo (CanOpenMessage *canOpenMsg) 00107 { 00108 printf(" RPDO:\r\n"); 00109 00110 int result = 0; 00111 00112 if (CANOPEN_TYPE_REMOTE == canOpenMsg->type) { 00113 00114 } else { 00115 int pdoNum = 0; 00116 int bSearching = 1; 00117 00118 ObjectData * parameterObject; 00119 00120 /* search through PDO parameters until we find a match */ 00121 while (bSearching) { 00122 00123 //printf(" PDO %d?", pdoNum); 00124 parameterObject = ScanIndex (0x1400 + pdoNum); 00125 if (parameterObject) { 00126 00127 uint32_t *cobId = (uint32_t*) parameterObject->entries[1].pData; 00128 if (canOpenMsg->id == *cobId) { 00129 bSearching = 0; 00130 00131 //printf(" ... match!!!\r\n"); 00132 } else { 00133 pdoNum++; 00134 //printf(" ... nope\r\n"); 00135 } 00136 00137 } else { 00138 bSearching = 0; 00139 printf("\r\n ERROR: No PDO Parameter match found\r\n"); 00140 } 00141 00142 00143 } 00144 00145 /* if a matching parameter object was found then get mapping */ 00146 if (parameterObject) { 00147 00148 ObjectData *mappingObject = ScanIndex (0x1600 + pdoNum); 00149 00150 if (mappingObject) { 00151 SubIndexSize *numMappedVariables = (SubIndexSize*) mappingObject->entries[0].pData; 00152 00153 /* variable to track position in data copy */ 00154 uint8_t sourceBitNum = 0; 00155 00156 int bError = 0; 00157 00158 /* for each mapped variable, write in data from message */ 00159 for (SubIndexSize subIndexIterator = 1; subIndexIterator <= *numMappedVariables && !bError; subIndexIterator++) { 00160 00161 uint32_t *map = (uint32_t*)mappingObject->entries[subIndexIterator].pData; 00162 00163 uint8_t mappedBits = (uint8_t) (*map); 00164 uint8_t mappedBytes = mappedBits / 8; 00165 SubIndexSize mappedSubIndex = (SubIndexSize) (*map >> 8); 00166 IndexSize mappedIndex = (IndexSize) (*map >> 16); 00167 00168 printf(" mapped: %#06x, %#04x, %d\r\n", 00169 mappedIndex, mappedSubIndex, mappedBits); 00170 00171 /* if less than 0x1000 then it is a dummy value */ 00172 if (mappedIndex < 0x1000) { 00173 sourceBitNum += mappedBits; 00174 } else { 00175 00176 /* get the index object */ 00177 ObjectData *mappedObject = ScanIndex (mappedIndex); 00178 if (mappedObject) { 00179 00180 /* get the subindex object */ 00181 if (mappedSubIndex <= *(SubIndexSize*) mappedObject->entries[0].pData) { 00182 00183 EntryData *destinationEntry = &mappedObject->entries[mappedSubIndex]; 00184 00185 /* can we write to it? */ 00186 if (!(destinationEntry->properties & EntryData::PROPERTY_WRITEABLE)) { 00187 printf(" ERROR: Mapped SubIndex is not writeable!\r\n"); 00188 bError = 1; 00189 } 00190 00191 uint8_t sourceByteNum = sourceBitNum / 8; 00192 if (sourceByteNum + mappedBytes > canOpenMsg->dataCount) { 00193 printf(" ERROR: Insufficient mapped data remaining!\r\n"); 00194 bError = 1; 00195 } 00196 00197 if (destinationEntry->size < mappedBytes) { 00198 printf(" ERROR: Too much data to pack into destination!\r\n"); 00199 bError = 1; 00200 } 00201 00202 if (!bError) { 00203 00204 //printf(" No Errors, copying data...\r\n"); 00205 00206 uint8_t* destData = (uint8_t*) destinationEntry->pData; 00207 uint8_t* sourceData = canOpenMsg->data; 00208 00209 uint8_t destBitNum = 0; 00210 CopyBits(sourceData, destData, mappedBits, sourceBitNum, destBitNum); 00211 } 00212 00213 } else { 00214 printf(" ERROR: Mapped SubIndex does not exist!\r\n"); 00215 bError = 1; 00216 } 00217 00218 } else { 00219 printf(" ERROR: Mapped Index does not exist!\r\n"); 00220 bError = 1; 00221 } 00222 00223 } /* if mappedIndex < 0x1000 */ 00224 00225 00226 } /* for each mapping */ 00227 00228 if (!bError) { 00229 result = 1; 00230 } 00231 00232 } else { 00233 printf(" ERROR: No PDO Mapping match found\r\n"); 00234 } 00235 00236 } /* if parameter exists */ 00237 00238 } /* if remote message */ 00239 00240 return result; 00241 } 00242 00243 00244 int Node::HandleSdo (CanOpenMessage *canOpenMsg) 00245 { 00246 00247 printf(" SDO:\r\n"); 00248 00249 int result = 0; 00250 00251 int bSearching = 1; 00252 00253 int sdoNum = 0; 00254 int sdoType = 0; /* 0=unknown, 1=receive/server, 2=transmit/client */ 00255 ObjectData * sdoObject; 00256 00257 /* search through PDO parameters until we find a match */ 00258 while (bSearching) { 00259 00260 sdoObject = ScanIndex (0x1200 + sdoNum); 00261 if (sdoObject) { 00262 00263 uint32_t *receiveCobId = (uint32_t*) sdoObject->entries[1].pData; 00264 uint32_t *transmitCobId = (uint32_t*) sdoObject->entries[2].pData; 00265 00266 if (canOpenMsg->id == *receiveCobId) { 00267 bSearching = 0; 00268 sdoType = 1; 00269 printf(" Receive SDO\r\n"); 00270 } else if (canOpenMsg->id == *transmitCobId) { 00271 bSearching = 0; 00272 sdoType = 2; 00273 printf(" Transmit SDO\r\n"); 00274 } 00275 00276 } else { 00277 printf(" ERROR: No SDO parameters found"); 00278 bSearching = 0; 00279 } 00280 00281 sdoNum++; 00282 } 00283 00284 return result; 00285 } 00286 00287 int Node::ConsumeEmergency (void) 00288 { 00289 return 0; 00290 } 00291 00292 int Node::HandleNodeControl (CanOpenMessage *canOpenMsg) 00293 { 00294 int result = 0; 00295 00296 if (canOpenMsg->data[0] == nodeId) { 00297 00298 int commandSpecifier = (int)canOpenMsg->data[1]; 00299 00300 switch (commandSpecifier) { 00301 case NMT_CS_START: 00302 printf(" NMT_CS_START\r\n"); 00303 if ((State::INITIALIZED == state.nmtState) || 00304 (State::PREOPERATIONAL == state.nmtState) || 00305 (State::STOPPED == state.nmtState)) 00306 { 00307 00308 state.nmtState = State::OPERATIONAL; 00309 state.bBoot = 0; 00310 state.bSDO = 1; 00311 state.bEmergency = 1; 00312 state.bSYNC = 1; 00313 state.bLifeGuard = 1; 00314 state.bPDO = 1; 00315 state.bLSS = 0; 00316 OnOperational(); 00317 result = 1; 00318 } 00319 break; 00320 00321 case NMT_CS_STOP: 00322 if (State::INITIALIZED != state.nmtState) { 00323 state.nmtState = State::STOPPED; 00324 state.bBoot = 0; 00325 state.bSDO = 0; 00326 state.bEmergency = 0; 00327 state.bSYNC = 0; 00328 state.bLifeGuard = 1; 00329 state.bPDO = 0; 00330 state.bLSS = 1; 00331 } 00332 OnStopped(); 00333 result = 1; 00334 break; 00335 00336 case NMT_CS_ENTER_PREOP: 00337 state.nmtState = State::PREOPERATIONAL; 00338 state.bBoot = 0; 00339 state.bSDO = 1; 00340 state.bEmergency = 1; 00341 state.bSYNC = 1; 00342 state.bLifeGuard = 1; 00343 state.bPDO = 0; 00344 state.bLSS = 1; 00345 OnPreoperational(); 00346 result = 1; 00347 break; 00348 00349 case NMT_CS_RESET_NODE: 00350 case NMT_CS_RESET_COM: 00351 printf(" NMT_CS_RESET\r\n"); 00352 00353 state.nmtState = State::INITIALIZED; 00354 state.bBoot = 1; 00355 state.bSDO = 0; 00356 state.bEmergency = 0; 00357 state.bSYNC = 0; 00358 state.bLifeGuard = 0; 00359 state.bPDO = 0; 00360 state.bLSS = 0; 00361 00362 state.bLifeGuardToggle = 0; 00363 00364 /* boot message is actually just the first node guard/ heart beat message */ 00365 // TODO: wrap up into heartbeat/lifeguard message 00366 CanOpenMessage msgBoot; 00367 msgBoot.id = CANOPEN_FUNCTION_CODE_NODE_GUARD | 5; 00368 msgBoot.format = CANOPEN_FORMAT_STANDARD; 00369 msgBoot.type = CANOPEN_TYPE_DATA; 00370 msgBoot.dataCount = 1; 00371 msgBoot.data[0] = 0; 00372 00373 pMyProvider->PostMessage(nodeId, &msgBoot); 00374 00375 OnInitialize(); 00376 00377 state.nmtState = State::PREOPERATIONAL; 00378 state.bBoot = 0; 00379 state.bSDO = 1; 00380 state.bEmergency = 1; 00381 state.bSYNC = 1; 00382 state.bLifeGuard = 1; 00383 state.bPDO = 0; 00384 state.bLSS = 1; 00385 00386 OnPreoperational(); 00387 00388 result = 1; 00389 break; 00390 00391 default: 00392 break; 00393 } 00394 00395 } 00396 00397 return result; 00398 } 00399 00400 int Node::HandleNodeGuardRequest (const int masterId) 00401 { 00402 return 0; 00403 } 00404 00405 int Node::ConsumeHeartbeat (const int producerId) 00406 { 00407 return 0; 00408 } 00409 00410 /*============================================================================= 00411 * Methods to handle operation of node device 00412 *============================================================================= 00413 */ 00414 00415 void Node::FixedUpdate (uint32_t time) 00416 { 00417 timeSinceLastTick = time - timeCurrentTick; 00418 timeCurrentTick = time; 00419 00420 if (State::OPERATIONAL == state.nmtState) { 00421 OnFixedUpdate(); 00422 } 00423 00424 } 00425 00426 void Node::Update (void) 00427 { 00428 if (State::OPERATIONAL == state.nmtState) { 00429 OnUpdate(); 00430 } 00431 } 00432 00433 /*============================================================================= 00434 * Other Member Functions 00435 *============================================================================= 00436 */ 00437 00438 int Node::PostTPDO (int specifiedCobId) 00439 { 00440 printf(" TPDO:\r\n"); 00441 00442 int result = 0; 00443 00444 /* initialize a blank message -------------------------------------------*/ 00445 CanOpenMessage msg; 00446 00447 msg.id = specifiedCobId; 00448 msg.dataCount = 0; 00449 msg.type = CANOPEN_TYPE_DATA; 00450 msg.format = CANOPEN_FORMAT_STANDARD; 00451 00452 msg.data[0] = 0; 00453 msg.data[1] = 0; 00454 msg.data[2] = 0; 00455 msg.data[3] = 0; 00456 msg.data[4] = 0; 00457 msg.data[5] = 0; 00458 msg.data[6] = 0; 00459 msg.data[7] = 0; 00460 00461 int pdoNum = 0; 00462 int bSearching = 1; 00463 00464 /* Find the mapped data and send ----------------------------------------*/ 00465 00466 ObjectData * parameterObject; 00467 00468 /* search through PDO parameters until we find a match */ 00469 while (bSearching) { 00470 00471 //printf(" PDO %d?", pdoNum); 00472 parameterObject = ScanIndex (0x1800 + pdoNum); 00473 if (parameterObject) { 00474 00475 uint32_t *cobId = (uint32_t*) parameterObject->entries[1].pData; 00476 if (specifiedCobId == *cobId) { 00477 bSearching = 0; 00478 00479 //printf(" ... match!!!\r\n"); 00480 } else { 00481 pdoNum++; 00482 //printf(" ... nope\r\n"); 00483 } 00484 00485 } else { 00486 bSearching = 0; 00487 printf("\r\n ERROR: No PDO Parameter match found\r\n"); 00488 } 00489 } 00490 00491 /* if a matching parameter object was found then get mapping */ 00492 if (parameterObject) { 00493 00494 ObjectData *mappingObject = ScanIndex (0x1A00 + pdoNum); 00495 00496 if (mappingObject) { 00497 SubIndexSize *numMappedVariables = (SubIndexSize*) mappingObject->entries[0].pData; 00498 00499 //printf(" numMappedVariables: %d\r\n", *numMappedVariables); 00500 00501 /* variable to track position in data copy */ 00502 uint8_t destBitNum = 0; 00503 00504 int bError = 0; 00505 00506 /* for each mapped variable, write in data from message */ 00507 for (SubIndexSize subIndexIterator = 1; subIndexIterator <= *numMappedVariables && !bError; subIndexIterator++) { 00508 00509 uint32_t *map = (uint32_t*)mappingObject->entries[subIndexIterator].pData; 00510 00511 uint8_t mappedBits = (uint8_t) (*map); 00512 uint8_t mappedBytes = mappedBits / 8; 00513 SubIndexSize mappedSubIndex = (SubIndexSize) (*map >> 8); 00514 IndexSize mappedIndex = (IndexSize) (*map >> 16); 00515 00516 msg.dataCount += mappedBytes; 00517 00518 //printf(" mapped: %#06x, %#04x, %d\r\n", 00519 // mappedIndex, mappedSubIndex, mappedBits); 00520 00521 /* if less than 0x1000 then it is a dummy value */ 00522 if (mappedIndex < 0x1000) { 00523 destBitNum += mappedBits; 00524 //printf(" No Errors, skipping VOID data...\r\n"); 00525 } else { 00526 00527 /* push into the TPDO data */ 00528 ObjectData *mappedObject = ScanIndex (mappedIndex); 00529 if (mappedObject) { 00530 00531 /* get the subindex object */ 00532 if (mappedSubIndex <= *(SubIndexSize*) mappedObject->entries[0].pData) { 00533 00534 EntryData *sourceEntry = &mappedObject->entries[mappedSubIndex]; 00535 00536 /* can we write to it? */ 00537 if (!(sourceEntry->properties & EntryData::PROPERTY_READABLE)) { 00538 printf(" ERROR: Mapped SubIndex is not readable!\r\n"); 00539 bError = 1; 00540 } 00541 00542 uint8_t destByteNum = destBitNum / 8; 00543 if (destByteNum + mappedBytes > 8) { 00544 printf(" ERROR: Too much data to pack into destination!\r\n"); 00545 bError = 1; 00546 } 00547 00548 if (sourceEntry->size < mappedBytes) { 00549 printf(" ERROR: trying to grab too much information!\r\n"); 00550 bError = 1; 00551 } 00552 00553 if (!bError) { 00554 00555 //printf(" No Errors, copying data...\r\n"); 00556 00557 uint8_t *destData = msg.data; 00558 uint8_t *sourceData = (uint8_t*)sourceEntry->pData; 00559 00560 uint8_t sourceBitNum = 0; 00561 CopyBits(sourceData, destData, mappedBits, sourceBitNum, destBitNum); 00562 } 00563 00564 } else { 00565 printf(" ERROR: Mapped SubIndex does not exist!\r\n"); 00566 bError = 1; 00567 } 00568 00569 } else { 00570 printf(" ERROR: Mapped Index does not exist!\r\n"); 00571 bError = 1; 00572 } 00573 00574 } /* if mappedIndex < 0x1000 */ 00575 00576 00577 } /* for each mapping */ 00578 00579 if (!bError) { 00580 result = 1; 00581 00582 /* Send the message we built up */ 00583 pMyProvider->PostMessage(nodeId, &msg); 00584 } 00585 00586 } else { 00587 printf(" ERROR: No PDO Mapping match found\r\n"); 00588 } /* if mappingObject exists */ 00589 00590 } /* if parameter exists */ 00591 00592 00593 return result; 00594 } 00595 00596 /*============================================================================= 00597 * Private functions 00598 *============================================================================= 00599 */ 00600 00601 void ChangeState(int newState) { 00602 00603 } 00604 00605 /*============================================================================= 00606 * Local functions 00607 *============================================================================= 00608 */ 00609 00610 void CopyBits(uint8_t *sourceData, 00611 uint8_t *destData, 00612 uint8_t mappedBits, 00613 uint8_t &sourceBitNum, 00614 uint8_t &destBitNum) 00615 { 00616 uint8_t sourceByteNum; 00617 uint8_t destByteNum; 00618 00619 uint8_t bitCounter = 0; 00620 00621 if ((mappedBits % 8) == 0 && (sourceBitNum % 8) == 0 && (destBitNum % 8) == 0) { 00622 00623 //printf(" Loading BYTEwise...\r\n"); 00624 /* load in by bytes */ 00625 uint8_t destByteNum = 0; 00626 while (bitCounter < mappedBits) { 00627 00628 00629 destByteNum = destBitNum / 8; 00630 sourceByteNum = sourceBitNum / 8; 00631 00632 destData[destByteNum] = sourceData[sourceByteNum]; 00633 00634 destBitNum += 8; 00635 sourceBitNum += 8; 00636 bitCounter += 8; 00637 } 00638 00639 } else { 00640 00641 00642 //printf(" Loading BITwise..."); 00643 /* not a multiple of 8, so do bit by bit */ 00644 while (bitCounter < mappedBits) { 00645 00646 destByteNum = destBitNum / 8; 00647 sourceByteNum = sourceBitNum / 8; 00648 00649 /* clear the destination bit */ 00650 destData[destByteNum] &= ~(1 << destBitNum); 00651 /* get source bit value */ 00652 uint8_t destValue = (sourceData[sourceByteNum] & (1 << (sourceBitNum % 8))) >> (sourceBitNum % 8) << destBitNum; 00653 /* set dest bit */ 00654 destData[destByteNum] |= destValue; 00655 00656 destBitNum++; 00657 sourceBitNum++; 00658 bitCounter++; 00659 } 00660 } 00661 } 00662 00663 /*============================================================================= 00664 *============================================================================= 00665 * Methods to handle message requests and responses 00666 * Called by the node, usually during Update() or during handling of 00667 * incoming messages. 00668 * 00669 * Removed from Service ProviderClass. Not sure if we will need these in the future 00670 *============================================================================= 00671 *============================================================================= 00672 */ 00673 00674 /* PDO (7.2.2), MPDO (7.2.3) --------------------------------------------*/ 00675 00676 /** Build and send a PDO request 00677 * @note 00678 * @param 00679 */ 00680 void RequestPdo (int pdoNum) {} 00681 00682 /** Build and send a PDO 00683 * @note 00684 * @param 00685 */ 00686 void ProducePdo (int pdoNum, char * data) {} 00687 00688 00689 /* SDO (7.2.4) ----------------------------------------------------------*/ 00690 00691 /** initiate SDO download 00692 * @note Handles automatically whether it will be a expedited transfer or 00693 * or if message will be split into 00694 * @param 00695 * 00696 * Node will create a big data array and Service provide will have to 00697 * iterate through and send all of the data. ServiceProvider will pass 00698 * the confirmation to the node, and the node will free up it's buffer. 00699 */ 00700 void DownloadSdo (int sdoNum, int index, int subindex, int size, char * data) {} 00701 00702 /** initiate SDO upload 00703 * @note 00704 * @param 00705 */ 00706 void UploadSdo (int sdoNum, int index, int subindex) {} 00707 00708 /** Acknowledge that SDO was recieved properly 00709 * @note 00710 * @param 00711 */ 00712 void ConfirmSdo (int sdoNum, int bSuccess) {} 00713 00714 /** Abort current SDO transfer 00715 * @note 00716 * @param 00717 */ 00718 void AbortSdo (int sdoNum) {} 00719 00720 00721 /* Emergency object (7.2.7) ---------------------------------------------*/ 00722 00723 // TODO: emergency producer 00724 00725 00726 /* Network Management (7.2.8) -------------------------------------------*/ 00727 /* ---- Node Control (7.2.8.2.1) ----------------------------------------*/ 00728 00729 /** Build a CANOpen nmt control message to a node 00730 * @note 00731 * @param 00732 */ 00733 int SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId) 00734 { 00735 return 0; 00736 } 00737 00738 00739 /* ---- Error Control (7.2.8.2.2) ---------------------------------------*/ 00740 00741 /** Build a CANOpen error control request to a node 00742 * @note 00743 * @param 00744 */ 00745 int RequestErrorControl (NmtCommandSpecifier cs, unsigned int nodeId) 00746 { 00747 return 0; 00748 } 00749 00750 /** Build a CANOpen error control response 00751 * @note 00752 * @param 00753 */ 00754 int RespondErrorControl (NmtCommandSpecifier cs, unsigned int nodeId) 00755 { 00756 return 0; 00757 } 00758 00759 00760 } /* namspace ppCANOpen */ 00761 00762
Generated on Sun Jul 17 2022 07:51:19 by 1.7.2