Sam Grove / Mbed 2 deprecated canopen_slavenode

CANfestival - an open source CANopen framework

Dependencies:   mbed

framework/src/states.c@0:6219434a0cb5, 2011-05-30 (annotated)

Committer:
sam_grove
Date:
Mon May 30 07:14:41 2011 +0000
Revision:
0:6219434a0cb5
Initial public release of slave node framework port

Who changed what in which revision?

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