First Last / Mbed 2 deprecated EtherCAT-XbusMaster

EtherCAT slave that reads 3 Xsens IMU's connected to a Xbus Master

Dependencies:   MODSERIAL mbed KL25Z_ClockControl

Fork of EtherCAT by First Last

soes.cpp@27:93c0e4ae943e, 2015-03-09 (annotated)

Committer:
vsluiter
Date:
Mon Mar 09 15:58:00 2015 +0000
Revision:
27:93c0e4ae943e
Parent:
26:c7959f1fd09a
Child:
28:8505285f65ed
Set to communicate at 230400. Somehow messes up timing, as messages aren't interpretted correctly anymore. Issue with serial transceiver, or interrupt handling?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 1:5e22bf1a3817 1 /*
vsluiter 1:5e22bf1a3817 2 * SOES Simple Open EtherCAT Slave
vsluiter 1:5e22bf1a3817 3 *
vsluiter 1:5e22bf1a3817 4 * File : soes.c
vsluiter 1:5e22bf1a3817 5 * Version : 0.9.2
vsluiter 1:5e22bf1a3817 6 * Date : 22-02-2010
vsluiter 1:5e22bf1a3817 7 * Copyright (C) 2007-2010 Arthur Ketels
vsluiter 1:5e22bf1a3817 8 *
vsluiter 1:5e22bf1a3817 9 * SOES is free software; you can redistribute it and/or modify it under
vsluiter 1:5e22bf1a3817 10 * the terms of the GNU General Public License version 2 as published by the Free
vsluiter 1:5e22bf1a3817 11 * Software Foundation.
vsluiter 1:5e22bf1a3817 12 *
vsluiter 1:5e22bf1a3817 13 * SOES is distributed in the hope that it will be useful, but WITHOUT ANY
vsluiter 1:5e22bf1a3817 14 * WARRANTY; without even the implied warranty of MERCHANTABILITY or
vsluiter 1:5e22bf1a3817 15 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
vsluiter 1:5e22bf1a3817 16 * for more details.
vsluiter 1:5e22bf1a3817 17 *
vsluiter 1:5e22bf1a3817 18 * As a special exception, if other files instantiate templates or use macros
vsluiter 1:5e22bf1a3817 19 * or inline functions from this file, or you compile this file and link it
vsluiter 1:5e22bf1a3817 20 * with other works to produce a work based on this file, this file does not
vsluiter 1:5e22bf1a3817 21 * by itself cause the resulting work to be covered by the GNU General Public
vsluiter 1:5e22bf1a3817 22 * License. However the source code for this file must still be made available
vsluiter 1:5e22bf1a3817 23 * in accordance with section (3) of the GNU General Public License.
vsluiter 1:5e22bf1a3817 24 *
vsluiter 1:5e22bf1a3817 25 * This exception does not invalidate any other reasons why a work based on
vsluiter 1:5e22bf1a3817 26 * this file might be covered by the GNU General Public License.
vsluiter 1:5e22bf1a3817 27 *
vsluiter 1:5e22bf1a3817 28 * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual
vsluiter 1:5e22bf1a3817 29 * property of, and protected by Beckhoff Automation GmbH.
vsluiter 1:5e22bf1a3817 30 */
vsluiter 1:5e22bf1a3817 31
vsluiter 1:5e22bf1a3817 32 /****************************************************
vsluiter 1:5e22bf1a3817 33 Chip type : STM32F051R8
vsluiter 1:5e22bf1a3817 34 Clock frequency : 48 MHz
vsluiter 1:5e22bf1a3817 35 *****************************************************/
vsluiter 1:5e22bf1a3817 36
vsluiter 1:5e22bf1a3817 37 /* Includes ------------------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 38 #include "mbed.h"
vsluiter 1:5e22bf1a3817 39 #include "cpuinit.h"
vsluiter 1:5e22bf1a3817 40 #include "utypes.h"
vsluiter 1:5e22bf1a3817 41 #include "esc.h"
vsluiter 12:cac4e7c2bd0f 42 #include "MODSERIAL.h"
vsluiter 17:c5946a0fde83 43 #include "xbus.h"
vsluiter 1:5e22bf1a3817 44
vsluiter 1:5e22bf1a3817 45 /* Private typedef -----------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 46 /* Private define ------------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 47 #define wd_reset 1000
vsluiter 1:5e22bf1a3817 48
vsluiter 1:5e22bf1a3817 49 /* Private macro -------------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 50 /* Private variables ---------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 51 _ESCvar ESCvar;
vsluiter 1:5e22bf1a3817 52 uint8 APPstate;
vsluiter 1:5e22bf1a3817 53 _MBX MBX[MBXBUFFERS];
vsluiter 1:5e22bf1a3817 54 _MBXcontrol MBXcontrol[MBXBUFFERS];
vsluiter 1:5e22bf1a3817 55 uint8 MBXrun=0;
vsluiter 1:5e22bf1a3817 56 uint16 SM2_sml,SM3_sml;
vsluiter 1:5e22bf1a3817 57 _Rbuffer Rb;
vsluiter 1:5e22bf1a3817 58 _Wbuffer Wb;
vsluiter 11:166353137b95 59 _Ebuffer Eb; //EEprom
vsluiter 1:5e22bf1a3817 60 uint8 TXPDOsize,RXPDOsize;
vsluiter 1:5e22bf1a3817 61 uint16 wd_ok = 1, wd_cnt = wd_reset;
vsluiter 1:5e22bf1a3817 62
vsluiter 25:829af6f3429f 63 _Rbuffer local_Rb; //to prevent issues when updating
vsluiter 1:5e22bf1a3817 64 DigitalOut led(LED_PIN);
vsluiter 1:5e22bf1a3817 65 DigitalOut et1100_ss(ET1100_SS);
vsluiter 1:5e22bf1a3817 66 DigitalIn et1100_miso(ET1100_MISO);
vsluiter 1:5e22bf1a3817 67 SPI et1100_spi(ET1100_MOSI,ET1100_MISO,ET1100_SCK);
vsluiter 18:6629e8c5d59e 68 MODSERIAL xbus_serial(PTC4,PTC3);
vsluiter 18:6629e8c5d59e 69 xbus_t xbus_master;
vsluiter 27:93c0e4ae943e 70 MODSERIAL pc(USBTX,USBRX,512);
vsluiter 16:bfc7ea6bd1af 71
vsluiter 22:3f3a9f55054c 72 DigitalOut ploep(PTA13);
vsluiter 22:3f3a9f55054c 73 #define PLOEP do{ploep = !(ploep);}while(0);
vsluiter 19:8ead12aec8d5 74
vsluiter 1:5e22bf1a3817 75 /* Private function prototypes -----------------------------------------------*/
vsluiter 1:5e22bf1a3817 76 /* Private functions ---------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 77
vsluiter 1:5e22bf1a3817 78 /** void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 79 \brief Object handler, declared from esc.h, as extern function
vsluiter 1:5e22bf1a3817 80 \param index
vsluiter 1:5e22bf1a3817 81 \param subindex
vsluiter 1:5e22bf1a3817 82 */
vsluiter 1:5e22bf1a3817 83 void ESC_objecthandler(uint16 index, uint8 subindex);
vsluiter 1:5e22bf1a3817 84 void TXPDO_update(void);
vsluiter 1:5e22bf1a3817 85 void RXPDO_update(void);
vsluiter 1:5e22bf1a3817 86 void DIG_process(void);
vsluiter 5:6d75f432a41f 87 void sample(void);
vsluiter 1:5e22bf1a3817 88
vsluiter 1:5e22bf1a3817 89 void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 90 {
vsluiter 1:5e22bf1a3817 91 uint8 dummy8;
vsluiter 1:5e22bf1a3817 92 uint16 dummy16;
vsluiter 1:5e22bf1a3817 93 switch (index)
vsluiter 1:5e22bf1a3817 94 {
vsluiter 1:5e22bf1a3817 95 case 0x8000:
vsluiter 1:5e22bf1a3817 96 switch (subindex)
vsluiter 1:5e22bf1a3817 97 {
vsluiter 1:5e22bf1a3817 98 case 0x01:
vsluiter 26:c7959f1fd09a 99 {
vsluiter 26:c7959f1fd09a 100 uint32_t rates[] = {460800,230400,115200,76800,57600,38400,28800,19200,14400,9600};
vsluiter 26:c7959f1fd09a 101 if(Eb.setting8 < (sizeof(rates)/sizeof(uint32_t)) )
vsluiter 27:93c0e4ae943e 102 {
vsluiter 27:93c0e4ae943e 103 XbusGoToConfig();
vsluiter 26:c7959f1fd09a 104 XbusSetBaudRate(Eb.setting8);
vsluiter 26:c7959f1fd09a 105 XbusReset();
vsluiter 27:93c0e4ae943e 106 wait(2);
vsluiter 26:c7959f1fd09a 107 xbus_serial.baud(rates[Eb.setting8]);
vsluiter 26:c7959f1fd09a 108 XbusInitializeXbusMaster();
vsluiter 26:c7959f1fd09a 109
vsluiter 26:c7959f1fd09a 110 }
vsluiter 1:5e22bf1a3817 111 break;
vsluiter 26:c7959f1fd09a 112 }
vsluiter 1:5e22bf1a3817 113 case 0x02:
vsluiter 5:6d75f432a41f 114 dummy16 = 0;//Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16);
vsluiter 1:5e22bf1a3817 115 break;
vsluiter 1:5e22bf1a3817 116 }
vsluiter 1:5e22bf1a3817 117 break;
vsluiter 1:5e22bf1a3817 118 }
vsluiter 1:5e22bf1a3817 119 }
vsluiter 1:5e22bf1a3817 120
vsluiter 1:5e22bf1a3817 121 void TXPDO_update(void)
vsluiter 1:5e22bf1a3817 122 {
vsluiter 1:5e22bf1a3817 123 ESC_write(SM3_sma, &Rb, TXPDOsize, &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 124 }
vsluiter 1:5e22bf1a3817 125
vsluiter 1:5e22bf1a3817 126 void RXPDO_update(void)
vsluiter 1:5e22bf1a3817 127 {
vsluiter 1:5e22bf1a3817 128 ESC_read(SM2_sma, &Wb, RXPDOsize, &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 129 }
vsluiter 1:5e22bf1a3817 130
vsluiter 1:5e22bf1a3817 131 void APP_safeoutput(void)
vsluiter 1:5e22bf1a3817 132 {
vsluiter 1:5e22bf1a3817 133 asm("nop");
vsluiter 1:5e22bf1a3817 134 //Wb.dout = 0;
vsluiter 1:5e22bf1a3817 135 //DOUTPORT = (Wb.dout >> 4) & 0xf0;
vsluiter 1:5e22bf1a3817 136 }
vsluiter 1:5e22bf1a3817 137
vsluiter 1:5e22bf1a3817 138 void DIG_process(void)
vsluiter 1:5e22bf1a3817 139 {
vsluiter 16:bfc7ea6bd1af 140 uint8_t correct_offset;
vsluiter 1:5e22bf1a3817 141 if (APPstate & APPSTATE_OUTPUT) //output enabled
vsluiter 1:5e22bf1a3817 142 {
vsluiter 1:5e22bf1a3817 143 if (ESCvar.ALevent & ESCREG_ALEVENT_SM2) // SM2 trigger ?
vsluiter 1:5e22bf1a3817 144 {
vsluiter 1:5e22bf1a3817 145 ESCvar.ALevent &= ~ESCREG_ALEVENT_SM2;
vsluiter 8:09dcef3ed467 146 RXPDO_update();
vsluiter 1:5e22bf1a3817 147 // dummy output point
vsluiter 5:6d75f432a41f 148 correct_offset = Wb.correct_offset;
vsluiter 5:6d75f432a41f 149 if(correct_offset & 0x01) {
vsluiter 8:09dcef3ed467 150 // led.write(1);
vsluiter 14:e3c49b75fee9 151 asm("nop");
vsluiter 8:09dcef3ed467 152 }
vsluiter 8:09dcef3ed467 153 // else
vsluiter 8:09dcef3ed467 154 // led.write(0);
vsluiter 1:5e22bf1a3817 155 wd_cnt = wd_reset;
vsluiter 1:5e22bf1a3817 156 }
vsluiter 1:5e22bf1a3817 157
vsluiter 5:6d75f432a41f 158 if (!wd_cnt) {
vsluiter 1:5e22bf1a3817 159 ESC_stopoutput();
vsluiter 1:5e22bf1a3817 160 // watchdog, invalid outputs
vsluiter 1:5e22bf1a3817 161 ESC_ALerror(ALERR_WATCHDOG);
vsluiter 1:5e22bf1a3817 162 // goto safe-op with error bit set
vsluiter 1:5e22bf1a3817 163 ESC_ALstatus(ESCsafeop | ESCerror);
vsluiter 1:5e22bf1a3817 164 }
vsluiter 1:5e22bf1a3817 165 }
vsluiter 1:5e22bf1a3817 166 else
vsluiter 1:5e22bf1a3817 167 {
vsluiter 1:5e22bf1a3817 168 //wd_ok = 1;
vsluiter 1:5e22bf1a3817 169 wd_cnt = wd_reset;
vsluiter 1:5e22bf1a3817 170 }
vsluiter 1:5e22bf1a3817 171 if (APPstate) //input or output enabled
vsluiter 5:6d75f432a41f 172 {
vsluiter 25:829af6f3429f 173 //Rb.timestamp = ESCvar.Time;
vsluiter 1:5e22bf1a3817 174 //just some dummy data to test
vsluiter 4:bb72df6dce33 175 //Rb.counter++;
vsluiter 4:bb72df6dce33 176 //Rb.diginput = diginput;
vsluiter 4:bb72df6dce33 177 //Rb.analog[0] = 1;
vsluiter 4:bb72df6dce33 178 //Rb.analog[1] = 2;
vsluiter 14:e3c49b75fee9 179
vsluiter 25:829af6f3429f 180 memcpy(&Rb, &local_Rb, sizeof(Rb));
vsluiter 1:5e22bf1a3817 181 TXPDO_update();
vsluiter 5:6d75f432a41f 182 }
vsluiter 1:5e22bf1a3817 183 }
vsluiter 1:5e22bf1a3817 184
vsluiter 21:6150ca891301 185 //Watch out, this is an uggly fix; for odd numbers of num_bytes, still the trailing byte will be used too.
vsluiter 21:6150ca891301 186 //No memory protection what so ever.
vsluiter 21:6150ca891301 187 void memcpy_byteswap(uint8_t * dest, uint8_t * source, uint16_t num_bytes)
vsluiter 21:6150ca891301 188 {
vsluiter 21:6150ca891301 189 for( int i = 0 ; i < num_bytes ; i+=2 )
vsluiter 21:6150ca891301 190 {
vsluiter 21:6150ca891301 191 dest[i] = source[i+1];
vsluiter 21:6150ca891301 192 dest[i+1] = source[i];
vsluiter 21:6150ca891301 193 }
vsluiter 21:6150ca891301 194 }
vsluiter 17:c5946a0fde83 195
vsluiter 1:5e22bf1a3817 196 int main(void)
vsluiter 1:5e22bf1a3817 197 {
vsluiter 25:829af6f3429f 198
vsluiter 1:5e22bf1a3817 199 /*!< At this stage the microcontroller clock setting is already configured,
vsluiter 1:5e22bf1a3817 200 this is done through SystemInit() function which is called from startup
vsluiter 1:5e22bf1a3817 201 file (startup_stm32f0xx.s) before to branch to application main.
vsluiter 1:5e22bf1a3817 202 To reconfigure the default setting of SystemInit() function, refer to
vsluiter 1:5e22bf1a3817 203 system_stm32f0xx.c file
vsluiter 1:5e22bf1a3817 204 */
vsluiter 1:5e22bf1a3817 205 cpuinit();
vsluiter 1:5e22bf1a3817 206 TXPDOsize = sizeTXPDO();
vsluiter 1:5e22bf1a3817 207 RXPDOsize = sizeRXPDO();
vsluiter 1:5e22bf1a3817 208 wait_ms(200);
vsluiter 1:5e22bf1a3817 209 /*initialize configuration*/
vsluiter 5:6d75f432a41f 210 //Eb.setting16 = 0xABCD;
vsluiter 5:6d75f432a41f 211 //Eb.setting8 = 111;
vsluiter 1:5e22bf1a3817 212 // wait until ESC is started up
vsluiter 1:5e22bf1a3817 213 while ((ESCvar.DLstatus & 0x0001) == 0)
vsluiter 1:5e22bf1a3817 214 ESC_read(ESCREG_DLSTATUS, &ESCvar.DLstatus, sizeof(ESCvar.DLstatus), &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 215
vsluiter 1:5e22bf1a3817 216 // reset ESC to init state
vsluiter 1:5e22bf1a3817 217 ESC_ALstatus(ESCinit);
vsluiter 1:5e22bf1a3817 218 ESC_ALerror(ALERR_NONE);
vsluiter 1:5e22bf1a3817 219 ESC_stopmbx();
vsluiter 1:5e22bf1a3817 220 ESC_stopinput();
vsluiter 1:5e22bf1a3817 221 ESC_stopoutput();
vsluiter 25:829af6f3429f 222
vsluiter 25:829af6f3429f 223 // pc.baud(115200);
vsluiter 1:5e22bf1a3817 224 // application run loop
vsluiter 1:5e22bf1a3817 225 while (1)
vsluiter 1:5e22bf1a3817 226 {
vsluiter 20:ad13d4f5fd98 227 while(xbus_serial.readable())
vsluiter 20:ad13d4f5fd98 228 {
vsluiter 20:ad13d4f5fd98 229 XbusReceiveState(&xbus_master, xbus_serial.getc());
vsluiter 20:ad13d4f5fd98 230 if(xbus_master.rx.checksum_ok)
vsluiter 20:ad13d4f5fd98 231 {
vsluiter 20:ad13d4f5fd98 232 if(xbus_master.rx.buffer[2] == 0x32)
vsluiter 20:ad13d4f5fd98 233 {
vsluiter 25:829af6f3429f 234 memcpy_byteswap((uint8_t *)&local_Rb.timestamp ,&xbus_master.rx.buffer[4], 2);
vsluiter 25:829af6f3429f 235 memcpy_byteswap((uint8_t *)&local_Rb.first.acc[0],&xbus_master.rx.buffer[6],60);
vsluiter 25:829af6f3429f 236 //pc.printf("%x %x\n",Rb.timestamp, *((uint16_t *)(&xbus_master.rx.buffer[4])));
vsluiter 27:93c0e4ae943e 237 PLOEP;
vsluiter 20:ad13d4f5fd98 238 }
vsluiter 20:ad13d4f5fd98 239 xbus_master.rx.checksum_ok = 0;
vsluiter 20:ad13d4f5fd98 240 }
vsluiter 18:6629e8c5d59e 241 }
vsluiter 1:5e22bf1a3817 242 ESC_read(ESCREG_LOCALTIME, &ESCvar.Time, sizeof(ESCvar.Time), &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 243 ESC_ALevent();
vsluiter 1:5e22bf1a3817 244 ESC_state();
vsluiter 1:5e22bf1a3817 245 if (ESC_mbxprocess())
vsluiter 1:5e22bf1a3817 246 {
vsluiter 1:5e22bf1a3817 247 ESC_coeprocess();
vsluiter 1:5e22bf1a3817 248 ESC_xoeprocess();
vsluiter 1:5e22bf1a3817 249 }
vsluiter 1:5e22bf1a3817 250 DIG_process();
vsluiter 1:5e22bf1a3817 251 }
vsluiter 1:5e22bf1a3817 252 }
vsluiter 1:5e22bf1a3817 253
vsluiter 1:5e22bf1a3817 254 /** void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 255 \brief Object handler, declared from esc.h, as extern function
vsluiter 1:5e22bf1a3817 256 \param index
vsluiter 1:5e22bf1a3817 257 \param subindex
vsluiter 1:5e22bf1a3817 258 */
vsluiter 1:5e22bf1a3817 259
vsluiter 1:5e22bf1a3817 260
vsluiter 1:5e22bf1a3817 261 #ifdef USE_FULL_ASSERT
vsluiter 1:5e22bf1a3817 262
vsluiter 1:5e22bf1a3817 263 /**
vsluiter 1:5e22bf1a3817 264 * @brief Reports the name of the source file and the source line number
vsluiter 1:5e22bf1a3817 265 * where the assert_param error has occurred.
vsluiter 1:5e22bf1a3817 266 * @param file: pointer to the source file name
vsluiter 1:5e22bf1a3817 267 * @param line: assert_param error line source number
vsluiter 1:5e22bf1a3817 268 * @retval None
vsluiter 1:5e22bf1a3817 269 */
vsluiter 1:5e22bf1a3817 270 void assert_failed(uint8_t* file, uint32_t line)
vsluiter 1:5e22bf1a3817 271 {
vsluiter 1:5e22bf1a3817 272 /* User can add his own implementation to report the file name and line number,
vsluiter 1:5e22bf1a3817 273 ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
vsluiter 1:5e22bf1a3817 274
vsluiter 1:5e22bf1a3817 275 /* Infinite loop */
vsluiter 1:5e22bf1a3817 276 while (1)
vsluiter 1:5e22bf1a3817 277 {
vsluiter 1:5e22bf1a3817 278 }
vsluiter 1:5e22bf1a3817 279 }
vsluiter 1:5e22bf1a3817 280 #endif
vsluiter 1:5e22bf1a3817 281
vsluiter 1:5e22bf1a3817 282 /**
vsluiter 1:5e22bf1a3817 283 * @}
vsluiter 1:5e22bf1a3817 284 */
vsluiter 1:5e22bf1a3817 285
vsluiter 1:5e22bf1a3817 286 /**
vsluiter 1:5e22bf1a3817 287 * @}
vsluiter 1:5e22bf1a3817 288 */
vsluiter 1:5e22bf1a3817 289
vsluiter 1:5e22bf1a3817 290 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
vsluiter 1:5e22bf1a3817 291