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