pdo.c

This library is Copyright (c) Raphael Zulliger <[e-mail]>. It is licensed under the GNU L

void processReceivedPDO( s_rx_buffer_message* canopenMessage )
{
    BYTE    prp_i,                      // number of the acutal processed pdo-nr.
    		prp_j;

    BYTE XHUGE*     pMappingCount = NULL;      // count of mapped objects...
    void XHUGE*     pMappedAppObject = NULL;   // pointer to the var which is mapped to a pdo...
    DWORD XHUGE*    pMappingParameter = NULL;   // pointer fo the var which holds the mapping parameter of an mapping entry
    BYTE XHUGE*     pTransmissionType = NULL;  // pointer to the transmissiontype...
    WORD XHUGE*  	pwCobId = NULL;

    BYTE      writtenBits = (BYTE)0x00;
    BYTE      writtenMappingEntries = (BYTE)0x00;

    DWORD XDATA*    pSize;
    DWORD XDATA     size;

    pSize = &size;

	// first make sure we are in OPERATIONAL state. only in OPERATIONAL state PDO transfer is allowed
	if( bStateMachineState == (BYTE)OPERATIONAL ) 
	{
	    for( prp_i=(BYTE)0x00; prp_i<MAX_COUNT_OF_PDO; prp_i++ )
    	{  // check wheter the received COBs are interesting for us:
   	    	if( getODEntry( (WORD)0x1400 + prp_i, (BYTE)1, (void XHUGE* XDATA*)&pwCobId, pSize ) == SUCCESSFUL )
	       	{
    	        if( *pwCobId == canopenMessage->m.cob_id.w )
   	    	    { // the received PDO matches one of the PDOs defined in the object directory
            	    if( getODEntry( (WORD)0x1600 + prp_i, (BYTE)0, (void XHUGE* XDATA*)&pMappingCount, pSize ) == (DWORD)SUCCESSFUL )
	                {   // getting the count of mapped parameters was successfull
    	                for( prp_j=(BYTE)0x00; prp_j<*pMappingCount; prp_j++ )
   	    	            {   // for each mapping parameter: get the parameter and of course: write the vars...
       	    	            if( getODEntry( (WORD)0x1600 + prp_i, prp_j + (BYTE)1, (void XHUGE* XDATA*)&pMappingParameter, pSize ) == SUCCESSFUL )
	           	            {
    	                        if( getODEntry( (*pMappingParameter) >> (BYTE)16, ( (*pMappingParameter) >> (BYTE)8 ) & (DWORD)0x000000FF, (void XHUGE* XDATA*)&pMappedAppObject, pSize ) == SUCCESSFUL )
   	    	                    {
	            	                // now copy: the value of the mapped var. the count of bits. the pointre of the destination var.
   		                            RxPDOTransferTable[prp_i].mappingEntry[prp_j].pObject = pMappedAppObject;
            	                    memcpy( (void*)&RxPDOTransferTable[prp_i].mappingEntry[prp_j].value, (void*)&canopenMessage->m.data[writtenBits/8], (*pMappingParameter & 0x000000FF)/8 );
	                                RxPDOTransferTable[prp_i].mappingEntry[prp_j].size = (*pMappingParameter & 0x000000FF);
		
        	                        // increase writteBits to the new value.
            	                    writtenBits += *pMappingParameter & 0x000000FF;
	                                writtenMappingEntries++;
    	                        }
        	                }
	                    }
    	            }
        	    }   // end: we've found a matching pdo.
	        }
    	    if( writtenBits != (BYTE)0x00 )
        	{
	            RxPDOTransferTable[prp_i].processed = FALSE;
    	        RxPDOTransferTable[prp_i].count = writtenMappingEntries;
        	    // getting the transmissiontype...
	            if( getODEntry( (WORD)0x1400 + prp_i, (BYTE)2, (void XHUGE* XDATA*)&pTransmissionType, pSize ) == SUCCESSFUL )
    	        {
        	        RxPDOTransferTable[prp_i].transmissionType = *pTransmissionType;
	            }
    	        else
        	    {   // getting of the correct transmissiontype failed: so assign a standard transmissiontype 255
	            }
   		        // resetting counting vars...
        	    writtenBits = (BYTE)0x00;
    	        writtenMappingEntries = (BYTE)0x00;
        	}
	    }
	    // process all data in the receiveTransferTable...
	    for( prp_i=(BYTE)0x00; prp_i<MAX_COUNT_OF_PDO; prp_i++ )
	    {
	        if( ( RxPDOTransferTable[prp_i].transmissionType == (BYTE)255 ) && ( RxPDOTransferTable[prp_i].processed == FALSE ) )
	        {
	            for( prp_j=(BYTE)0x00; prp_j<RxPDOTransferTable[prp_i].count; prp_j++ )
	            {
					// !!!! is this correct?!?!
					memcpy( RxPDOTransferTable[prp_i].mappingEntry[prp_j].pObject, (void*)&RxPDOTransferTable[prp_i].mappingEntry[prp_j].value, (BYTE)0x01 );
	            }
	            RxPDOTransferTable[prp_i].processed = TRUE;
	        }
	    }
	}

}
	


