nico@207: nico@207:
nico@215:00001 /* nico@207: 00002 This file is part of CanFestival, a library implementing CanOpen Stack. nico@207: 00003 nico@207: 00004 Copyright (C): Edouard TISSERANT and Francis DUPIN nico@207: 00005 nico@207: 00006 See COPYING file for copyrights details. nico@207: 00007 nico@207: 00008 This library is free software; you can redistribute it and/or nico@207: 00009 modify it under the terms of the GNU Lesser General Public nico@207: 00010 License as published by the Free Software Foundation; either nico@207: 00011 version 2.1 of the License, or (at your option) any later version. nico@207: 00012 nico@207: 00013 This library is distributed in the hope that it will be useful, nico@207: 00014 but WITHOUT ANY WARRANTY; without even the implied warranty of nico@207: 00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU nico@207: 00016 Lesser General Public License for more details. nico@207: 00017 nico@207: 00018 You should have received a copy of the GNU Lesser General Public nico@207: 00019 License along with this library; if not, write to the Free Software nico@207: 00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA nico@207: 00021 */ nico@207: 00022 nico@215: 00033 #include "data.h" nico@215: 00034 #include "sync.h" nico@215: 00035 #include "canfestival.h" nico@210: 00036 nico@210: 00037 /* Prototypes for internals functions */ nico@210: 00038 nico@215: 00045 void SyncAlarm(CO_Data* d, UNS32 id); nico@215: 00046 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, nico@215: 00047 UNS8 unsused_bSubindex); nico@207: 00048 nico@215: 00055 void SyncAlarm(CO_Data* d, UNS32 id) nico@210: 00056 { nico@215: 00057 sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ; nico@210: 00058 } nico@210: 00059 nico@215: 00069 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex) nico@210: 00070 { nico@215: 00071 startSYNC(d); nico@210: 00072 return 0; nico@210: 00073 } nico@210: 00074 nico@215: 00080 void startSYNC(CO_Data* d) nico@210: 00081 { nico@215: 00082 RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate); nico@215: 00083 RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate); nico@210: 00084 nico@215: 00085 if(d->syncTimer != TIMER_NONE){ nico@215: 00086 stopSYNC(d); nico@210: 00087 } nico@210: 00088 nico@215: 00089 if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period) nico@210: 00090 { nico@215: 00091 d->syncTimer = SetAlarm( nico@215: 00092 d, nico@210: 00093 0 /*No id needed*/, nico@210: 00094 &SyncAlarm, nico@215: 00095 US_TO_TIMEVAL(*d->Sync_Cycle_Period), nico@215: 00096 US_TO_TIMEVAL(*d->Sync_Cycle_Period)); nico@210: 00097 } nico@210: 00098 } nico@210: 00099 nico@215: 00105 void stopSYNC(CO_Data* d) nico@210: 00106 { nico@215: 00107 d->syncTimer = DelAlarm(d->syncTimer); nico@210: 00108 } nico@210: 00109 nico@215: 00118 UNS8 sendSYNC(CO_Data* d, UNS32 cob_id) nico@210: 00119 { nico@215: 00120 Message m; nico@215: 00121 UNS8 resultat ; nico@210: 00122 nico@215: 00123 MSG_WAR(0x3001, "sendSYNC ", 0); nico@210: 00124 nico@215: 00125 m.cob_id.w = cob_id ; nico@215: 00126 m.rtr = NOT_A_REQUEST; nico@215: 00127 m.len = 0; nico@215: 00128 resultat = canSend(d->canHandle,&m) ; nico@215: 00129 proceedSYNC(d, &m) ; nico@210: 00130 return resultat ; nico@210: 00131 } nico@210: 00132 nico@215: 00141 UNS8 proceedSYNC(CO_Data* d, Message *m) nico@210: 00142 { nico@210: 00143 nico@215: 00144 UNS8 pdoNum, /* number of the actual processed pdo-nr. */ nico@210: 00145 prp_j; nico@210: 00146 nico@215: 00147 const UNS8 * pMappingCount = NULL; /* count of mapped objects...*/ nico@215: 00148 /* pointer to the var which is mapped to a pdo */ nico@210: 00149 /* void * pMappedAppObject = NULL; */ nico@215: 00150 /* pointer fo the var which holds the mapping parameter of an mapping entry */ nico@215: 00151 UNS32 * pMappingParameter = NULL; nico@215: 00152 /* pointer to the transmissiontype...*/ nico@215: 00153 UNS8 * pTransmissionType = NULL; nico@215: 00154 UNS32 * pwCobId = NULL; nico@210: 00155 nico@215: 00156 UNS8 dataType; nico@215: 00157 UNS16 index; nico@215: 00158 UNS8 subIndex; nico@215: 00159 UNS8 offset; nico@215: 00160 UNS8 status; nico@215: 00161 UNS8 Size; nico@215: 00162 UNS32 objDict; nico@215: 00163 UNS16 offsetObjdict; nico@215: 00164 UNS16 offsetObjdictMap; nico@215: 00165 UNS16 lastIndex; nico@210: 00166 nico@215: 00167 status = state3; nico@210: 00168 pdoNum = 0x00; nico@210: 00169 prp_j = 0x00; nico@210: 00170 offset = 0x00; nico@210: 00171 nico@215: 00172 MSG_WAR(0x3002, "SYNC received. Proceed. ", 0); nico@210: 00173 nico@215: 00174 (*d->post_sync)(); nico@210: 00175 nico@215: 00176 /* only operational state allows PDO transmission */ nico@215: 00177 if( d->nodeState != Operational ) nico@210: 00178 return 0; nico@210: 00179 nico@215: 00180 /* So, the node is in operational state */ nico@215: 00181 /* study all PDO stored in the objects dictionary */ nico@215: 00182 nico@215: 00183 offsetObjdict = d->firstIndex->PDO_TRS; nico@215: 00184 lastIndex = d->lastIndex->PDO_TRS; nico@215: 00185 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; nico@210: 00186 nico@210: 00187 if(offsetObjdict) while( offsetObjdict <= lastIndex) { nico@210: 00188 switch( status ) { nico@210: 00189 nico@215: 00190 case state3: /* get the PDO transmission type */ nico@215: 00191 if (d->objdict[offsetObjdict].bSubCount <= 2) { nico@215: 00192 MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); nico@210: 00193 return 0xFF; nico@210: 00194 } nico@215: 00195 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; nico@215: 00196 MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); nico@215: 00197 status = state4; nico@210: 00198 break; nico@215: 00199 case state4: /* check if transmission type is after (this) SYNC */ nico@215: 00200 /* The message may not be transmited every SYNC but every n SYNC */ nico@215: 00201 if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) && nico@215: 00202 (++d->count_sync[pdoNum] == *pTransmissionType) ) { nico@215: 00203 d->count_sync[pdoNum] = 0; nico@215: 00204 MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); nico@215: 00205 status = state5; nico@210: 00206 break; nico@210: 00207 } nico@210: 00208 else { nico@215: 00209 MSG_WAR(0x3008, " Not on synchro or not at this SYNC. Trans type : ", nico@210: 00210 *pTransmissionType); nico@210: 00211 pdoNum++; nico@210: 00212 offsetObjdict++; nico@210: 00213 offsetObjdictMap++; nico@215: 00214 status = state11; nico@210: 00215 break; nico@210: 00216 } nico@215: 00217 case state5: /* get PDO CobId */ nico@215: 00218 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; nico@215: 00219 MSG_WAR(0x3009, " PDO CobId is : ", *pwCobId); nico@215: 00220 status = state7; nico@210: 00221 break; nico@215: 00222 case state7: /* get mapped objects number to transmit with this PDO */ nico@215: 00223 pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject; nico@215: 00224 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); nico@215: 00225 status = state8; nico@215: 00226 case state8: /* get mapping parameters */ nico@215: 00227 pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject; nico@215: 00228 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); nico@215: 00229 MSG_WAR(0x3050, " at index : ", 0x1A00 + pdoNum); nico@215: 00230 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); nico@215: 00231 status = state9; nico@210: 00232 nico@215: 00233 case state9: /* get data to transmit */ nico@210: 00234 { nico@215: 00235 UNS8 ByteSize; nico@215: 00236 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; nico@215: 00237 index = (UNS16)((*pMappingParameter) >> 16); nico@215: 00238 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); nico@215: 00239 Size = (UNS8)(*pMappingParameter); /* Size in bits */ nico@210: 00240 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ nico@215: 00241 objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ); nico@215: 00242 /* copy bit per bit in little endian*/ nico@215: 00243 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0); nico@210: 00244 } nico@215: 00245 if( objDict != OD_SUCCESSFUL ){ nico@215: 00246 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); nico@210: 00247 return 0xFF; nico@210: 00248 } nico@210: 00249 nico@210: 00250 offset += Size ; nico@215: 00251 d->process_var.count = 1 + ((offset - 1) >> 3); nico@210: 00252 prp_j++; nico@215: 00253 status = state10; nico@210: 00254 break; nico@210: 00255 nico@215: 00256 case state10: /* loop to get all the data to transmit */ nico@210: 00257 if( prp_j < *pMappingCount ){ nico@215: 00258 MSG_WAR(0x3014, " next variable mapped : ", prp_j); nico@215: 00259 status = state8; nico@210: 00260 break; nico@210: 00261 } nico@210: 00262 else { nico@215: 00263 MSG_WAR(0x3015, " End scan mapped variable", 0); nico@215: 00264 PDOmGR( d, *pwCobId ); nico@215: 00265 MSG_WAR(0x3016, " End of this pdo. Should have been sent", 0); nico@210: 00266 pdoNum++; nico@210: 00267 offsetObjdict++; nico@210: 00268 offsetObjdictMap++; nico@210: 00269 offset = 0x00; nico@210: 00270 prp_j = 0x00; nico@215: 00271 status = state11; nico@210: 00272 break; nico@210: 00273 } nico@210: 00274 nico@215: 00275 case state11: nico@215: 00276 MSG_WAR(0x3017, "next pdo index : ", pdoNum); nico@215: 00277 status = state3; nico@210: 00278 break; nico@210: 00279 nico@210: 00280 default: nico@215: 00281 MSG_ERR(0x1019,"Unknown state has been reached : %d",status); nico@210: 00282 return 0xFF; nico@210: 00283 }/* end switch case */ nico@210: 00284 nico@210: 00285 }/* end while( prp_i<dict_cstes.max_count_of_PDO_transmit ) */ nico@210: 00286 nico@215: 00287 (*d->post_TPDO)(); nico@210: 00288 nico@210: 00289 return 0; nico@210: 00290 } nico@210: 00291 nico@210: 00292 nico@215: 00293 void _post_sync(){} nico@215: 00294 void _post_TPDO(){} nico@215: