--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/src/sync.c Wed May 10 16:59:40 2006 +0200
@@ -0,0 +1,228 @@
+/*
+This file is part of CanFestival, a library implementing CanOpen Stack.
+
+Copyright (C): Edouard TISSERANT and Francis DUPIN
+
+See COPYING file for copyrights details.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+#include "data.h"
+#include "sync.h"
+
+void SyncAlarm(CO_Data* d, UNS32 id)
+{
+ sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;
+}
+
+// This is called when Index 0x1005 is updated.
+UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
+{
+ startSYNC(d);
+}
+
+void startSYNC(CO_Data* d)
+{
+ RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
+ RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
+
+ if(d->syncTimer != TIMER_NONE){
+ stopSYNC(d);
+ }
+
+ if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period)
+ {
+ d->syncTimer = SetAlarm(
+ d,
+ 0/*No id needed*/,
+ &SyncAlarm,
+ US_TO_TIMEVAL(*d->Sync_Cycle_Period),
+ US_TO_TIMEVAL(*d->Sync_Cycle_Period));
+ }
+}
+
+void stopSYNC(CO_Data* d)
+{
+ d->syncTimer = DelAlarm(d->syncTimer);
+}
+
+/*********************************************************************/
+UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)
+{
+ Message m;
+ UNS8 resultat ;
+
+ MSG_WAR(0x3001, "sendSYNC ", 0);
+
+ m.cob_id.w = cob_id ;
+ m.rtr = NOT_A_REQUEST;
+ m.len = 0;
+ resultat = (*d->canSend)(&m) ;
+ proceedSYNC(d, &m) ;
+ return resultat ;
+}
+
+/*****************************************************************************/
+UNS8 proceedSYNC(CO_Data* d, Message *m)
+{
+
+ MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
+
+ UNS8 pdoNum, // number of the actual processed pdo-nr.
+ prp_j;
+
+ const UNS8 * pMappingCount = NULL; // count of mapped objects...
+ // pointer to the var which is mapped to a pdo
+// void * pMappedAppObject = NULL;
+ // pointer fo the var which holds the mapping parameter of an mapping entry
+ UNS32 * pMappingParameter = NULL;
+ // pointer to the transmissiontype...
+ UNS8 * pTransmissionType = NULL;
+ UNS32 * pwCobId = NULL;
+
+ UNS8 * pSize;
+ UNS8 size;
+ UNS8 dataType;
+ UNS16 index;
+ UNS8 subIndex;
+ UNS8 offset;
+ UNS8 status;
+ UNS8 sizeData;
+ pSize = &size;
+ UNS32 objDict;
+ status = state3;
+ pdoNum=0x00;
+ prp_j=0x00;
+ offset = 0x00;
+ UNS16 offsetObjdict;
+ UNS16 offsetObjdictMap;
+ UNS16 lastIndex;
+
+ /* only operational state allows PDO transmission */
+ if( d->nodeState != Operational )
+ return 0;
+
+ (*d->post_sync)();
+
+ /* So, the node is in operational state */
+ /* study all PDO stored in the objects dictionary */
+
+ offsetObjdict = d->firstIndex->PDO_TRS;
+ lastIndex = d->lastIndex->PDO_TRS;
+ offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
+
+ if(offsetObjdict) while( offsetObjdict <= lastIndex) {
+ switch( status ) {
+
+ case state3: /* get the PDO transmission type */
+ if (d->objdict[offsetObjdict].bSubCount <= 2) {
+ MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum);
+ return 0xFF;
+ }
+ pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
+ MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);
+ status = state4;
+ break;
+ case state4: /* check if transmission type is after (this) SYNC */
+ /* The message may not be transmited every SYNC but every n SYNC */
+ if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&
+ (++d->count_sync[pdoNum] == *pTransmissionType) ) {
+ d->count_sync[pdoNum] = 0;
+ MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
+ status = state5;
+ break;
+ }
+ else {
+ MSG_WAR(0x3008, " Not on synchro or not at this SYNC. Trans type : ",
+ *pTransmissionType);
+ pdoNum++;
+ offsetObjdict++;
+ offsetObjdictMap++;
+ status = state11;
+ break;
+ }
+ case state5: /* get PDO CobId */
+ pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
+ MSG_WAR(0x3009, " PDO CobId is : ", *pwCobId);
+ status = state7;
+ break;
+ case state7: /* get mapped objects number to transmit with this PDO */
+ pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;
+ MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount );
+ status = state8;
+ case state8: /* get mapping parameters */
+ pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;
+ MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter);
+ MSG_WAR(0x3050, " at index : ", 0x1A00 + pdoNum);
+ MSG_WAR(0x3051, " sub-index : ", prp_j + 1);
+ status = state9;
+
+ case state9: /* get data to transmit */
+ index = (UNS16)((*pMappingParameter) >> 16);
+ subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
+ // <<3 because in *pMappingParameter the size is in bits
+ sizeData = (UNS8) ((*pMappingParameter & (UNS32)0x000000FF) >> 3) ;
+
+ objDict = getODentry(d, index, subIndex, (void *)&d->process_var.data[offset], &sizeData, &dataType, 0 );
+
+ if( objDict != OD_SUCCESSFUL ){
+ MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
+ return 0xFF;
+ }
+
+ offset += sizeData ;
+ d->process_var.count = offset;
+ prp_j++;
+ status = state10;
+ break;
+
+ case state10: /* loop to get all the data to transmit */
+ if( prp_j < *pMappingCount ){
+ MSG_WAR(0x3014, " next variable mapped : ", prp_j);
+ status = state8;
+ break;
+ }
+ else {
+ MSG_WAR(0x3015, " End scan mapped variable", 0);
+ PDOmGR( d, *pwCobId );
+ MSG_WAR(0x3016, " End of this pdo. Should have been sent", 0);
+ pdoNum++;
+ offsetObjdict++;
+ offsetObjdictMap++;
+ offset = 0x00;
+ prp_j = 0x00;
+ status = state11;
+ break;
+ }
+
+ case state11:
+ MSG_WAR(0x3017, "next pdo index : ", pdoNum);
+ status = state3;
+ break;
+
+ default:
+ MSG_ERR(0x1019,"Unknown state has been reached : %d",status);
+ return 0xFF;
+ }// end switch case
+
+ }// end while( prp_i<dict_cstes.max_count_of_PDO_transmit )
+
+ (*d->post_TPDO)();
+
+ return 0;
+}
+
+