void sendPDO( const WORD wID, const BYTE* pbData, const BYTE bLength, const BOOL bRemoteReceive )
{
  Message m;

  // copies the data into the message struct
  memcpy( m.data, pbData, bLength );
  m.len = bLength;
  // the ID must be the CANopen ID of the PDO (so not the ID of the CANopen device!)
  m.cob_id.w = wID;
  // do we want that the node send us a pdo or do we want to send a PDO?
  m.rtr = bRemoteReceive == TRUE? (BYTE)0xFF : (BYTE)0x00;
  canSend( 0, &m );
}


void CopyBits(int NbBits, char* SrcByteIndex, int SrcBitIndex, BOOL SrcLittleEndian, char* DestByteIndex, int DestBitIndex, BOOL DestLittleEndian)
{
    //This loop copy as many bits that it can each time, crossing successively bytes
    // boundaries from LSB to MSB.
    while(NbBits > 0)
    {
        // Bit missalignement between src and dest
        int Vect;
        char Aligned;
        int BoudaryLimit;
        int BitsToCopy;
        char Mask;
        char Filtered;

		Vect = DestBitIndex - SrcBitIndex;

        // We can now get src and align it to dest
		Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;

        // Compute the nb of bit we will be able to copy
		BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
		BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;

        // Create a mask that will serve in:
		Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
/*
        printf("SrcBitIndex : %d, DestBitIndex : %d , BitsToCopy : %d ", SrcBitIndex, DestBitIndex, BitsToCopy);

        for(int i = 0; i < 8; i++)
            printf("%d", (Mask << i & 0x80)?1:0);
        printf(" %2.2x", Mask);
        printf("\n");
*/
        // - Filtering src
		Filtered = Aligned & ~Mask;

        // - and erase bits where we write, preserve where we don't
        *DestByteIndex &= Mask;

        // Then write.
        *DestByteIndex |= Filtered ;

        //Compute next time cursors for src
        if((SrcBitIndex += BitsToCopy)>7)    // cross boundary ?
        {
            SrcBitIndex = 0;                            // First bit
            SrcByteIndex += (SrcLittleEndian ? -1 : 1);    // Next byte
        }

        //Compute next time cursors for dest
        if((DestBitIndex += BitsToCopy)>7)
        {
            DestBitIndex = 0;                            // First bit
            DestByteIndex += (DestLittleEndian ? -1 : 1);// Next byte
        }

        //And decrement counter.
        NbBits -= BitsToCopy;
    }

}

BOOL resetTxPDOData( BYTE bPDONr )
{
	BYTE i;
	WORD* pwCobId;
	DWORD size;
	DWORD* pSize;
	
	pSize = &size;
	
	// get the cob-id of the bPDONr-th PDO.
	if( getODEntry( (WORD)0x1800 | (bPDONr-1), (BYTE)1, (void XHUGE* XDATA*)&pwCobId, pSize ) == SUCCESSFUL )
	{
		for( i=(BYTE)0; i<MAX_COUNT_OF_PDO ; i++ )
		{
			if( *pwCobId == TxPDOTransferTable[i].canMessage.cob_id.w )
			{
				TxPDOTransferTable[i].dataValid = FALSE;
				break;
			}	
		}
		return TRUE;
	}
	else
		return FALSE;
}