![](/media/cache/profiles/IMG_2753.JPG.50x50_q85.jpg)
Can_open_masternode
framework/src/states.c@0:9dd7c6129683, 2011-05-30 (annotated)
- Committer:
- sam_grove
- Date:
- Mon May 30 07:36:47 2011 +0000
- Revision:
- 0:9dd7c6129683
Initial public release of master node example
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sam_grove |
0:9dd7c6129683 | 1 | /* |
sam_grove |
0:9dd7c6129683 | 2 | This file is part of CanFestival, a library implementing CanOpen Stack. |
sam_grove |
0:9dd7c6129683 | 3 | |
sam_grove |
0:9dd7c6129683 | 4 | Copyright (C): Edouard TISSERANT and Francis DUPIN |
sam_grove |
0:9dd7c6129683 | 5 | |
sam_grove |
0:9dd7c6129683 | 6 | See COPYING file for copyrights details. |
sam_grove |
0:9dd7c6129683 | 7 | |
sam_grove |
0:9dd7c6129683 | 8 | This library is free software; you can redistribute it and/or |
sam_grove |
0:9dd7c6129683 | 9 | modify it under the terms of the GNU Lesser General Public |
sam_grove |
0:9dd7c6129683 | 10 | License as published by the Free Software Foundation; either |
sam_grove |
0:9dd7c6129683 | 11 | version 2.1 of the License, or (at your option) any later version. |
sam_grove |
0:9dd7c6129683 | 12 | |
sam_grove |
0:9dd7c6129683 | 13 | This library is distributed in the hope that it will be useful, |
sam_grove |
0:9dd7c6129683 | 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
sam_grove |
0:9dd7c6129683 | 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
sam_grove |
0:9dd7c6129683 | 16 | Lesser General Public License for more details. |
sam_grove |
0:9dd7c6129683 | 17 | |
sam_grove |
0:9dd7c6129683 | 18 | You should have received a copy of the GNU Lesser General Public |
sam_grove |
0:9dd7c6129683 | 19 | License along with this library; if not, write to the Free Software |
sam_grove |
0:9dd7c6129683 | 20 | Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
sam_grove |
0:9dd7c6129683 | 21 | */ |
sam_grove |
0:9dd7c6129683 | 22 | /*! |
sam_grove |
0:9dd7c6129683 | 23 | ** @file states.c |
sam_grove |
0:9dd7c6129683 | 24 | ** @author Edouard TISSERANT and Francis DUPIN |
sam_grove |
0:9dd7c6129683 | 25 | ** @date Tue Jun 5 09:32:32 2007 |
sam_grove |
0:9dd7c6129683 | 26 | ** |
sam_grove |
0:9dd7c6129683 | 27 | ** @brief |
sam_grove |
0:9dd7c6129683 | 28 | ** |
sam_grove |
0:9dd7c6129683 | 29 | ** |
sam_grove |
0:9dd7c6129683 | 30 | */ |
sam_grove |
0:9dd7c6129683 | 31 | |
sam_grove |
0:9dd7c6129683 | 32 | #include "data.h" |
sam_grove |
0:9dd7c6129683 | 33 | #include "sysdep.h" |
sam_grove |
0:9dd7c6129683 | 34 | |
sam_grove |
0:9dd7c6129683 | 35 | /** Prototypes for internals functions */ |
sam_grove |
0:9dd7c6129683 | 36 | /*! |
sam_grove |
0:9dd7c6129683 | 37 | ** |
sam_grove |
0:9dd7c6129683 | 38 | ** |
sam_grove |
0:9dd7c6129683 | 39 | ** @param d |
sam_grove |
0:9dd7c6129683 | 40 | ** @param newCommunicationState |
sam_grove |
0:9dd7c6129683 | 41 | **/ |
sam_grove |
0:9dd7c6129683 | 42 | void switchCommunicationState(CO_Data* d, |
sam_grove |
0:9dd7c6129683 | 43 | s_state_communication *newCommunicationState); |
sam_grove |
0:9dd7c6129683 | 44 | |
sam_grove |
0:9dd7c6129683 | 45 | /*! |
sam_grove |
0:9dd7c6129683 | 46 | ** |
sam_grove |
0:9dd7c6129683 | 47 | ** |
sam_grove |
0:9dd7c6129683 | 48 | ** @param d |
sam_grove |
0:9dd7c6129683 | 49 | ** |
sam_grove |
0:9dd7c6129683 | 50 | ** @return |
sam_grove |
0:9dd7c6129683 | 51 | **/ |
sam_grove |
0:9dd7c6129683 | 52 | e_nodeState getState(CO_Data* d) |
sam_grove |
0:9dd7c6129683 | 53 | { |
sam_grove |
0:9dd7c6129683 | 54 | return d->nodeState; |
sam_grove |
0:9dd7c6129683 | 55 | } |
sam_grove |
0:9dd7c6129683 | 56 | |
sam_grove |
0:9dd7c6129683 | 57 | /*! |
sam_grove |
0:9dd7c6129683 | 58 | ** |
sam_grove |
0:9dd7c6129683 | 59 | ** |
sam_grove |
0:9dd7c6129683 | 60 | ** @param d |
sam_grove |
0:9dd7c6129683 | 61 | ** @param m |
sam_grove |
0:9dd7c6129683 | 62 | **/ |
sam_grove |
0:9dd7c6129683 | 63 | void canDispatch(CO_Data* d, Message *m) |
sam_grove |
0:9dd7c6129683 | 64 | { |
sam_grove |
0:9dd7c6129683 | 65 | UNS16 cob_id = UNS16_LE(m->cob_id); |
sam_grove |
0:9dd7c6129683 | 66 | switch(cob_id >> 7) |
sam_grove |
0:9dd7c6129683 | 67 | { |
sam_grove |
0:9dd7c6129683 | 68 | case SYNC: /* can be a SYNC or a EMCY message */ |
sam_grove |
0:9dd7c6129683 | 69 | if(cob_id == 0x080) /* SYNC */ |
sam_grove |
0:9dd7c6129683 | 70 | { |
sam_grove |
0:9dd7c6129683 | 71 | if(d->CurrentCommunicationState.csSYNC) |
sam_grove |
0:9dd7c6129683 | 72 | proceedSYNC(d); |
sam_grove |
0:9dd7c6129683 | 73 | } else /* EMCY */ |
sam_grove |
0:9dd7c6129683 | 74 | if(d->CurrentCommunicationState.csEmergency) |
sam_grove |
0:9dd7c6129683 | 75 | proceedEMCY(d,m); |
sam_grove |
0:9dd7c6129683 | 76 | break; |
sam_grove |
0:9dd7c6129683 | 77 | /* case TIME_STAMP: */ |
sam_grove |
0:9dd7c6129683 | 78 | case PDO1tx: |
sam_grove |
0:9dd7c6129683 | 79 | case PDO1rx: |
sam_grove |
0:9dd7c6129683 | 80 | case PDO2tx: |
sam_grove |
0:9dd7c6129683 | 81 | case PDO2rx: |
sam_grove |
0:9dd7c6129683 | 82 | case PDO3tx: |
sam_grove |
0:9dd7c6129683 | 83 | case PDO3rx: |
sam_grove |
0:9dd7c6129683 | 84 | case PDO4tx: |
sam_grove |
0:9dd7c6129683 | 85 | case PDO4rx: |
sam_grove |
0:9dd7c6129683 | 86 | if (d->CurrentCommunicationState.csPDO) |
sam_grove |
0:9dd7c6129683 | 87 | proceedPDO(d,m); |
sam_grove |
0:9dd7c6129683 | 88 | break; |
sam_grove |
0:9dd7c6129683 | 89 | case SDOtx: |
sam_grove |
0:9dd7c6129683 | 90 | case SDOrx: |
sam_grove |
0:9dd7c6129683 | 91 | if (d->CurrentCommunicationState.csSDO) |
sam_grove |
0:9dd7c6129683 | 92 | proceedSDO(d,m); |
sam_grove |
0:9dd7c6129683 | 93 | break; |
sam_grove |
0:9dd7c6129683 | 94 | case NODE_GUARD: |
sam_grove |
0:9dd7c6129683 | 95 | if (d->CurrentCommunicationState.csHeartbeat) |
sam_grove |
0:9dd7c6129683 | 96 | proceedNODE_GUARD(d,m); |
sam_grove |
0:9dd7c6129683 | 97 | break; |
sam_grove |
0:9dd7c6129683 | 98 | case NMT: |
sam_grove |
0:9dd7c6129683 | 99 | if (*(d->iam_a_slave)) |
sam_grove |
0:9dd7c6129683 | 100 | { |
sam_grove |
0:9dd7c6129683 | 101 | proceedNMTstateChange(d,m); |
sam_grove |
0:9dd7c6129683 | 102 | } |
sam_grove |
0:9dd7c6129683 | 103 | #ifdef CO_ENABLE_LSS |
sam_grove |
0:9dd7c6129683 | 104 | case LSS: |
sam_grove |
0:9dd7c6129683 | 105 | if (!d->CurrentCommunicationState.csLSS)break; |
sam_grove |
0:9dd7c6129683 | 106 | if ((*(d->iam_a_slave)) && cob_id==MLSS_ADRESS) |
sam_grove |
0:9dd7c6129683 | 107 | { |
sam_grove |
0:9dd7c6129683 | 108 | proceedLSS_Slave(d,m); |
sam_grove |
0:9dd7c6129683 | 109 | } |
sam_grove |
0:9dd7c6129683 | 110 | else if(!(*(d->iam_a_slave)) && cob_id==SLSS_ADRESS) |
sam_grove |
0:9dd7c6129683 | 111 | { |
sam_grove |
0:9dd7c6129683 | 112 | proceedLSS_Master(d,m); |
sam_grove |
0:9dd7c6129683 | 113 | } |
sam_grove |
0:9dd7c6129683 | 114 | break; |
sam_grove |
0:9dd7c6129683 | 115 | #endif |
sam_grove |
0:9dd7c6129683 | 116 | } |
sam_grove |
0:9dd7c6129683 | 117 | } |
sam_grove |
0:9dd7c6129683 | 118 | |
sam_grove |
0:9dd7c6129683 | 119 | #define StartOrStop(CommType, FuncStart, FuncStop) \ |
sam_grove |
0:9dd7c6129683 | 120 | if(newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 0){\ |
sam_grove |
0:9dd7c6129683 | 121 | MSG_WAR(0x9999,#FuncStart, 9999);\ |
sam_grove |
0:9dd7c6129683 | 122 | d->CurrentCommunicationState.CommType = 1;\ |
sam_grove |
0:9dd7c6129683 | 123 | FuncStart;\ |
sam_grove |
0:9dd7c6129683 | 124 | }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 1){\ |
sam_grove |
0:9dd7c6129683 | 125 | MSG_WAR(0x9999,#FuncStop, 9999);\ |
sam_grove |
0:9dd7c6129683 | 126 | d->CurrentCommunicationState.CommType = 0;\ |
sam_grove |
0:9dd7c6129683 | 127 | FuncStop;\ |
sam_grove |
0:9dd7c6129683 | 128 | } |
sam_grove |
0:9dd7c6129683 | 129 | #define None |
sam_grove |
0:9dd7c6129683 | 130 | |
sam_grove |
0:9dd7c6129683 | 131 | /*! |
sam_grove |
0:9dd7c6129683 | 132 | ** |
sam_grove |
0:9dd7c6129683 | 133 | ** |
sam_grove |
0:9dd7c6129683 | 134 | ** @param d |
sam_grove |
0:9dd7c6129683 | 135 | ** @param newCommunicationState |
sam_grove |
0:9dd7c6129683 | 136 | **/ |
sam_grove |
0:9dd7c6129683 | 137 | void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) |
sam_grove |
0:9dd7c6129683 | 138 | { |
sam_grove |
0:9dd7c6129683 | 139 | #ifdef CO_ENABLE_LSS |
sam_grove |
0:9dd7c6129683 | 140 | StartOrStop(csLSS, startLSS(d), stopLSS(d)) |
sam_grove |
0:9dd7c6129683 | 141 | #endif |
sam_grove |
0:9dd7c6129683 | 142 | StartOrStop(csSDO, None, resetSDO(d)) |
sam_grove |
0:9dd7c6129683 | 143 | StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) |
sam_grove |
0:9dd7c6129683 | 144 | StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) |
sam_grove |
0:9dd7c6129683 | 145 | StartOrStop(csEmergency, emergencyInit(d), emergencyStop(d)) |
sam_grove |
0:9dd7c6129683 | 146 | StartOrStop(csPDO, PDOInit(d), PDOStop(d)) |
sam_grove |
0:9dd7c6129683 | 147 | StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) |
sam_grove |
0:9dd7c6129683 | 148 | } |
sam_grove |
0:9dd7c6129683 | 149 | |
sam_grove |
0:9dd7c6129683 | 150 | /*! |
sam_grove |
0:9dd7c6129683 | 151 | ** |
sam_grove |
0:9dd7c6129683 | 152 | ** |
sam_grove |
0:9dd7c6129683 | 153 | ** @param d |
sam_grove |
0:9dd7c6129683 | 154 | ** @param newState |
sam_grove |
0:9dd7c6129683 | 155 | ** |
sam_grove |
0:9dd7c6129683 | 156 | ** @return |
sam_grove |
0:9dd7c6129683 | 157 | **/ |
sam_grove |
0:9dd7c6129683 | 158 | UNS8 setState(CO_Data* d, e_nodeState newState) |
sam_grove |
0:9dd7c6129683 | 159 | { |
sam_grove |
0:9dd7c6129683 | 160 | if(newState != d->nodeState){ |
sam_grove |
0:9dd7c6129683 | 161 | switch( newState ){ |
sam_grove |
0:9dd7c6129683 | 162 | case Initialisation: |
sam_grove |
0:9dd7c6129683 | 163 | { |
sam_grove |
0:9dd7c6129683 | 164 | s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0, 0}; |
sam_grove |
0:9dd7c6129683 | 165 | d->nodeState = Initialisation; |
sam_grove |
0:9dd7c6129683 | 166 | switchCommunicationState(d, &newCommunicationState); |
sam_grove |
0:9dd7c6129683 | 167 | /* call user app init callback now. */ |
sam_grove |
0:9dd7c6129683 | 168 | /* d->initialisation MUST NOT CALL SetState */ |
sam_grove |
0:9dd7c6129683 | 169 | (*d->initialisation)(d); |
sam_grove |
0:9dd7c6129683 | 170 | } |
sam_grove |
0:9dd7c6129683 | 171 | |
sam_grove |
0:9dd7c6129683 | 172 | /* Automatic transition - No break statement ! */ |
sam_grove |
0:9dd7c6129683 | 173 | /* Transition from Initialisation to Pre_operational */ |
sam_grove |
0:9dd7c6129683 | 174 | /* is automatic as defined in DS301. */ |
sam_grove |
0:9dd7c6129683 | 175 | /* App don't have to call SetState(d, Pre_operational) */ |
sam_grove |
0:9dd7c6129683 | 176 | |
sam_grove |
0:9dd7c6129683 | 177 | case Pre_operational: |
sam_grove |
0:9dd7c6129683 | 178 | { |
sam_grove |
0:9dd7c6129683 | 179 | |
sam_grove |
0:9dd7c6129683 | 180 | s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0, 1}; |
sam_grove |
0:9dd7c6129683 | 181 | d->nodeState = Pre_operational; |
sam_grove |
0:9dd7c6129683 | 182 | switchCommunicationState(d, &newCommunicationState); |
sam_grove |
0:9dd7c6129683 | 183 | if (!(*(d->iam_a_slave))) |
sam_grove |
0:9dd7c6129683 | 184 | { |
sam_grove |
0:9dd7c6129683 | 185 | masterSendNMTstateChange (d, 0, NMT_Reset_Node); |
sam_grove |
0:9dd7c6129683 | 186 | } |
sam_grove |
0:9dd7c6129683 | 187 | (*d->preOperational)(d); |
sam_grove |
0:9dd7c6129683 | 188 | } |
sam_grove |
0:9dd7c6129683 | 189 | break; |
sam_grove |
0:9dd7c6129683 | 190 | |
sam_grove |
0:9dd7c6129683 | 191 | case Operational: |
sam_grove |
0:9dd7c6129683 | 192 | if(d->nodeState == Initialisation) return 0xFF; |
sam_grove |
0:9dd7c6129683 | 193 | { |
sam_grove |
0:9dd7c6129683 | 194 | s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1, 0}; |
sam_grove |
0:9dd7c6129683 | 195 | d->nodeState = Operational; |
sam_grove |
0:9dd7c6129683 | 196 | newState = Operational; |
sam_grove |
0:9dd7c6129683 | 197 | switchCommunicationState(d, &newCommunicationState); |
sam_grove |
0:9dd7c6129683 | 198 | (*d->operational)(d); |
sam_grove |
0:9dd7c6129683 | 199 | } |
sam_grove |
0:9dd7c6129683 | 200 | break; |
sam_grove |
0:9dd7c6129683 | 201 | |
sam_grove |
0:9dd7c6129683 | 202 | case Stopped: |
sam_grove |
0:9dd7c6129683 | 203 | if(d->nodeState == Initialisation) return 0xFF; |
sam_grove |
0:9dd7c6129683 | 204 | { |
sam_grove |
0:9dd7c6129683 | 205 | s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0, 1}; |
sam_grove |
0:9dd7c6129683 | 206 | d->nodeState = Stopped; |
sam_grove |
0:9dd7c6129683 | 207 | newState = Stopped; |
sam_grove |
0:9dd7c6129683 | 208 | switchCommunicationState(d, &newCommunicationState); |
sam_grove |
0:9dd7c6129683 | 209 | (*d->stopped)(d); |
sam_grove |
0:9dd7c6129683 | 210 | } |
sam_grove |
0:9dd7c6129683 | 211 | break; |
sam_grove |
0:9dd7c6129683 | 212 | default: |
sam_grove |
0:9dd7c6129683 | 213 | return 0xFF; |
sam_grove |
0:9dd7c6129683 | 214 | |
sam_grove |
0:9dd7c6129683 | 215 | }/* end switch case */ |
sam_grove |
0:9dd7c6129683 | 216 | |
sam_grove |
0:9dd7c6129683 | 217 | } |
sam_grove |
0:9dd7c6129683 | 218 | /* d->nodeState contains the final state */ |
sam_grove |
0:9dd7c6129683 | 219 | /* may not be the requested state */ |
sam_grove |
0:9dd7c6129683 | 220 | return d->nodeState; |
sam_grove |
0:9dd7c6129683 | 221 | } |
sam_grove |
0:9dd7c6129683 | 222 | |
sam_grove |
0:9dd7c6129683 | 223 | /*! |
sam_grove |
0:9dd7c6129683 | 224 | ** |
sam_grove |
0:9dd7c6129683 | 225 | ** |
sam_grove |
0:9dd7c6129683 | 226 | ** @param d |
sam_grove |
0:9dd7c6129683 | 227 | ** |
sam_grove |
0:9dd7c6129683 | 228 | ** @return |
sam_grove |
0:9dd7c6129683 | 229 | **/ |
sam_grove |
0:9dd7c6129683 | 230 | UNS8 getNodeId(CO_Data* d) |
sam_grove |
0:9dd7c6129683 | 231 | { |
sam_grove |
0:9dd7c6129683 | 232 | return *d->bDeviceNodeId; |
sam_grove |
0:9dd7c6129683 | 233 | } |
sam_grove |
0:9dd7c6129683 | 234 | |
sam_grove |
0:9dd7c6129683 | 235 | /*! |
sam_grove |
0:9dd7c6129683 | 236 | ** |
sam_grove |
0:9dd7c6129683 | 237 | ** |
sam_grove |
0:9dd7c6129683 | 238 | ** @param d |
sam_grove |
0:9dd7c6129683 | 239 | ** @param nodeId |
sam_grove |
0:9dd7c6129683 | 240 | **/ |
sam_grove |
0:9dd7c6129683 | 241 | void setNodeId(CO_Data* d, UNS8 nodeId) |
sam_grove |
0:9dd7c6129683 | 242 | { |
sam_grove |
0:9dd7c6129683 | 243 | UNS16 offset = d->firstIndex->SDO_SVR; |
sam_grove |
0:9dd7c6129683 | 244 | |
sam_grove |
0:9dd7c6129683 | 245 | #ifdef CO_ENABLE_LSS |
sam_grove |
0:9dd7c6129683 | 246 | d->lss_transfer.nodeID=nodeId; |
sam_grove |
0:9dd7c6129683 | 247 | if(nodeId==0xFF){ |
sam_grove |
0:9dd7c6129683 | 248 | *d->bDeviceNodeId = nodeId; |
sam_grove |
0:9dd7c6129683 | 249 | return; |
sam_grove |
0:9dd7c6129683 | 250 | } |
sam_grove |
0:9dd7c6129683 | 251 | else |
sam_grove |
0:9dd7c6129683 | 252 | #endif |
sam_grove |
0:9dd7c6129683 | 253 | if(!(nodeId>0 && nodeId<=127)){ |
sam_grove |
0:9dd7c6129683 | 254 | MSG_WAR(0x2D01, "Invalid NodeID",nodeId); |
sam_grove |
0:9dd7c6129683 | 255 | return; |
sam_grove |
0:9dd7c6129683 | 256 | } |
sam_grove |
0:9dd7c6129683 | 257 | |
sam_grove |
0:9dd7c6129683 | 258 | if(offset){ |
sam_grove |
0:9dd7c6129683 | 259 | /* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/ |
sam_grove |
0:9dd7c6129683 | 260 | if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){ |
sam_grove |
0:9dd7c6129683 | 261 | /* cob_id_client = 0x600 + nodeId; */ |
sam_grove |
0:9dd7c6129683 | 262 | *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; |
sam_grove |
0:9dd7c6129683 | 263 | } |
sam_grove |
0:9dd7c6129683 | 264 | /* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/ |
sam_grove |
0:9dd7c6129683 | 265 | if((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){ |
sam_grove |
0:9dd7c6129683 | 266 | /* cob_id_server = 0x580 + nodeId; */ |
sam_grove |
0:9dd7c6129683 | 267 | *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; |
sam_grove |
0:9dd7c6129683 | 268 | } |
sam_grove |
0:9dd7c6129683 | 269 | } |
sam_grove |
0:9dd7c6129683 | 270 | |
sam_grove |
0:9dd7c6129683 | 271 | /* |
sam_grove |
0:9dd7c6129683 | 272 | Initialize the server(s) SDO parameters |
sam_grove |
0:9dd7c6129683 | 273 | Remember that only one SDO server is allowed, defined at index 0x1200 |
sam_grove |
0:9dd7c6129683 | 274 | |
sam_grove |
0:9dd7c6129683 | 275 | Initialize the client(s) SDO parameters |
sam_grove |
0:9dd7c6129683 | 276 | Nothing to initialize (no default values required by the DS 401) |
sam_grove |
0:9dd7c6129683 | 277 | Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 |
sam_grove |
0:9dd7c6129683 | 278 | */ |
sam_grove |
0:9dd7c6129683 | 279 | { |
sam_grove |
0:9dd7c6129683 | 280 | UNS8 i = 0; |
sam_grove |
0:9dd7c6129683 | 281 | UNS16 offset = d->firstIndex->PDO_RCV; |
sam_grove |
0:9dd7c6129683 | 282 | UNS16 lastIndex = d->lastIndex->PDO_RCV; |
sam_grove |
0:9dd7c6129683 | 283 | UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500}; |
sam_grove |
0:9dd7c6129683 | 284 | if( offset ) while( (offset <= lastIndex) && (i < 4)) { |
sam_grove |
0:9dd7c6129683 | 285 | if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)) |
sam_grove |
0:9dd7c6129683 | 286 | *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
sam_grove |
0:9dd7c6129683 | 287 | i ++; |
sam_grove |
0:9dd7c6129683 | 288 | offset ++; |
sam_grove |
0:9dd7c6129683 | 289 | } |
sam_grove |
0:9dd7c6129683 | 290 | } |
sam_grove |
0:9dd7c6129683 | 291 | /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */ |
sam_grove |
0:9dd7c6129683 | 292 | { |
sam_grove |
0:9dd7c6129683 | 293 | UNS8 i = 0; |
sam_grove |
0:9dd7c6129683 | 294 | UNS16 offset = d->firstIndex->PDO_TRS; |
sam_grove |
0:9dd7c6129683 | 295 | UNS16 lastIndex = d->lastIndex->PDO_TRS; |
sam_grove |
0:9dd7c6129683 | 296 | UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480}; |
sam_grove |
0:9dd7c6129683 | 297 | i = 0; |
sam_grove |
0:9dd7c6129683 | 298 | if( offset ) while ((offset <= lastIndex) && (i < 4)) { |
sam_grove |
0:9dd7c6129683 | 299 | if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)) |
sam_grove |
0:9dd7c6129683 | 300 | *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
sam_grove |
0:9dd7c6129683 | 301 | i ++; |
sam_grove |
0:9dd7c6129683 | 302 | offset ++; |
sam_grove |
0:9dd7c6129683 | 303 | } |
sam_grove |
0:9dd7c6129683 | 304 | } |
sam_grove |
0:9dd7c6129683 | 305 | |
sam_grove |
0:9dd7c6129683 | 306 | /* Update EMCY COB-ID if already set to default*/ |
sam_grove |
0:9dd7c6129683 | 307 | if((*d->error_cobid == *d->bDeviceNodeId + 0x80)||(*d->bDeviceNodeId==0xFF)) |
sam_grove |
0:9dd7c6129683 | 308 | *d->error_cobid = nodeId + 0x80; |
sam_grove |
0:9dd7c6129683 | 309 | |
sam_grove |
0:9dd7c6129683 | 310 | /* bDeviceNodeId is defined in the object dictionary. */ |
sam_grove |
0:9dd7c6129683 | 311 | *d->bDeviceNodeId = nodeId; |
sam_grove |
0:9dd7c6129683 | 312 | } |
sam_grove |
0:9dd7c6129683 | 313 | |
sam_grove |
0:9dd7c6129683 | 314 | void _initialisation(CO_Data* d){} |
sam_grove |
0:9dd7c6129683 | 315 | void _preOperational(CO_Data* d){} |
sam_grove |
0:9dd7c6129683 | 316 | void _operational(CO_Data* d){} |
sam_grove |
0:9dd7c6129683 | 317 | void _stopped(CO_Data* d){} |