Paul Paterson / ppCANOpen

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

source/Node.cpp@5:22a337cdc0e3, 2016-02-13 (annotated)

Committer:
ptpaterson
Date:
Sat Feb 13 20:22:59 2016 +0000
Revision:
5:22a337cdc0e3
Parent:
4:2034b04c86d2 
PDO receive complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ptpaterson 2:c724ff3a4e4d 1 /**
ptpaterson 2:c724ff3a4e4d 2 ******************************************************************************
ptpaterson 2:c724ff3a4e4d 3 * @file
ptpaterson 2:c724ff3a4e4d 4 * @author Paul Paterson
ptpaterson 2:c724ff3a4e4d 5 * @version
ptpaterson 2:c724ff3a4e4d 6 * @date 2015-12-14
ptpaterson 2:c724ff3a4e4d 7 * @brief CANOpen implementation library
ptpaterson 2:c724ff3a4e4d 8 ******************************************************************************
ptpaterson 2:c724ff3a4e4d 9 * @attention
ptpaterson 2:c724ff3a4e4d 10 *
ptpaterson 2:c724ff3a4e4d 11 * <h2><center>&copy; COPYRIGHT(c) 2015 Paul Paterson
ptpaterson 2:c724ff3a4e4d 12 *
ptpaterson 2:c724ff3a4e4d 13 * All rights reserved.
ptpaterson 2:c724ff3a4e4d 14
ptpaterson 2:c724ff3a4e4d 15 This program is free software: you can redistribute it and/or modify
ptpaterson 2:c724ff3a4e4d 16 it under the terms of the GNU General Public License as published by
ptpaterson 2:c724ff3a4e4d 17 the Free Software Foundation, either version 3 of the License, or
ptpaterson 2:c724ff3a4e4d 18 (at your option) any later version.
ptpaterson 2:c724ff3a4e4d 19
ptpaterson 2:c724ff3a4e4d 20 This program is distributed in the hope that it will be useful,
ptpaterson 2:c724ff3a4e4d 21 but WITHOUT ANY WARRANTY; without even the implied warranty of
ptpaterson 2:c724ff3a4e4d 22 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
ptpaterson 2:c724ff3a4e4d 23 GNU General Public License for more details.
ptpaterson 2:c724ff3a4e4d 24
ptpaterson 2:c724ff3a4e4d 25 You should have received a copy of the GNU General Public License
ptpaterson 2:c724ff3a4e4d 26 along with this program. If not, see <http://www.gnu.org/licenses/>.
ptpaterson 2:c724ff3a4e4d 27 */
ptpaterson 2:c724ff3a4e4d 28
ptpaterson 2:c724ff3a4e4d 29 #include "Node.h"
ptpaterson 3:12b3c25bdeba 30 #include "ServiceProvider.h"
ptpaterson 2:c724ff3a4e4d 31 #include "ObjectDictionary.h"
ptpaterson 2:c724ff3a4e4d 32
ptpaterson 4:2034b04c86d2 33 #include "CanOpenMessage.h"
ptpaterson 4:2034b04c86d2 34
ptpaterson 4:2034b04c86d2 35 #include <stdio.h>
ptpaterson 4:2034b04c86d2 36
ptpaterson 2:c724ff3a4e4d 37 namespace ppCANOpen
ptpaterson 2:c724ff3a4e4d 38 {
ptpaterson 2:c724ff3a4e4d 39
ptpaterson 5:22a337cdc0e3 40 Node::Node (int id, ServiceProvider * pProvider, int bLoop)
ptpaterson 5:22a337cdc0e3 41 {
ptpaterson 5:22a337cdc0e3 42 nodeId = id;
ptpaterson 4:2034b04c86d2 43
ptpaterson 5:22a337cdc0e3 44 state.nmtState = State::INITIALIZED;
ptpaterson 4:2034b04c86d2 45
ptpaterson 5:22a337cdc0e3 46 bLoopbackOn = bLoop;
ptpaterson 4:2034b04c86d2 47
ptpaterson 4:2034b04c86d2 48 pMyProvider = pProvider;
ptpaterson 5:22a337cdc0e3 49 pProvider->AddNode(this);
ptpaterson 3:12b3c25bdeba 50 }
ptpaterson 3:12b3c25bdeba 51
ptpaterson 3:12b3c25bdeba 52 /*=============================================================================
ptpaterson 3:12b3c25bdeba 53 * Methods to handle message indication and confirmation
ptpaterson 3:12b3c25bdeba 54 *=============================================================================
ptpaterson 3:12b3c25bdeba 55 */
ptpaterson 3:12b3c25bdeba 56
ptpaterson 5:22a337cdc0e3 57 int Node::DispatchMessage(CanOpenMessage *canOpenMsg)
ptpaterson 3:12b3c25bdeba 58 {
ptpaterson 5:22a337cdc0e3 59 int command = MESSAGE_GET_COMMAND(canOpenMsg->id);
ptpaterson 4:2034b04c86d2 60
ptpaterson 3:12b3c25bdeba 61 switch (command) {
ptpaterson 3:12b3c25bdeba 62 case CANOPEN_FUNCTION_CODE_NMT:
ptpaterson 5:22a337cdc0e3 63 printf(" NMT Control: \r\n");
ptpaterson 5:22a337cdc0e3 64 HandleNodeControl(canOpenMsg);
ptpaterson 5:22a337cdc0e3 65 break;
ptpaterson 5:22a337cdc0e3 66
ptpaterson 5:22a337cdc0e3 67 case CANOPEN_FUNCTION_CODE_PDO1T:
ptpaterson 5:22a337cdc0e3 68 case CANOPEN_FUNCTION_CODE_PDO1R:
ptpaterson 5:22a337cdc0e3 69 case CANOPEN_FUNCTION_CODE_PDO2T:
ptpaterson 5:22a337cdc0e3 70 case CANOPEN_FUNCTION_CODE_PDO2R:
ptpaterson 5:22a337cdc0e3 71 case CANOPEN_FUNCTION_CODE_PDO3T:
ptpaterson 5:22a337cdc0e3 72 case CANOPEN_FUNCTION_CODE_PDO3R:
ptpaterson 5:22a337cdc0e3 73 case CANOPEN_FUNCTION_CODE_PDO4T:
ptpaterson 5:22a337cdc0e3 74 case CANOPEN_FUNCTION_CODE_PDO4R:
ptpaterson 5:22a337cdc0e3 75 if (state.bPDO) {
ptpaterson 5:22a337cdc0e3 76 HandlePdo(canOpenMsg);
ptpaterson 3:12b3c25bdeba 77 }
ptpaterson 3:12b3c25bdeba 78 break;
ptpaterson 5:22a337cdc0e3 79
ptpaterson 5:22a337cdc0e3 80 case CANOPEN_FUNCTION_CODE_SDOT:
ptpaterson 5:22a337cdc0e3 81 case CANOPEN_FUNCTION_CODE_SDOR:
ptpaterson 5:22a337cdc0e3 82 if (state.bSDO) {
ptpaterson 5:22a337cdc0e3 83 HandleSdo(canOpenMsg);
ptpaterson 5:22a337cdc0e3 84 }
ptpaterson 5:22a337cdc0e3 85 break;
ptpaterson 5:22a337cdc0e3 86
ptpaterson 3:12b3c25bdeba 87 default:
ptpaterson 5:22a337cdc0e3 88 printf(" some random message\r\n");
ptpaterson 3:12b3c25bdeba 89 break;
ptpaterson 3:12b3c25bdeba 90 }
ptpaterson 4:2034b04c86d2 91
ptpaterson 3:12b3c25bdeba 92 return 1;
ptpaterson 3:12b3c25bdeba 93 }
ptpaterson 2:c724ff3a4e4d 94
ptpaterson 2:c724ff3a4e4d 95 /*=============================================================================
ptpaterson 2:c724ff3a4e4d 96 * Methods to handle message indication and confirmation
ptpaterson 2:c724ff3a4e4d 97 *=============================================================================
ptpaterson 2:c724ff3a4e4d 98 */
ptpaterson 2:c724ff3a4e4d 99
ptpaterson 5:22a337cdc0e3 100 static void CopyBits(uint8_t *sourceData,
ptpaterson 5:22a337cdc0e3 101 uint8_t *destData,
ptpaterson 5:22a337cdc0e3 102 uint8_t mappedBits,
ptpaterson 5:22a337cdc0e3 103 uint8_t &sourceBitNum,
ptpaterson 5:22a337cdc0e3 104 uint8_t &destBitNum);
ptpaterson 5:22a337cdc0e3 105
ptpaterson 5:22a337cdc0e3 106 int Node::HandlePdo (CanOpenMessage *canOpenMsg)
ptpaterson 2:c724ff3a4e4d 107 {
ptpaterson 5:22a337cdc0e3 108 printf(" RPDO:\r\n");
ptpaterson 5:22a337cdc0e3 109
ptpaterson 5:22a337cdc0e3 110 int result = 0;
ptpaterson 5:22a337cdc0e3 111
ptpaterson 5:22a337cdc0e3 112 if (CANOPEN_TYPE_REMOTE == canOpenMsg->type) {
ptpaterson 5:22a337cdc0e3 113
ptpaterson 5:22a337cdc0e3 114 } else {
ptpaterson 5:22a337cdc0e3 115 int pdoNum = 0;
ptpaterson 5:22a337cdc0e3 116 int bSearching = 1;
ptpaterson 5:22a337cdc0e3 117
ptpaterson 5:22a337cdc0e3 118 ObjectData * parameterObject;
ptpaterson 5:22a337cdc0e3 119
ptpaterson 5:22a337cdc0e3 120 /* search through PDO parameters until we find a match */
ptpaterson 5:22a337cdc0e3 121 while (bSearching) {
ptpaterson 5:22a337cdc0e3 122
ptpaterson 5:22a337cdc0e3 123 //printf(" PDO %d?", pdoNum);
ptpaterson 5:22a337cdc0e3 124 parameterObject = ScanIndex (0x1400 + pdoNum);
ptpaterson 5:22a337cdc0e3 125 if (parameterObject) {
ptpaterson 5:22a337cdc0e3 126
ptpaterson 5:22a337cdc0e3 127 uint32_t *cobId = (uint32_t*) parameterObject->entries[1].pData;
ptpaterson 5:22a337cdc0e3 128 if (canOpenMsg->id == *cobId) {
ptpaterson 5:22a337cdc0e3 129 bSearching = 0;
ptpaterson 5:22a337cdc0e3 130
ptpaterson 5:22a337cdc0e3 131 //printf(" ... match!!!\r\n");
ptpaterson 5:22a337cdc0e3 132 } else {
ptpaterson 5:22a337cdc0e3 133 pdoNum++;
ptpaterson 5:22a337cdc0e3 134 //printf(" ... nope\r\n");
ptpaterson 5:22a337cdc0e3 135 }
ptpaterson 5:22a337cdc0e3 136
ptpaterson 5:22a337cdc0e3 137 } else {
ptpaterson 5:22a337cdc0e3 138 bSearching = 0;
ptpaterson 5:22a337cdc0e3 139 printf("\r\n ERROR: No PDO Parameter match found\r\n");
ptpaterson 5:22a337cdc0e3 140 }
ptpaterson 5:22a337cdc0e3 141
ptpaterson 5:22a337cdc0e3 142
ptpaterson 5:22a337cdc0e3 143 }
ptpaterson 5:22a337cdc0e3 144
ptpaterson 5:22a337cdc0e3 145 /* if a matching parameter object was found then get mapping */
ptpaterson 5:22a337cdc0e3 146 if (parameterObject) {
ptpaterson 5:22a337cdc0e3 147
ptpaterson 5:22a337cdc0e3 148 ObjectData *mappingObject = ScanIndex (0x1600 + pdoNum);
ptpaterson 5:22a337cdc0e3 149
ptpaterson 5:22a337cdc0e3 150 if (mappingObject) {
ptpaterson 5:22a337cdc0e3 151 SubIndexSize *numMappedVariables = (SubIndexSize*) mappingObject->entries[0].pData;
ptpaterson 5:22a337cdc0e3 152
ptpaterson 5:22a337cdc0e3 153 /* variable to track position in data copy */
ptpaterson 5:22a337cdc0e3 154 uint8_t sourceBitNum = 0;
ptpaterson 5:22a337cdc0e3 155
ptpaterson 5:22a337cdc0e3 156 int bError = 0;
ptpaterson 5:22a337cdc0e3 157
ptpaterson 5:22a337cdc0e3 158 /* for each mapped variable, write in data from message */
ptpaterson 5:22a337cdc0e3 159 for (SubIndexSize subIndexIterator = 1; subIndexIterator <= *numMappedVariables && !bError; subIndexIterator++) {
ptpaterson 5:22a337cdc0e3 160
ptpaterson 5:22a337cdc0e3 161 uint32_t *map = (uint32_t*)mappingObject->entries[subIndexIterator].pData;
ptpaterson 5:22a337cdc0e3 162
ptpaterson 5:22a337cdc0e3 163 uint8_t mappedBits = (uint8_t) (*map);
ptpaterson 5:22a337cdc0e3 164 uint8_t mappedBytes = mappedBits / 8;
ptpaterson 5:22a337cdc0e3 165 SubIndexSize mappedSubIndex = (SubIndexSize) (*map >> 8);
ptpaterson 5:22a337cdc0e3 166 IndexSize mappedIndex = (IndexSize) (*map >> 16);
ptpaterson 5:22a337cdc0e3 167
ptpaterson 5:22a337cdc0e3 168 printf(" mapped: %#06x, %#04x, %d\r\n",
ptpaterson 5:22a337cdc0e3 169 mappedIndex, mappedSubIndex, mappedBits);
ptpaterson 5:22a337cdc0e3 170
ptpaterson 5:22a337cdc0e3 171 /* if less than 0x1000 then it is a dummy value */
ptpaterson 5:22a337cdc0e3 172 if (mappedIndex < 0x1000) {
ptpaterson 5:22a337cdc0e3 173 sourceBitNum += mappedBits;
ptpaterson 5:22a337cdc0e3 174 } else {
ptpaterson 5:22a337cdc0e3 175
ptpaterson 5:22a337cdc0e3 176 /* get the index object */
ptpaterson 5:22a337cdc0e3 177 ObjectData *mappedObject = ScanIndex (mappedIndex);
ptpaterson 5:22a337cdc0e3 178 if (mappedObject) {
ptpaterson 5:22a337cdc0e3 179
ptpaterson 5:22a337cdc0e3 180 /* get the subindex object */
ptpaterson 5:22a337cdc0e3 181 if (mappedSubIndex <= *(SubIndexSize*) mappedObject->entries[0].pData) {
ptpaterson 5:22a337cdc0e3 182
ptpaterson 5:22a337cdc0e3 183 EntryData *destinationEntry = &mappedObject->entries[mappedSubIndex];
ptpaterson 5:22a337cdc0e3 184
ptpaterson 5:22a337cdc0e3 185 /* can we write to it? */
ptpaterson 5:22a337cdc0e3 186 if (!(destinationEntry->properties & EntryData::PROPERTY_WRITEABLE)) {
ptpaterson 5:22a337cdc0e3 187 printf(" ERROR: Mapped SubIndex is not writeable!\r\n");
ptpaterson 5:22a337cdc0e3 188 bError = 1;
ptpaterson 5:22a337cdc0e3 189 }
ptpaterson 5:22a337cdc0e3 190
ptpaterson 5:22a337cdc0e3 191 uint8_t sourceByteNum = sourceBitNum / 8;
ptpaterson 5:22a337cdc0e3 192 if (sourceByteNum + mappedBytes > canOpenMsg->dataCount) {
ptpaterson 5:22a337cdc0e3 193 printf(" ERROR: Insufficient mapped data remaining!\r\n");
ptpaterson 5:22a337cdc0e3 194 bError = 1;
ptpaterson 5:22a337cdc0e3 195 }
ptpaterson 5:22a337cdc0e3 196
ptpaterson 5:22a337cdc0e3 197 if (destinationEntry->size < mappedBytes) {
ptpaterson 5:22a337cdc0e3 198 printf(" ERROR: Too much data to pack into destination!\r\n");
ptpaterson 5:22a337cdc0e3 199 bError = 1;
ptpaterson 5:22a337cdc0e3 200 }
ptpaterson 5:22a337cdc0e3 201
ptpaterson 5:22a337cdc0e3 202 if (!bError) {
ptpaterson 5:22a337cdc0e3 203
ptpaterson 5:22a337cdc0e3 204 //printf(" No Errors, copying data...\r\n");
ptpaterson 5:22a337cdc0e3 205
ptpaterson 5:22a337cdc0e3 206 uint8_t* destData = (uint8_t*) destinationEntry->pData;
ptpaterson 5:22a337cdc0e3 207 uint8_t* sourceData = canOpenMsg->data;
ptpaterson 5:22a337cdc0e3 208
ptpaterson 5:22a337cdc0e3 209 uint8_t destBitNum = 0;
ptpaterson 5:22a337cdc0e3 210 CopyBits(sourceData, destData, mappedBits, sourceBitNum, destBitNum);
ptpaterson 5:22a337cdc0e3 211 }
ptpaterson 5:22a337cdc0e3 212
ptpaterson 5:22a337cdc0e3 213 } else {
ptpaterson 5:22a337cdc0e3 214 printf(" ERROR: Mapped SubIndex does not exist!\r\n");
ptpaterson 5:22a337cdc0e3 215 bError = 1;
ptpaterson 5:22a337cdc0e3 216 }
ptpaterson 5:22a337cdc0e3 217
ptpaterson 5:22a337cdc0e3 218 } else {
ptpaterson 5:22a337cdc0e3 219 printf(" ERROR: Mapped Index does not exist!\r\n");
ptpaterson 5:22a337cdc0e3 220 bError = 1;
ptpaterson 5:22a337cdc0e3 221 }
ptpaterson 5:22a337cdc0e3 222
ptpaterson 5:22a337cdc0e3 223 } /* if mappedIndex < 0x1000 */
ptpaterson 5:22a337cdc0e3 224
ptpaterson 5:22a337cdc0e3 225
ptpaterson 5:22a337cdc0e3 226 } /* for each mapping */
ptpaterson 5:22a337cdc0e3 227
ptpaterson 5:22a337cdc0e3 228 if (!bError) {
ptpaterson 5:22a337cdc0e3 229 result = 1;
ptpaterson 5:22a337cdc0e3 230 }
ptpaterson 5:22a337cdc0e3 231
ptpaterson 5:22a337cdc0e3 232 } else {
ptpaterson 5:22a337cdc0e3 233 printf(" ERROR: No PDO Mapping match found\r\n");
ptpaterson 5:22a337cdc0e3 234 }
ptpaterson 5:22a337cdc0e3 235
ptpaterson 5:22a337cdc0e3 236 } /* if parameter exists */
ptpaterson 5:22a337cdc0e3 237
ptpaterson 5:22a337cdc0e3 238 } /* if remote message */
ptpaterson 5:22a337cdc0e3 239
ptpaterson 5:22a337cdc0e3 240 return result;
ptpaterson 2:c724ff3a4e4d 241 }
ptpaterson 2:c724ff3a4e4d 242
ptpaterson 5:22a337cdc0e3 243
ptpaterson 5:22a337cdc0e3 244 int Node::HandleSdo (CanOpenMessage *canOpenMsg)
ptpaterson 2:c724ff3a4e4d 245 {
ptpaterson 5:22a337cdc0e3 246
ptpaterson 5:22a337cdc0e3 247 printf(" SDO:\r\n");
ptpaterson 5:22a337cdc0e3 248
ptpaterson 5:22a337cdc0e3 249 int result = 0;
ptpaterson 5:22a337cdc0e3 250
ptpaterson 5:22a337cdc0e3 251 int bSearching = 1;
ptpaterson 5:22a337cdc0e3 252
ptpaterson 5:22a337cdc0e3 253 int sdoNum = 0;
ptpaterson 5:22a337cdc0e3 254 int sdoType = 0; /* 0=unknown, 1=receive/server, 2=transmit/client */
ptpaterson 5:22a337cdc0e3 255 ObjectData * sdoObject;
ptpaterson 5:22a337cdc0e3 256
ptpaterson 5:22a337cdc0e3 257 /* search through PDO parameters until we find a match */
ptpaterson 5:22a337cdc0e3 258 while (bSearching) {
ptpaterson 5:22a337cdc0e3 259
ptpaterson 5:22a337cdc0e3 260 sdoObject = ScanIndex (0x1200 + sdoNum);
ptpaterson 5:22a337cdc0e3 261 if (sdoObject) {
ptpaterson 5:22a337cdc0e3 262
ptpaterson 5:22a337cdc0e3 263 uint32_t *receiveCobId = (uint32_t*) sdoObject->entries[1].pData;
ptpaterson 5:22a337cdc0e3 264 uint32_t *transmitCobId = (uint32_t*) sdoObject->entries[2].pData;
ptpaterson 5:22a337cdc0e3 265
ptpaterson 5:22a337cdc0e3 266 if (canOpenMsg->id == *receiveCobId) {
ptpaterson 5:22a337cdc0e3 267 bSearching = 0;
ptpaterson 5:22a337cdc0e3 268 sdoType = 1;
ptpaterson 5:22a337cdc0e3 269 printf(" Receive SDO\r\n");
ptpaterson 5:22a337cdc0e3 270 } else if (canOpenMsg->id == *transmitCobId) {
ptpaterson 5:22a337cdc0e3 271 bSearching = 0;
ptpaterson 5:22a337cdc0e3 272 sdoType = 2;
ptpaterson 5:22a337cdc0e3 273 printf(" Transmit SDO\r\n");
ptpaterson 5:22a337cdc0e3 274 }
ptpaterson 5:22a337cdc0e3 275
ptpaterson 5:22a337cdc0e3 276 } else {
ptpaterson 5:22a337cdc0e3 277 printf(" ERROR: No SDO parameters found");
ptpaterson 5:22a337cdc0e3 278 bSearching = 0;
ptpaterson 5:22a337cdc0e3 279 }
ptpaterson 5:22a337cdc0e3 280
ptpaterson 5:22a337cdc0e3 281 sdoNum++;
ptpaterson 5:22a337cdc0e3 282 }
ptpaterson 5:22a337cdc0e3 283
ptpaterson 5:22a337cdc0e3 284 return result;
ptpaterson 2:c724ff3a4e4d 285 }
ptpaterson 2:c724ff3a4e4d 286
ptpaterson 2:c724ff3a4e4d 287 int Node::ConsumeEmergency (void)
ptpaterson 2:c724ff3a4e4d 288 {
ptpaterson 2:c724ff3a4e4d 289 return 0;
ptpaterson 2:c724ff3a4e4d 290 }
ptpaterson 2:c724ff3a4e4d 291
ptpaterson 5:22a337cdc0e3 292 int Node::HandleNodeControl (CanOpenMessage *canOpenMsg)
ptpaterson 2:c724ff3a4e4d 293 {
ptpaterson 3:12b3c25bdeba 294 int result = 0;
ptpaterson 4:2034b04c86d2 295
ptpaterson 5:22a337cdc0e3 296 if (canOpenMsg->data[0] == nodeId) {
ptpaterson 5:22a337cdc0e3 297
ptpaterson 5:22a337cdc0e3 298 int commandSpecifier = (int)canOpenMsg->data[1];
ptpaterson 5:22a337cdc0e3 299
ptpaterson 5:22a337cdc0e3 300 switch (commandSpecifier) {
ptpaterson 5:22a337cdc0e3 301 case NMT_CS_START:
ptpaterson 5:22a337cdc0e3 302 printf(" NMT_CS_START\r\n");
ptpaterson 5:22a337cdc0e3 303 if ((State::INITIALIZED == state.nmtState) ||
ptpaterson 5:22a337cdc0e3 304 (State::PREOPERATIONAL == state.nmtState) ||
ptpaterson 5:22a337cdc0e3 305 (State::STOPPED == state.nmtState))
ptpaterson 5:22a337cdc0e3 306 {
ptpaterson 5:22a337cdc0e3 307
ptpaterson 5:22a337cdc0e3 308 state.nmtState = State::OPERATIONAL;
ptpaterson 5:22a337cdc0e3 309 state.bBoot = 0;
ptpaterson 5:22a337cdc0e3 310 state.bSDO = 1;
ptpaterson 5:22a337cdc0e3 311 state.bEmergency = 1;
ptpaterson 5:22a337cdc0e3 312 state.bSYNC = 1;
ptpaterson 5:22a337cdc0e3 313 state.bLifeGuard = 1;
ptpaterson 5:22a337cdc0e3 314 state.bPDO = 1;
ptpaterson 5:22a337cdc0e3 315 state.bLSS = 0;
ptpaterson 5:22a337cdc0e3 316 OnOperational();
ptpaterson 5:22a337cdc0e3 317 result = 1;
ptpaterson 5:22a337cdc0e3 318 }
ptpaterson 5:22a337cdc0e3 319 break;
ptpaterson 5:22a337cdc0e3 320
ptpaterson 5:22a337cdc0e3 321 case NMT_CS_STOP:
ptpaterson 5:22a337cdc0e3 322 if (State::INITIALIZED != state.nmtState) {
ptpaterson 5:22a337cdc0e3 323 state.nmtState = State::STOPPED;
ptpaterson 5:22a337cdc0e3 324 state.bBoot = 0;
ptpaterson 5:22a337cdc0e3 325 state.bSDO = 0;
ptpaterson 5:22a337cdc0e3 326 state.bEmergency = 0;
ptpaterson 5:22a337cdc0e3 327 state.bSYNC = 0;
ptpaterson 5:22a337cdc0e3 328 state.bLifeGuard = 1;
ptpaterson 5:22a337cdc0e3 329 state.bPDO = 0;
ptpaterson 5:22a337cdc0e3 330 state.bLSS = 1;
ptpaterson 5:22a337cdc0e3 331 }
ptpaterson 5:22a337cdc0e3 332 OnStopped();
ptpaterson 5:22a337cdc0e3 333 result = 1;
ptpaterson 5:22a337cdc0e3 334 break;
ptpaterson 5:22a337cdc0e3 335
ptpaterson 5:22a337cdc0e3 336 case NMT_CS_ENTER_PREOP:
ptpaterson 5:22a337cdc0e3 337 state.nmtState = State::PREOPERATIONAL;
ptpaterson 3:12b3c25bdeba 338 state.bBoot = 0;
ptpaterson 3:12b3c25bdeba 339 state.bSDO = 1;
ptpaterson 3:12b3c25bdeba 340 state.bEmergency = 1;
ptpaterson 3:12b3c25bdeba 341 state.bSYNC = 1;
ptpaterson 3:12b3c25bdeba 342 state.bLifeGuard = 1;
ptpaterson 5:22a337cdc0e3 343 state.bPDO = 0;
ptpaterson 5:22a337cdc0e3 344 state.bLSS = 1;
ptpaterson 5:22a337cdc0e3 345 OnPreoperational();
ptpaterson 3:12b3c25bdeba 346 result = 1;
ptpaterson 5:22a337cdc0e3 347 break;
ptpaterson 4:2034b04c86d2 348
ptpaterson 5:22a337cdc0e3 349 case NMT_CS_RESET_NODE:
ptpaterson 5:22a337cdc0e3 350 case NMT_CS_RESET_COM:
ptpaterson 5:22a337cdc0e3 351 printf(" NMT_CS_RESET\r\n");
ptpaterson 5:22a337cdc0e3 352
ptpaterson 5:22a337cdc0e3 353 state.nmtState = State::INITIALIZED;
ptpaterson 5:22a337cdc0e3 354 state.bBoot = 1;
ptpaterson 3:12b3c25bdeba 355 state.bSDO = 0;
ptpaterson 3:12b3c25bdeba 356 state.bEmergency = 0;
ptpaterson 3:12b3c25bdeba 357 state.bSYNC = 0;
ptpaterson 5:22a337cdc0e3 358 state.bLifeGuard = 0;
ptpaterson 5:22a337cdc0e3 359 state.bPDO = 0;
ptpaterson 5:22a337cdc0e3 360 state.bLSS = 0;
ptpaterson 5:22a337cdc0e3 361
ptpaterson 5:22a337cdc0e3 362 state.bLifeGuardToggle = 0;
ptpaterson 5:22a337cdc0e3 363
ptpaterson 5:22a337cdc0e3 364 /* boot message is actually just the first node guard/ heart beat message */
ptpaterson 5:22a337cdc0e3 365 // TODO: wrap up into heartbeat/lifeguard message
ptpaterson 5:22a337cdc0e3 366 CanOpenMessage msgBoot;
ptpaterson 5:22a337cdc0e3 367 msgBoot.id = CANOPEN_FUNCTION_CODE_NODE_GUARD | 5;
ptpaterson 5:22a337cdc0e3 368 msgBoot.format = CANOPEN_FORMAT_STANDARD;
ptpaterson 5:22a337cdc0e3 369 msgBoot.type = CANOPEN_TYPE_DATA;
ptpaterson 5:22a337cdc0e3 370 msgBoot.dataCount = 1;
ptpaterson 5:22a337cdc0e3 371 msgBoot.data[0] = 0;
ptpaterson 5:22a337cdc0e3 372
ptpaterson 5:22a337cdc0e3 373 pMyProvider->PostMessage(nodeId, &msgBoot);
ptpaterson 5:22a337cdc0e3 374
ptpaterson 5:22a337cdc0e3 375 OnInitialize();
ptpaterson 5:22a337cdc0e3 376
ptpaterson 5:22a337cdc0e3 377 state.nmtState = State::PREOPERATIONAL;
ptpaterson 5:22a337cdc0e3 378 state.bBoot = 0;
ptpaterson 5:22a337cdc0e3 379 state.bSDO = 1;
ptpaterson 5:22a337cdc0e3 380 state.bEmergency = 1;
ptpaterson 5:22a337cdc0e3 381 state.bSYNC = 1;
ptpaterson 3:12b3c25bdeba 382 state.bLifeGuard = 1;
ptpaterson 3:12b3c25bdeba 383 state.bPDO = 0;
ptpaterson 3:12b3c25bdeba 384 state.bLSS = 1;
ptpaterson 5:22a337cdc0e3 385
ptpaterson 5:22a337cdc0e3 386 OnPreoperational();
ptpaterson 4:2034b04c86d2 387
ptpaterson 5:22a337cdc0e3 388 result = 1;
ptpaterson 5:22a337cdc0e3 389 break;
ptpaterson 4:2034b04c86d2 390
ptpaterson 5:22a337cdc0e3 391 default:
ptpaterson 5:22a337cdc0e3 392 break;
ptpaterson 5:22a337cdc0e3 393 }
ptpaterson 5:22a337cdc0e3 394
ptpaterson 3:12b3c25bdeba 395 }
ptpaterson 4:2034b04c86d2 396
ptpaterson 3:12b3c25bdeba 397 return result;
ptpaterson 2:c724ff3a4e4d 398 }
ptpaterson 2:c724ff3a4e4d 399
ptpaterson 2:c724ff3a4e4d 400 int Node::HandleNodeGuardRequest (const int masterId)
ptpaterson 2:c724ff3a4e4d 401 {
ptpaterson 2:c724ff3a4e4d 402 return 0;
ptpaterson 2:c724ff3a4e4d 403 }
ptpaterson 2:c724ff3a4e4d 404
ptpaterson 2:c724ff3a4e4d 405 int Node::ConsumeHeartbeat (const int producerId)
ptpaterson 2:c724ff3a4e4d 406 {
ptpaterson 2:c724ff3a4e4d 407 return 0;
ptpaterson 2:c724ff3a4e4d 408 }
ptpaterson 2:c724ff3a4e4d 409
ptpaterson 2:c724ff3a4e4d 410 /*=============================================================================
ptpaterson 2:c724ff3a4e4d 411 * Methods to handle operation of node device
ptpaterson 2:c724ff3a4e4d 412 *=============================================================================
ptpaterson 2:c724ff3a4e4d 413 */
ptpaterson 4:2034b04c86d2 414
ptpaterson 5:22a337cdc0e3 415 void Node::FixedUpdate (uint32_t time)
ptpaterson 5:22a337cdc0e3 416 {
ptpaterson 5:22a337cdc0e3 417 timeSinceLastTick = time - timeCurrentTick;
ptpaterson 5:22a337cdc0e3 418 timeCurrentTick = time;
ptpaterson 5:22a337cdc0e3 419
ptpaterson 5:22a337cdc0e3 420 if (State::OPERATIONAL == state.nmtState) {
ptpaterson 5:22a337cdc0e3 421 OnFixedUpdate();
ptpaterson 5:22a337cdc0e3 422 }
ptpaterson 5:22a337cdc0e3 423
ptpaterson 5:22a337cdc0e3 424 }
ptpaterson 5:22a337cdc0e3 425
ptpaterson 2:c724ff3a4e4d 426 void Node::Update (void)
ptpaterson 2:c724ff3a4e4d 427 {
ptpaterson 5:22a337cdc0e3 428 if (State::OPERATIONAL == state.nmtState) {
ptpaterson 3:12b3c25bdeba 429 OnUpdate();
ptpaterson 4:2034b04c86d2 430 }
ptpaterson 2:c724ff3a4e4d 431 }
ptpaterson 2:c724ff3a4e4d 432
ptpaterson 2:c724ff3a4e4d 433 /*=============================================================================
ptpaterson 5:22a337cdc0e3 434 * Other Member Functions
ptpaterson 2:c724ff3a4e4d 435 *=============================================================================
ptpaterson 2:c724ff3a4e4d 436 */
ptpaterson 2:c724ff3a4e4d 437
ptpaterson 5:22a337cdc0e3 438 int Node::PostTPDO (int specifiedCobId)
ptpaterson 5:22a337cdc0e3 439 {
ptpaterson 5:22a337cdc0e3 440 printf(" TPDO:\r\n");
ptpaterson 5:22a337cdc0e3 441
ptpaterson 5:22a337cdc0e3 442 int result = 0;
ptpaterson 5:22a337cdc0e3 443
ptpaterson 5:22a337cdc0e3 444 /* initialize a blank message -------------------------------------------*/
ptpaterson 5:22a337cdc0e3 445 CanOpenMessage msg;
ptpaterson 5:22a337cdc0e3 446
ptpaterson 5:22a337cdc0e3 447 msg.id = specifiedCobId;
ptpaterson 5:22a337cdc0e3 448 msg.dataCount = 0;
ptpaterson 5:22a337cdc0e3 449 msg.type = CANOPEN_TYPE_DATA;
ptpaterson 5:22a337cdc0e3 450 msg.format = CANOPEN_FORMAT_STANDARD;
ptpaterson 5:22a337cdc0e3 451
ptpaterson 5:22a337cdc0e3 452 msg.data[0] = 0;
ptpaterson 5:22a337cdc0e3 453 msg.data[1] = 0;
ptpaterson 5:22a337cdc0e3 454 msg.data[2] = 0;
ptpaterson 5:22a337cdc0e3 455 msg.data[3] = 0;
ptpaterson 5:22a337cdc0e3 456 msg.data[4] = 0;
ptpaterson 5:22a337cdc0e3 457 msg.data[5] = 0;
ptpaterson 5:22a337cdc0e3 458 msg.data[6] = 0;
ptpaterson 5:22a337cdc0e3 459 msg.data[7] = 0;
ptpaterson 5:22a337cdc0e3 460
ptpaterson 5:22a337cdc0e3 461 int pdoNum = 0;
ptpaterson 5:22a337cdc0e3 462 int bSearching = 1;
ptpaterson 5:22a337cdc0e3 463
ptpaterson 5:22a337cdc0e3 464 /* Find the mapped data and send ----------------------------------------*/
ptpaterson 5:22a337cdc0e3 465
ptpaterson 5:22a337cdc0e3 466 ObjectData * parameterObject;
ptpaterson 5:22a337cdc0e3 467
ptpaterson 5:22a337cdc0e3 468 /* search through PDO parameters until we find a match */
ptpaterson 5:22a337cdc0e3 469 while (bSearching) {
ptpaterson 5:22a337cdc0e3 470
ptpaterson 5:22a337cdc0e3 471 //printf(" PDO %d?", pdoNum);
ptpaterson 5:22a337cdc0e3 472 parameterObject = ScanIndex (0x1800 + pdoNum);
ptpaterson 5:22a337cdc0e3 473 if (parameterObject) {
ptpaterson 5:22a337cdc0e3 474
ptpaterson 5:22a337cdc0e3 475 uint32_t *cobId = (uint32_t*) parameterObject->entries[1].pData;
ptpaterson 5:22a337cdc0e3 476 if (specifiedCobId == *cobId) {
ptpaterson 5:22a337cdc0e3 477 bSearching = 0;
ptpaterson 5:22a337cdc0e3 478
ptpaterson 5:22a337cdc0e3 479 //printf(" ... match!!!\r\n");
ptpaterson 5:22a337cdc0e3 480 } else {
ptpaterson 5:22a337cdc0e3 481 pdoNum++;
ptpaterson 5:22a337cdc0e3 482 //printf(" ... nope\r\n");
ptpaterson 5:22a337cdc0e3 483 }
ptpaterson 5:22a337cdc0e3 484
ptpaterson 5:22a337cdc0e3 485 } else {
ptpaterson 5:22a337cdc0e3 486 bSearching = 0;
ptpaterson 5:22a337cdc0e3 487 printf("\r\n ERROR: No PDO Parameter match found\r\n");
ptpaterson 5:22a337cdc0e3 488 }
ptpaterson 5:22a337cdc0e3 489 }
ptpaterson 5:22a337cdc0e3 490
ptpaterson 5:22a337cdc0e3 491 /* if a matching parameter object was found then get mapping */
ptpaterson 5:22a337cdc0e3 492 if (parameterObject) {
ptpaterson 5:22a337cdc0e3 493
ptpaterson 5:22a337cdc0e3 494 ObjectData *mappingObject = ScanIndex (0x1A00 + pdoNum);
ptpaterson 5:22a337cdc0e3 495
ptpaterson 5:22a337cdc0e3 496 if (mappingObject) {
ptpaterson 5:22a337cdc0e3 497 SubIndexSize *numMappedVariables = (SubIndexSize*) mappingObject->entries[0].pData;
ptpaterson 5:22a337cdc0e3 498
ptpaterson 5:22a337cdc0e3 499 //printf(" numMappedVariables: %d\r\n", *numMappedVariables);
ptpaterson 5:22a337cdc0e3 500
ptpaterson 5:22a337cdc0e3 501 /* variable to track position in data copy */
ptpaterson 5:22a337cdc0e3 502 uint8_t destBitNum = 0;
ptpaterson 5:22a337cdc0e3 503
ptpaterson 5:22a337cdc0e3 504 int bError = 0;
ptpaterson 5:22a337cdc0e3 505
ptpaterson 5:22a337cdc0e3 506 /* for each mapped variable, write in data from message */
ptpaterson 5:22a337cdc0e3 507 for (SubIndexSize subIndexIterator = 1; subIndexIterator <= *numMappedVariables && !bError; subIndexIterator++) {
ptpaterson 5:22a337cdc0e3 508
ptpaterson 5:22a337cdc0e3 509 uint32_t *map = (uint32_t*)mappingObject->entries[subIndexIterator].pData;
ptpaterson 5:22a337cdc0e3 510
ptpaterson 5:22a337cdc0e3 511 uint8_t mappedBits = (uint8_t) (*map);
ptpaterson 5:22a337cdc0e3 512 uint8_t mappedBytes = mappedBits / 8;
ptpaterson 5:22a337cdc0e3 513 SubIndexSize mappedSubIndex = (SubIndexSize) (*map >> 8);
ptpaterson 5:22a337cdc0e3 514 IndexSize mappedIndex = (IndexSize) (*map >> 16);
ptpaterson 5:22a337cdc0e3 515
ptpaterson 5:22a337cdc0e3 516 msg.dataCount += mappedBytes;
ptpaterson 5:22a337cdc0e3 517
ptpaterson 5:22a337cdc0e3 518 //printf(" mapped: %#06x, %#04x, %d\r\n",
ptpaterson 5:22a337cdc0e3 519 // mappedIndex, mappedSubIndex, mappedBits);
ptpaterson 5:22a337cdc0e3 520
ptpaterson 5:22a337cdc0e3 521 /* if less than 0x1000 then it is a dummy value */
ptpaterson 5:22a337cdc0e3 522 if (mappedIndex < 0x1000) {
ptpaterson 5:22a337cdc0e3 523 destBitNum += mappedBits;
ptpaterson 5:22a337cdc0e3 524 //printf(" No Errors, skipping VOID data...\r\n");
ptpaterson 5:22a337cdc0e3 525 } else {
ptpaterson 5:22a337cdc0e3 526
ptpaterson 5:22a337cdc0e3 527 /* push into the TPDO data */
ptpaterson 5:22a337cdc0e3 528 ObjectData *mappedObject = ScanIndex (mappedIndex);
ptpaterson 5:22a337cdc0e3 529 if (mappedObject) {
ptpaterson 5:22a337cdc0e3 530
ptpaterson 5:22a337cdc0e3 531 /* get the subindex object */
ptpaterson 5:22a337cdc0e3 532 if (mappedSubIndex <= *(SubIndexSize*) mappedObject->entries[0].pData) {
ptpaterson 5:22a337cdc0e3 533
ptpaterson 5:22a337cdc0e3 534 EntryData *sourceEntry = &mappedObject->entries[mappedSubIndex];
ptpaterson 5:22a337cdc0e3 535
ptpaterson 5:22a337cdc0e3 536 /* can we write to it? */
ptpaterson 5:22a337cdc0e3 537 if (!(sourceEntry->properties & EntryData::PROPERTY_READABLE)) {
ptpaterson 5:22a337cdc0e3 538 printf(" ERROR: Mapped SubIndex is not readable!\r\n");
ptpaterson 5:22a337cdc0e3 539 bError = 1;
ptpaterson 5:22a337cdc0e3 540 }
ptpaterson 5:22a337cdc0e3 541
ptpaterson 5:22a337cdc0e3 542 uint8_t destByteNum = destBitNum / 8;
ptpaterson 5:22a337cdc0e3 543 if (destByteNum + mappedBytes > 8) {
ptpaterson 5:22a337cdc0e3 544 printf(" ERROR: Too much data to pack into destination!\r\n");
ptpaterson 5:22a337cdc0e3 545 bError = 1;
ptpaterson 5:22a337cdc0e3 546 }
ptpaterson 5:22a337cdc0e3 547
ptpaterson 5:22a337cdc0e3 548 if (sourceEntry->size < mappedBytes) {
ptpaterson 5:22a337cdc0e3 549 printf(" ERROR: trying to grab too much information!\r\n");
ptpaterson 5:22a337cdc0e3 550 bError = 1;
ptpaterson 5:22a337cdc0e3 551 }
ptpaterson 5:22a337cdc0e3 552
ptpaterson 5:22a337cdc0e3 553 if (!bError) {
ptpaterson 5:22a337cdc0e3 554
ptpaterson 5:22a337cdc0e3 555 //printf(" No Errors, copying data...\r\n");
ptpaterson 5:22a337cdc0e3 556
ptpaterson 5:22a337cdc0e3 557 uint8_t *destData = msg.data;
ptpaterson 5:22a337cdc0e3 558 uint8_t *sourceData = (uint8_t*)sourceEntry->pData;
ptpaterson 5:22a337cdc0e3 559
ptpaterson 5:22a337cdc0e3 560 uint8_t sourceBitNum = 0;
ptpaterson 5:22a337cdc0e3 561 CopyBits(sourceData, destData, mappedBits, sourceBitNum, destBitNum);
ptpaterson 5:22a337cdc0e3 562 }
ptpaterson 5:22a337cdc0e3 563
ptpaterson 5:22a337cdc0e3 564 } else {
ptpaterson 5:22a337cdc0e3 565 printf(" ERROR: Mapped SubIndex does not exist!\r\n");
ptpaterson 5:22a337cdc0e3 566 bError = 1;
ptpaterson 5:22a337cdc0e3 567 }
ptpaterson 5:22a337cdc0e3 568
ptpaterson 5:22a337cdc0e3 569 } else {
ptpaterson 5:22a337cdc0e3 570 printf(" ERROR: Mapped Index does not exist!\r\n");
ptpaterson 5:22a337cdc0e3 571 bError = 1;
ptpaterson 5:22a337cdc0e3 572 }
ptpaterson 5:22a337cdc0e3 573
ptpaterson 5:22a337cdc0e3 574 } /* if mappedIndex < 0x1000 */
ptpaterson 5:22a337cdc0e3 575
ptpaterson 5:22a337cdc0e3 576
ptpaterson 5:22a337cdc0e3 577 } /* for each mapping */
ptpaterson 5:22a337cdc0e3 578
ptpaterson 5:22a337cdc0e3 579 if (!bError) {
ptpaterson 5:22a337cdc0e3 580 result = 1;
ptpaterson 5:22a337cdc0e3 581
ptpaterson 5:22a337cdc0e3 582 /* Send the message we built up */
ptpaterson 5:22a337cdc0e3 583 pMyProvider->PostMessage(nodeId, &msg);
ptpaterson 5:22a337cdc0e3 584 }
ptpaterson 5:22a337cdc0e3 585
ptpaterson 5:22a337cdc0e3 586 } else {
ptpaterson 5:22a337cdc0e3 587 printf(" ERROR: No PDO Mapping match found\r\n");
ptpaterson 5:22a337cdc0e3 588 } /* if mappingObject exists */
ptpaterson 5:22a337cdc0e3 589
ptpaterson 5:22a337cdc0e3 590 } /* if parameter exists */
ptpaterson 5:22a337cdc0e3 591
ptpaterson 5:22a337cdc0e3 592
ptpaterson 5:22a337cdc0e3 593 return result;
ptpaterson 5:22a337cdc0e3 594 }
ptpaterson 5:22a337cdc0e3 595
ptpaterson 5:22a337cdc0e3 596 /*=============================================================================
ptpaterson 5:22a337cdc0e3 597 * Private functions
ptpaterson 5:22a337cdc0e3 598 *=============================================================================
ptpaterson 5:22a337cdc0e3 599 */
ptpaterson 5:22a337cdc0e3 600
ptpaterson 5:22a337cdc0e3 601 void ChangeState(int newState) {
ptpaterson 5:22a337cdc0e3 602
ptpaterson 5:22a337cdc0e3 603 }
ptpaterson 5:22a337cdc0e3 604
ptpaterson 2:c724ff3a4e4d 605 /*=============================================================================
ptpaterson 2:c724ff3a4e4d 606 * Local functions
ptpaterson 2:c724ff3a4e4d 607 *=============================================================================
ptpaterson 2:c724ff3a4e4d 608 */
ptpaterson 4:2034b04c86d2 609
ptpaterson 5:22a337cdc0e3 610 void CopyBits(uint8_t *sourceData,
ptpaterson 5:22a337cdc0e3 611 uint8_t *destData,
ptpaterson 5:22a337cdc0e3 612 uint8_t mappedBits,
ptpaterson 5:22a337cdc0e3 613 uint8_t &sourceBitNum,
ptpaterson 5:22a337cdc0e3 614 uint8_t &destBitNum)
ptpaterson 5:22a337cdc0e3 615 {
ptpaterson 5:22a337cdc0e3 616 uint8_t sourceByteNum;
ptpaterson 5:22a337cdc0e3 617 uint8_t destByteNum;
ptpaterson 4:2034b04c86d2 618
ptpaterson 5:22a337cdc0e3 619 uint8_t bitCounter = 0;
ptpaterson 5:22a337cdc0e3 620
ptpaterson 5:22a337cdc0e3 621 if ((mappedBits % 8) == 0 && (sourceBitNum % 8) == 0 && (destBitNum % 8) == 0) {
ptpaterson 5:22a337cdc0e3 622
ptpaterson 5:22a337cdc0e3 623 //printf(" Loading BYTEwise...\r\n");
ptpaterson 5:22a337cdc0e3 624 /* load in by bytes */
ptpaterson 5:22a337cdc0e3 625 uint8_t destByteNum = 0;
ptpaterson 5:22a337cdc0e3 626 while (bitCounter < mappedBits) {
ptpaterson 5:22a337cdc0e3 627
ptpaterson 5:22a337cdc0e3 628
ptpaterson 5:22a337cdc0e3 629 destByteNum = destBitNum / 8;
ptpaterson 5:22a337cdc0e3 630 sourceByteNum = sourceBitNum / 8;
ptpaterson 5:22a337cdc0e3 631
ptpaterson 5:22a337cdc0e3 632 destData[destByteNum] = sourceData[sourceByteNum];
ptpaterson 5:22a337cdc0e3 633
ptpaterson 5:22a337cdc0e3 634 destBitNum += 8;
ptpaterson 5:22a337cdc0e3 635 sourceBitNum += 8;
ptpaterson 5:22a337cdc0e3 636 bitCounter += 8;
ptpaterson 5:22a337cdc0e3 637 }
ptpaterson 5:22a337cdc0e3 638
ptpaterson 5:22a337cdc0e3 639 } else {
ptpaterson 5:22a337cdc0e3 640
ptpaterson 5:22a337cdc0e3 641
ptpaterson 5:22a337cdc0e3 642 //printf(" Loading BITwise...");
ptpaterson 5:22a337cdc0e3 643 /* not a multiple of 8, so do bit by bit */
ptpaterson 5:22a337cdc0e3 644 while (bitCounter < mappedBits) {
ptpaterson 5:22a337cdc0e3 645
ptpaterson 5:22a337cdc0e3 646 destByteNum = destBitNum / 8;
ptpaterson 5:22a337cdc0e3 647 sourceByteNum = sourceBitNum / 8;
ptpaterson 5:22a337cdc0e3 648
ptpaterson 5:22a337cdc0e3 649 /* clear the destination bit */
ptpaterson 5:22a337cdc0e3 650 destData[destByteNum] &= ~(1 << destBitNum);
ptpaterson 5:22a337cdc0e3 651 /* get source bit value */
ptpaterson 5:22a337cdc0e3 652 uint8_t destValue = (sourceData[sourceByteNum] & (1 << (sourceBitNum % 8))) >> (sourceBitNum % 8) << destBitNum;
ptpaterson 5:22a337cdc0e3 653 /* set dest bit */
ptpaterson 5:22a337cdc0e3 654 destData[destByteNum] |= destValue;
ptpaterson 5:22a337cdc0e3 655
ptpaterson 5:22a337cdc0e3 656 destBitNum++;
ptpaterson 5:22a337cdc0e3 657 sourceBitNum++;
ptpaterson 5:22a337cdc0e3 658 bitCounter++;
ptpaterson 5:22a337cdc0e3 659 }
ptpaterson 5:22a337cdc0e3 660 }
ptpaterson 5:22a337cdc0e3 661 }
ptpaterson 4:2034b04c86d2 662
ptpaterson 4:2034b04c86d2 663 /*=============================================================================
ptpaterson 5:22a337cdc0e3 664 *=============================================================================
ptpaterson 4:2034b04c86d2 665 * Methods to handle message requests and responses
ptpaterson 4:2034b04c86d2 666 * Called by the node, usually during Update() or during handling of
ptpaterson 4:2034b04c86d2 667 * incoming messages.
ptpaterson 4:2034b04c86d2 668 *
ptpaterson 4:2034b04c86d2 669 * Removed from Service ProviderClass. Not sure if we will need these in the future
ptpaterson 4:2034b04c86d2 670 *=============================================================================
ptpaterson 4:2034b04c86d2 671 *=============================================================================
ptpaterson 4:2034b04c86d2 672 */
ptpaterson 4:2034b04c86d2 673
ptpaterson 4:2034b04c86d2 674 /* PDO (7.2.2), MPDO (7.2.3) --------------------------------------------*/
ptpaterson 4:2034b04c86d2 675
ptpaterson 4:2034b04c86d2 676 /** Build and send a PDO request
ptpaterson 4:2034b04c86d2 677 * @note
ptpaterson 4:2034b04c86d2 678 * @param
ptpaterson 4:2034b04c86d2 679 */
ptpaterson 5:22a337cdc0e3 680 void RequestPdo (int pdoNum) {}
ptpaterson 4:2034b04c86d2 681
ptpaterson 4:2034b04c86d2 682 /** Build and send a PDO
ptpaterson 4:2034b04c86d2 683 * @note
ptpaterson 4:2034b04c86d2 684 * @param
ptpaterson 4:2034b04c86d2 685 */
ptpaterson 5:22a337cdc0e3 686 void ProducePdo (int pdoNum, char * data) {}
ptpaterson 4:2034b04c86d2 687
ptpaterson 4:2034b04c86d2 688
ptpaterson 4:2034b04c86d2 689 /* SDO (7.2.4) ----------------------------------------------------------*/
ptpaterson 4:2034b04c86d2 690
ptpaterson 4:2034b04c86d2 691 /** initiate SDO download
ptpaterson 4:2034b04c86d2 692 * @note Handles automatically whether it will be a expedited transfer or
ptpaterson 5:22a337cdc0e3 693 * or if message will be split into
ptpaterson 4:2034b04c86d2 694 * @param
ptpaterson 4:2034b04c86d2 695 *
ptpaterson 4:2034b04c86d2 696 * Node will create a big data array and Service provide will have to
ptpaterson 4:2034b04c86d2 697 * iterate through and send all of the data. ServiceProvider will pass
ptpaterson 4:2034b04c86d2 698 * the confirmation to the node, and the node will free up it's buffer.
ptpaterson 4:2034b04c86d2 699 */
ptpaterson 5:22a337cdc0e3 700 void DownloadSdo (int sdoNum, int index, int subindex, int size, char * data) {}
ptpaterson 4:2034b04c86d2 701
ptpaterson 4:2034b04c86d2 702 /** initiate SDO upload
ptpaterson 5:22a337cdc0e3 703 * @note
ptpaterson 4:2034b04c86d2 704 * @param
ptpaterson 4:2034b04c86d2 705 */
ptpaterson 5:22a337cdc0e3 706 void UploadSdo (int sdoNum, int index, int subindex) {}
ptpaterson 4:2034b04c86d2 707
ptpaterson 4:2034b04c86d2 708 /** Acknowledge that SDO was recieved properly
ptpaterson 5:22a337cdc0e3 709 * @note
ptpaterson 4:2034b04c86d2 710 * @param
ptpaterson 4:2034b04c86d2 711 */
ptpaterson 5:22a337cdc0e3 712 void ConfirmSdo (int sdoNum, int bSuccess) {}
ptpaterson 4:2034b04c86d2 713
ptpaterson 4:2034b04c86d2 714 /** Abort current SDO transfer
ptpaterson 5:22a337cdc0e3 715 * @note
ptpaterson 4:2034b04c86d2 716 * @param
ptpaterson 4:2034b04c86d2 717 */
ptpaterson 5:22a337cdc0e3 718 void AbortSdo (int sdoNum) {}
ptpaterson 4:2034b04c86d2 719
ptpaterson 4:2034b04c86d2 720
ptpaterson 4:2034b04c86d2 721 /* Emergency object (7.2.7) ---------------------------------------------*/
ptpaterson 4:2034b04c86d2 722
ptpaterson 4:2034b04c86d2 723 // TODO: emergency producer
ptpaterson 4:2034b04c86d2 724
ptpaterson 4:2034b04c86d2 725
ptpaterson 4:2034b04c86d2 726 /* Network Management (7.2.8) -------------------------------------------*/
ptpaterson 4:2034b04c86d2 727 /* ---- Node Control (7.2.8.2.1) ----------------------------------------*/
ptpaterson 4:2034b04c86d2 728
ptpaterson 4:2034b04c86d2 729 /** Build a CANOpen nmt control message to a node
ptpaterson 4:2034b04c86d2 730 * @note
ptpaterson 4:2034b04c86d2 731 * @param
ptpaterson 4:2034b04c86d2 732 */
ptpaterson 5:22a337cdc0e3 733 int SendNodeControl (NmtCommandSpecifier cs, unsigned int nodeId)
ptpaterson 5:22a337cdc0e3 734 {
ptpaterson 5:22a337cdc0e3 735 return 0;
ptpaterson 5:22a337cdc0e3 736 }
ptpaterson 4:2034b04c86d2 737
ptpaterson 4:2034b04c86d2 738
ptpaterson 4:2034b04c86d2 739 /* ---- Error Control (7.2.8.2.2) ---------------------------------------*/
ptpaterson 4:2034b04c86d2 740
ptpaterson 4:2034b04c86d2 741 /** Build a CANOpen error control request to a node
ptpaterson 4:2034b04c86d2 742 * @note
ptpaterson 4:2034b04c86d2 743 * @param
ptpaterson 4:2034b04c86d2 744 */
ptpaterson 5:22a337cdc0e3 745 int RequestErrorControl (NmtCommandSpecifier cs, unsigned int nodeId)
ptpaterson 5:22a337cdc0e3 746 {
ptpaterson 5:22a337cdc0e3 747 return 0;
ptpaterson 5:22a337cdc0e3 748 }
ptpaterson 4:2034b04c86d2 749
ptpaterson 4:2034b04c86d2 750 /** Build a CANOpen error control response
ptpaterson 4:2034b04c86d2 751 * @note
ptpaterson 4:2034b04c86d2 752 * @param
ptpaterson 4:2034b04c86d2 753 */
ptpaterson 5:22a337cdc0e3 754 int RespondErrorControl (NmtCommandSpecifier cs, unsigned int nodeId)
ptpaterson 5:22a337cdc0e3 755 {
ptpaterson 5:22a337cdc0e3 756 return 0;
ptpaterson 5:22a337cdc0e3 757 }
ptpaterson 4:2034b04c86d2 758
ptpaterson 2:c724ff3a4e4d 759
ptpaterson 2:c724ff3a4e4d 760 } /* namspace ppCANOpen */
ptpaterson 2:c724ff3a4e4d 761
ptpaterson 2:c724ff3a4e4d 762