CAN.cpp

00001 #include "all_includes.h"
00002 
00003 extern CANMessage msgRxBuffer[SIZE_FIFO];
00004 extern CAN can;
00005 extern DigitalOut led2;
00006 extern void GetPositionAx12(void);
00007 
00008 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
00009 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
00010 
00011 unsigned char FlagAx12 = 0;
00012 
00013 
00014 /*********************************************************************************************************/
00015 /* FUNCTION NAME: SendRawId                                                                              */
00016 /* DESCRIPTION  : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
00017 /*********************************************************************************************************/
00018 void SendRawId (unsigned short id)
00019 {
00020     CANMessage msgTx=CANMessage();
00021     msgTx.id=id;
00022     msgTx.len=0;
00023     can.write(msgTx);
00024     wait_us(50);
00025 }
00026 
00027 
00028 /*********************************************************************************************************/
00029 /* FUNCTION NAME: canRx_ISR                                                                            */
00030 /* DESCRIPTION  : lit les messages sur le can et les stocke dans la FIFO                                */
00031 /*********************************************************************************************************/
00032 void canRx_ISR (void)
00033 {
00034     if (can.read(msgRxBuffer[FIFO_ecriture]))
00035         FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
00036 }
00037 
00038 
00039 
00040 /****************************************************************************************/
00041 /* FUNCTION NAME: canProcessRx                                                          */
00042 /* DESCRIPTION  : Fonction de traitement des messages CAN                               */
00043 /****************************************************************************************/
00044 void canProcessRx(void){
00045     static signed char FIFO_occupation=0,FIFO_max_occupation=0;
00046     CANMessage msgTx=CANMessage();
00047     FIFO_occupation=FIFO_ecriture-FIFO_lecture;
00048     if(FIFO_occupation<0)
00049         FIFO_occupation=FIFO_occupation+SIZE_FIFO;
00050     if(FIFO_max_occupation<FIFO_occupation)
00051         FIFO_max_occupation=FIFO_occupation;
00052     if(FIFO_occupation!=0) {
00053         
00054         switch(msgRxBuffer[FIFO_lecture].id) {
00055             
00056             case SERVOVANNE:
00057                 EtatServoVanne  = msgRxBuffer[FIFO_lecture].data[0];
00058                 break;
00059             
00060             case POMPE_DROITE:
00061                 ActionPompe = 1;
00062                 EtatPompeDroite  = msgRxBuffer[FIFO_lecture].data[0];
00063                 break;
00064                 
00065             case POMPE_GAUCHE:
00066                 ActionPompe = 1;
00067                 EtatPompeGauche  = msgRxBuffer[FIFO_lecture].data[0];
00068                 break;
00069                 
00070             case TURBINE:
00071                 EtatTurbine = msgRxBuffer[FIFO_lecture].data[0];
00072                 break;
00073                 
00074             case LANCEUR:
00075                 EtatLanceur  = msgRxBuffer[FIFO_lecture].data[0];
00076                 break;
00077                 
00078            case CHECK_AX12:
00079                 SendRawId(ALIVE_AX12);
00080                 FlagAx12 = 1;
00081                 break;
00082             
00083             case SERVO_AX12_ACTION :
00084             
00085                 ActionAx12=1; 
00086                 EtatAx12 = msgRxBuffer[FIFO_lecture].data[0];
00087                 ChoixBras = msgRxBuffer[FIFO_lecture].data[1];
00088                 
00089                 //ACK de reception des actions a effectuer
00090                 msgTx.id = SERVO_AX12_ACK;
00091                 msgTx.len = 1;
00092                 msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
00093                 can.write(msgTx);
00094                 break;
00095                 
00096             case 0x123:
00097                 SendRawId(100);
00098                 GetPositionAx12();
00099                 break;
00100             
00101         }
00102         action_a_effectuer=1;
00103         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
00104     }
00105 }
00106     
00107         
00108                                                                     
00109 
00110 void CAN2_wrFilter (uint32_t id)  {
00111     static int CAN_std_cnt = 0;
00112     uint32_t buf0, buf1;
00113     int cnt1, cnt2, bound1;
00114  
00115     /* Acceptance Filter Memory full */
00116     if (((CAN_std_cnt + 1) >> 1) >= 512)
00117         return;                                       /* error: objects full */
00118  
00119     /* Setup Acceptance Filter Configuration
00120       Acceptance Filter Mode Register = Off  */
00121     LPC_CANAF->AFMR = 0x00000001;
00122  
00123     id |= 1 << 13;                        /* Add controller number(2) */
00124     id &= 0x0000F7FF;                            /* Mask out 16-bits of ID */
00125  
00126     if (CAN_std_cnt == 0)  {                     /* For entering first  ID */
00127         LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
00128     }  else if (CAN_std_cnt == 1)  {             /* For entering second ID */
00129         if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
00130             LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
00131         else
00132             LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
00133     }  else  {
00134         /* Find where to insert new ID */
00135         cnt1 = 0;
00136         cnt2 = CAN_std_cnt;
00137         bound1 = (CAN_std_cnt - 1) >> 1;
00138         while (cnt1 <= bound1)  {                  /* Loop through standard existing IDs */
00139             if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id)  {
00140                 cnt2 = cnt1 * 2;
00141                 break;
00142             }
00143             if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id)  {
00144                 cnt2 = cnt1 * 2 + 1;
00145                 break;
00146             }
00147             cnt1++;                                  /* cnt1 = U32 where to insert new ID */
00148         }                                          /* cnt2 = U16 where to insert new ID */
00149  
00150         if (cnt1 > bound1)  {                      /* Adding ID as last entry */
00151             if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
00152                 LPC_CANAF_RAM->mask[cnt1]  = 0x0000FFFF | (id << 16);
00153             else                                     /* Odd  number of IDs exists */
00154                 LPC_CANAF_RAM->mask[cnt1]  = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
00155         }  else  {
00156             buf0 = LPC_CANAF_RAM->mask[cnt1];        /* Remember current entry */
00157             if ((cnt2 & 0x0001) == 0)                /* Insert new mask to even address */
00158                 buf1 = (id << 16) | (buf0 >> 16);
00159             else                                     /* Insert new mask to odd  address */
00160                 buf1 = (buf0 & 0xFFFF0000) | id;
00161  
00162             LPC_CANAF_RAM->mask[cnt1] = buf1;        /* Insert mask */
00163  
00164             bound1 = CAN_std_cnt >> 1;
00165             /* Move all remaining standard mask entries one place up */
00166             while (cnt1 < bound1)  {
00167                 cnt1++;
00168                 buf1  = LPC_CANAF_RAM->mask[cnt1];
00169                 LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
00170                 buf0  = buf1;
00171             }
00172  
00173             if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
00174                 LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
00175         }
00176     }
00177     CAN_std_cnt++;
00178  
00179     /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */
00180     buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
00181     buf1 = buf0;
00182  
00183     /* Setup acceptance filter pointers */
00184     LPC_CANAF->SFF_sa     = 0;
00185     LPC_CANAF->SFF_GRP_sa = buf0;
00186     LPC_CANAF->EFF_sa     = buf0;
00187     LPC_CANAF->EFF_GRP_sa = buf1;
00188     LPC_CANAF->ENDofTable = buf1;
00189  
00190     LPC_CANAF->AFMR = 0x00000000;                  /* Use acceptance filter */
00191 } // CAN2_wrFilter