nico@207: nico@207:
nico@215:00001 /* nico@210: 00002 This file is part of CanFestival, a library implementing CanOpen nico@210: 00003 Stack. nico@210: 00004 nico@210: 00005 Copyright (C): Edouard TISSERANT and Francis DUPIN nico@210: 00006 nico@210: 00007 See COPYING file for copyrights details. nico@210: 00008 nico@210: 00009 This library is free software; you can redistribute it and/or nico@210: 00010 modify it under the terms of the GNU Lesser General Public nico@210: 00011 License as published by the Free Software Foundation; either nico@210: 00012 version 2.1 of the License, or (at your option) any later version. nico@210: 00013 nico@210: 00014 This library is distributed in the hope that it will be useful, nico@210: 00015 but WITHOUT ANY WARRANTY; without even the implied warranty of nico@210: 00016 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU nico@210: 00017 Lesser General Public License for more details. nico@210: 00018 nico@210: 00019 You should have received a copy of the GNU Lesser General Public nico@210: 00020 License along with this library; if not, write to the Free Software nico@210: 00021 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 nico@210: 00022 USA nico@210: 00023 */ nico@210: 00024 nico@215: 00035 #include <data.h> nico@215: 00036 #include "lifegrd.h" nico@215: 00037 #include "canfestival.h" nico@207: 00038 nico@210: 00039 etisserant@240: 00040 void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id); nico@210: 00041 nico@210: 00042 etisserant@240: 00043 void ProducerHearbeatAlarm(CO_Data* d, UNS32 id); nico@210: 00044 etisserant@240: 00045 UNS32 OnHearbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex); nico@210: 00046 etisserant@240: 00055 e_nodeState getNodeState (CO_Data* d, UNS8 nodeId) nico@210: 00056 { etisserant@240: 00057 e_nodeState networkNodeState = d->NMTable[nodeId]; nico@210: 00058 return networkNodeState; nico@210: 00059 } nico@210: 00060 etisserant@240: 00067 void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id) nico@210: 00068 { nico@210: 00069 /*MSG_WAR(0x00, "ConsumerHearbeatAlarm", 0x00);*/ nico@210: 00070 etisserant@240: 00072 (*d->heartbeatError)((UNS8)( ((d->ConsumerHeartbeatEntries[id]) & (UNS32)0x00FF0000) >> (UNS8)16 )); nico@210: 00073 } nico@210: 00074 etisserant@240: 00081 void proceedNODE_GUARD(CO_Data* d, Message* m ) nico@210: 00082 { etisserant@240: 00083 UNS8 nodeId = (UNS8) GET_NODE_ID((*m)); nico@210: 00084 etisserant@240: 00085 if((m->rtr == 1) ) nico@210: 00090 { etisserant@240: 00097 if (nodeId == *d->bDeviceNodeId ) nico@210: 00098 { nico@215: 00099 Message msg; etisserant@240: 00100 msg.cob_id.w = *d->bDeviceNodeId + 0x700; etisserant@240: 00101 msg.len = (UNS8)0x01; etisserant@240: 00102 msg.rtr = 0; etisserant@240: 00103 msg.data[0] = d->nodeState; etisserant@240: 00104 if (d->toggle) nico@210: 00105 { etisserant@240: 00106 msg.data[0] |= 0x80 ; etisserant@240: 00107 d->toggle = 0 ; nico@210: 00108 } nico@210: 00109 else etisserant@240: 00110 d->toggle = 1 ; nico@215: 00111 /* send the nodeguard response. */ etisserant@240: 00112 MSG_WAR(0x3130, "Sending NMT Nodeguard to master, state: ", d->nodeState); etisserant@240: 00113 canSend(d->canHandle,&msg ); nico@210: 00114 } nico@210: 00115 nico@215: 00116 }else{ /* Not a request CAN */ nico@215: 00117 etisserant@240: 00118 MSG_WAR(0x3110, "Received NMT nodeId : ", nodeId); nico@215: 00119 /* the slave's state receievd is stored in the NMTable */ nico@215: 00120 /* The state is stored on 7 bit */ etisserant@240: 00121 d->NMTable[nodeId] = (e_nodeState) ((*m).data[0] & 0x7F) ; nico@210: 00122 nico@215: 00123 /* Boot-Up frame reception */ etisserant@240: 00124 if ( d->NMTable[nodeId] == Initialisation) nico@210: 00125 { nico@215: 00126 /* nico@215: 00127 ** The device send the boot-up message (Initialisation) nico@215: 00128 ** to indicate the master that it is entered in nico@215: 00129 ** pre_operational mode nico@215: 00130 ** Because the device enter automaticaly in pre_operational nico@215: 00131 ** mode, nico@215: 00132 ** the pre_operational mode is stored nico@215: 00133 ** NMTable[bus_id][nodeId] = Pre_operational nico@215: 00134 */ etisserant@240: 00135 MSG_WAR(0x3100, "The NMT is a bootup from node : ", nodeId); nico@210: 00136 } nico@210: 00137 etisserant@240: 00138 if( d->NMTable[nodeId] != Unknown_state ) { etisserant@240: 00139 UNS8 index, ConsummerHeartBeat_nodeId ; etisserant@240: 00140 for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ ) nico@210: 00141 { etisserant@240: 00142 ConsummerHeartBeat_nodeId = (UNS8)( ((d->ConsumerHeartbeatEntries[index]) & (UNS32)0x00FF0000) >> (UNS8)16 ); nico@210: 00143 if ( nodeId == ConsummerHeartBeat_nodeId ) nico@210: 00144 { etisserant@240: 00145 TIMEVAL time = ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ; nico@210: 00146 /* Renew alarm for next heartbeat. */ etisserant@240: 00147 DelAlarm(d->ConsumerHeartBeatTimers[index]); etisserant@240: 00148 d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0); nico@210: 00149 } nico@210: 00150 } nico@210: 00151 } nico@210: 00152 } nico@210: 00153 } nico@210: 00154 etisserant@240: 00161 void ProducerHearbeatAlarm(CO_Data* d, UNS32 id) nico@210: 00162 { etisserant@240: 00163 if(*d->ProducerHeartBeatTime) nico@210: 00164 { nico@215: 00165 Message msg; nico@215: 00166 /* Time expired, the heartbeat must be sent immediately nico@215: 00167 ** generate the correct node-id: this is done by the offset 1792 nico@215: 00168 ** (decimal) and additionaly nico@215: 00169 ** the node-id of this device. nico@215: 00170 */ nico@215: 00171 etisserant@240: 00172 msg.cob_id.w = *d->bDeviceNodeId + 0x700; etisserant@240: 00173 msg.len = (UNS8)0x01; etisserant@240: 00174 msg.rtr = 0; etisserant@240: 00175 msg.data[0] = d->nodeState; /* No toggle for heartbeat !*/ nico@215: 00176 /* send the heartbeat */ etisserant@240: 00177 MSG_WAR(0x3130, "Producing heartbeat: ", d->nodeState); etisserant@240: 00178 canSend(d->canHandle,&msg ); nico@210: 00179 nico@210: 00180 }else{ etisserant@240: 00181 d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer); nico@210: 00182 } nico@210: 00183 } nico@210: 00184 etisserant@240: 00194 UNS32 OnHeartbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex) nico@210: 00195 { etisserant@240: 00196 heartbeatStop(d); etisserant@240: 00197 heartbeatInit(d); nico@210: 00198 return 0; nico@210: 00199 } nico@210: 00200 etisserant@240: 00206 void heartbeatInit(CO_Data* d) nico@210: 00207 { nico@210: 00208 etisserant@240: 00209 UNS8 index; /* Index to scan the table of heartbeat consumers */ etisserant@240: 00210 RegisterSetODentryCallBack(d, 0x1017, 0x00, &OnHeartbeatProducerUpdate); nico@210: 00211 etisserant@240: 00212 d->toggle = 0; nico@210: 00213 etisserant@240: 00214 for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ ) nico@210: 00215 { etisserant@240: 00216 TIMEVAL time = (UNS16) ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ; nico@215: 00217 /* MSG_WAR(0x3121, "should_time : ", should_time ) ; */ nico@210: 00218 if ( time ) nico@210: 00219 { etisserant@240: 00220 d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0); nico@210: 00221 } nico@210: 00222 } nico@210: 00223 etisserant@240: 00224 if ( *d->ProducerHeartBeatTime ) nico@210: 00225 { etisserant@240: 00226 TIMEVAL time = *d->ProducerHeartBeatTime; etisserant@240: 00227 d->ProducerHeartBeatTimer = SetAlarm(d, 0, &ProducerHearbeatAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time)); nico@210: 00228 } nico@210: 00229 } nico@210: 00230 etisserant@240: 00236 void heartbeatStop(CO_Data* d) nico@210: 00237 { etisserant@240: 00238 UNS8 index; etisserant@240: 00239 for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ ) nico@210: 00240 { etisserant@240: 00241 d->ConsumerHeartBeatTimers[index + 1] = DelAlarm(d->ConsumerHeartBeatTimers[index + 1]);; nico@210: 00242 } nico@210: 00243 etisserant@240: 00244 d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer);; nico@210: 00245 } nico@210: 00246 etisserant@240: 00252 void _heartbeatError(UNS8 heartbeatID){} etisserant@240: