Luis Silvio Cordova Rivadeneira / Mbed 2 deprecated Can_open_masternode

Can_open_masternode

Dependencies:   mbed

framework/src/states.c@7:537bae5a6fc6, 2012-09-26 (annotated)

Committer:
sam_grove
Date:
Wed Sep 26 05:48:14 2012 +0000
Revision:
7:537bae5a6fc6
Parent:
0:9dd7c6129683 
Pushing the project into the new repo

Who changed what in which revision?

UserRevisionLine numberNew 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){}