soes.cpp

00001 /*
00002  * SOES Simple Open EtherCAT Slave
00003  *
00004  * File    : soes.c
00005  * Version : 0.9.2
00006  * Date    : 22-02-2010
00007  * Copyright (C) 2007-2010 Arthur Ketels
00008  *
00009  * SOES is free software; you can redistribute it and/or modify it under
00010  * the terms of the GNU General Public License version 2 as published by the Free
00011  * Software Foundation.
00012  *
00013  * SOES is distributed in the hope that it will be useful, but WITHOUT ANY
00014  * WARRANTY; without even the implied warranty of MERCHANTABILITY or
00015  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
00016  * for more details.
00017  *
00018  * As a special exception, if other files instantiate templates or use macros
00019  * or inline functions from this file, or you compile this file and link it
00020  * with other works to produce a work based on this file, this file does not
00021  * by itself cause the resulting work to be covered by the GNU General Public
00022  * License. However the source code for this file must still be made available
00023  * in accordance with section (3) of the GNU General Public License.
00024  *
00025  * This exception does not invalidate any other reasons why a work based on
00026  * this file might be covered by the GNU General Public License.
00027  *
00028  * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual
00029  * property of, and protected by Beckhoff Automation GmbH.
00030  */
00031 
00032 /****************************************************
00033 Chip type           : STM32F051R8
00034 Clock frequency     : 48 MHz
00035 *****************************************************/
00036 
00037 /* Includes ------------------------------------------------------------------*/
00038 #include "mbed.h"
00039 #include "cpuinit.h"
00040 #include "utypes.h"
00041 #include "esc.h"
00042 #include "MODSERIAL.h"
00043 #include "xbus.h"
00044 
00045 /* Private typedef -----------------------------------------------------------*/
00046 /* Private define ------------------------------------------------------------*/
00047 #define wd_reset 1000
00048 
00049 /* Private macro -------------------------------------------------------------*/
00050 /* Private variables ---------------------------------------------------------*/
00051 _ESCvar         ESCvar;
00052 uint8           APPstate;
00053 _MBX            MBX[MBXBUFFERS];
00054 _MBXcontrol     MBXcontrol[MBXBUFFERS];
00055 uint8           MBXrun=0;
00056 uint16          SM2_sml,SM3_sml;
00057 _Rbuffer        Rb;
00058 _Wbuffer        Wb;
00059 _Ebuffer        Eb; //EEprom
00060 uint8           TXPDOsize,RXPDOsize;
00061 uint16          wd_ok = 1, wd_cnt = wd_reset;
00062 
00063 _Rbuffer local_Rb; //to prevent issues when updating
00064 DigitalOut led(LED_PIN);
00065 DigitalOut et1100_ss(ET1100_SS);
00066 DigitalIn  et1100_miso(ET1100_MISO);
00067 SPI et1100_spi(ET1100_MOSI,ET1100_MISO,ET1100_SCK);
00068 MODSERIAL xbus_serial(PTA2,PTA1, 512);
00069 xbus_t xbus_master;
00070 //MODSERIAL pc(USBTX,USBRX,512);
00071 
00072 DigitalOut ploep(PTA13);
00073 //#define PLOEP do{ploep = !(ploep);}while(0);
00074 
00075 /* Private function prototypes -----------------------------------------------*/
00076 /* Private functions ---------------------------------------------------------*/
00077 
00078 /** void ESC_objecthandler(uint16 index, uint8 subindex)
00079     \brief Object handler, declared from esc.h, as extern function
00080     \param index
00081     \param subindex
00082 */
00083 void ESC_objecthandler(uint16 index, uint8 subindex);
00084 void TXPDO_update(void);
00085 void RXPDO_update(void);
00086 void DIG_process(void);
00087 void sample(void);
00088 
00089 void ESC_objecthandler(uint16 index, uint8 subindex)
00090 {
00091     uint8 dummy8;
00092     uint16 dummy16;
00093     switch (index)
00094     {
00095     case 0x8000:
00096         switch (subindex)
00097         {
00098         case 0x01:
00099         {
00100             uint32_t rates[] = {460800,230400,115200,76800,57600,38400,28800,19200,14400,9600};
00101             if(Eb.setting8 < (sizeof(rates)/sizeof(uint32_t))  )
00102             {   
00103                 XbusGoToConfig();
00104                 XbusSetBaudRate(Eb.setting8);
00105                 XbusReset();
00106                 wait(2);
00107                 xbus_serial.baud(rates[Eb.setting8]);
00108                 XbusInitializeXbusMaster();
00109                 
00110             }
00111             else if(Eb.setting8 == 0x80)
00112             {
00113                 XbusGoToConfig();
00114                 XbusSetBaudRate(0x80);
00115                 XbusReset();
00116                 wait(2);
00117                 xbus_serial.baud(960800);
00118                 XbusInitializeXbusMaster();
00119                 
00120             }
00121             break;
00122         }
00123         case 0x02:
00124             dummy16 = 0;//Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16);
00125             break;
00126         }
00127         break;
00128     }
00129 }
00130 
00131 void TXPDO_update(void)
00132 {
00133   ESC_write(SM3_sma, &Rb, TXPDOsize, &ESCvar.ALevent);
00134 }
00135 
00136 void RXPDO_update(void)
00137 {
00138   ESC_read(SM2_sma, &Wb, RXPDOsize, &ESCvar.ALevent);
00139 }
00140 
00141 void APP_safeoutput(void)
00142 {
00143   asm("nop");
00144   //Wb.dout = 0;
00145   //DOUTPORT = (Wb.dout >> 4) & 0xf0;
00146 }
00147 
00148 void DIG_process(void)
00149 {
00150   uint8_t correct_offset;
00151   if (APPstate & APPSTATE_OUTPUT) //output enabled
00152     {
00153         if (ESCvar.ALevent & ESCREG_ALEVENT_SM2) // SM2 trigger ?
00154         {
00155           ESCvar.ALevent &= ~ESCREG_ALEVENT_SM2;
00156           RXPDO_update();
00157             // dummy output point
00158             correct_offset = Wb.correct_offset;
00159             if(correct_offset & 0x01) {
00160             //    led.write(1);
00161                 asm("nop");
00162            }
00163            // else
00164            //   led.write(0);
00165             wd_cnt = wd_reset;
00166         }
00167 
00168         if (!wd_cnt) {
00169             ESC_stopoutput();
00170             // watchdog, invalid outputs
00171             ESC_ALerror(ALERR_WATCHDOG);
00172             // goto safe-op with error bit set
00173             ESC_ALstatus(ESCsafeop | ESCerror);
00174         }
00175     }
00176     else
00177     {
00178         //wd_ok = 1;
00179         wd_cnt = wd_reset;
00180     }
00181   if (APPstate) //input or output enabled
00182     {   
00183         //Rb.timestamp = ESCvar.Time;
00184         //just some dummy data to test
00185         //Rb.counter++;
00186         //Rb.diginput = diginput;
00187         //Rb.analog[0] = 1;
00188         //Rb.analog[1] = 2;
00189  
00190         memcpy(&Rb, &local_Rb, sizeof(Rb));
00191         TXPDO_update();
00192     } 
00193 }
00194 
00195 float ReverseFloat( const float inFloat )
00196 {
00197    float retVal;
00198    char *floatToConvert = ( char* ) & inFloat;
00199    char *returnFloat = ( char* ) & retVal;
00200 
00201    // swap the bytes into a temporary buffer
00202    returnFloat[0] = floatToConvert[3];
00203    returnFloat[1] = floatToConvert[2];
00204    returnFloat[2] = floatToConvert[1];
00205    returnFloat[3] = floatToConvert[0];
00206 
00207    return retVal;
00208 }
00209 
00210 //Watch out, this is an uggly fix; for odd numbers of num_bytes, still the trailing byte will be used too.
00211 //No memory protection what so ever.
00212 void memcpy_byteswap(uint8_t * dest, uint8_t * source, uint16_t num_bytes)
00213 {
00214     for( int i = 0 ; i < num_bytes ; i+=2 )
00215     {
00216         dest[i]   = source[i+1];
00217         dest[i+1] = source[i];
00218     }
00219 }
00220 
00221 //Watch out, this is an uggly fix; for odd numbers of num_bytes, still the trailing byte will be used too.
00222 //No memory protection what so ever.
00223 void memcpy_floatswap(uint8_t * dest, uint8_t * source, uint16_t num_bytes)
00224 {
00225     for( int i = 0 ; i < num_bytes ; i+=4 )
00226     {
00227         dest[i]   = source[i+3];
00228         dest[i+1] = source[i+2];
00229         dest[i+2]   = source[i+1];
00230         dest[i+3] = source[i];
00231     }
00232 }
00233 
00234 
00235 int main(void)
00236 {
00237 
00238     /*!< At this stage the microcontroller clock setting is already configured,
00239          this is done through SystemInit() function which is called from startup
00240          file (startup_stm32f0xx.s) before to branch to application main.
00241          To reconfigure the default setting of SystemInit() function, refer to
00242          system_stm32f0xx.c file
00243        */
00244     cpuinit();
00245     TXPDOsize = sizeTXPDO();
00246     RXPDOsize = sizeRXPDO();
00247     wait_ms(200);
00248     /*initialize configuration*/
00249     //Eb.setting16 = 0xABCD;
00250     Eb.setting8  = 1;
00251     // wait until ESC is started up
00252     while ((ESCvar.DLstatus & 0x0001) == 0)
00253         ESC_read(ESCREG_DLSTATUS, &ESCvar.DLstatus, sizeof(ESCvar.DLstatus), &ESCvar.ALevent);
00254 
00255 // reset ESC to init state
00256     ESC_ALstatus(ESCinit);
00257     ESC_ALerror(ALERR_NONE);
00258     ESC_stopmbx();
00259     ESC_stopinput();
00260     ESC_stopoutput();
00261     
00262 //  pc.baud(115200);
00263 // application run loop
00264     while (1)
00265     {
00266         while(xbus_serial.readable())
00267         {
00268             XbusReceiveState(&xbus_master, xbus_serial.getc());
00269             if(xbus_master.rx.checksum_ok)
00270             {
00271                 if(xbus_master.rx.buffer[2] == 0x32)
00272                 {
00273                     for(int sensor = 0; sensor < 3 ; sensor++)
00274                     {
00275                         const int num_bytes_quat = 4*4;//sizeof(float);//4 floats * 4 bytes
00276                         const int num_bytes_timestamp = 2;
00277                         const int num_bytes_msg = num_bytes_quat + num_bytes_timestamp; //timestamp
00278                         memcpy_byteswap((uint8_t *)&local_Rb.xsens_imu[sensor].samplecounter   ,&xbus_master.rx.buffer[6]+num_bytes_quat, num_bytes_timestamp);
00279                         memcpy_floatswap((uint8_t *)&local_Rb.xsens_imu[sensor].q0,&xbus_master.rx.buffer[6+(sensor*num_bytes_msg)], num_bytes_quat);
00280                         //memcpy_byteswap((uint8_t *)&local_Rb.xsens_imu[sensor].samplecounter,&xbus_master.rx.buffer[6+num_bytes+(sensor*num_bytes)],2);
00281                         //pc.printf("%x %x\n",Rb.timestamp, *((uint16_t *)(&xbus_master.rx.buffer[4])));
00282                         //PLOEP;
00283                     }
00284                 }
00285                 xbus_master.rx.checksum_ok = 0;
00286             }
00287         }
00288         ESC_read(ESCREG_LOCALTIME, &ESCvar.Time, sizeof(ESCvar.Time), &ESCvar.ALevent);
00289         ESC_ALevent();
00290         ESC_state();
00291         if (ESC_mbxprocess())
00292         {
00293             ESC_coeprocess();
00294             ESC_xoeprocess();
00295         }
00296         DIG_process();
00297     }
00298 }
00299 
00300 /** void ESC_objecthandler(uint16 index, uint8 subindex)
00301     \brief Object handler, declared from esc.h, as extern function
00302     \param index
00303     \param subindex
00304 */
00305 
00306 
00307 #ifdef  USE_FULL_ASSERT
00308 
00309 /**
00310   * @brief  Reports the name of the source file and the source line number
00311   *         where the assert_param error has occurred.
00312   * @param  file: pointer to the source file name
00313   * @param  line: assert_param error line source number
00314   * @retval None
00315   */
00316 void assert_failed(uint8_t* file, uint32_t line)
00317 {
00318     /* User can add his own implementation to report the file name and line number,
00319        ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
00320 
00321     /* Infinite loop */
00322     while (1)
00323     {
00324     }
00325 }
00326 #endif
00327 
00328 /**
00329   * @}
00330   */
00331 
00332 /**
00333   * @}
00334   */
00335 
00336 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
00337