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@21:6150ca891301, 2015-03-06 (annotated)

Committer:
vsluiter
Date:
Fri Mar 06 09:16:18 2015 +0000
Revision:
21:6150ca891301
Parent:
20:ad13d4f5fd98
Child:
22:3f3a9f55054c
Seems to be working! Need to add the sample counter. ; Had to remove some debugging code (still available as commented code) to get the EtherCAT process to run continuously.

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 18:6629e8c5d59e 63 uint8_t copy_new_data = 0;
vsluiter 18:6629e8c5d59e 64 uint8_t copydata;
vsluiter 18:6629e8c5d59e 65
vsluiter 1:5e22bf1a3817 66 DigitalOut led(LED_PIN);
vsluiter 1:5e22bf1a3817 67 DigitalOut et1100_ss(ET1100_SS);
vsluiter 1:5e22bf1a3817 68 DigitalIn et1100_miso(ET1100_MISO);
vsluiter 1:5e22bf1a3817 69 SPI et1100_spi(ET1100_MOSI,ET1100_MISO,ET1100_SCK);
vsluiter 18:6629e8c5d59e 70 MODSERIAL xbus_serial(PTC4,PTC3);
vsluiter 18:6629e8c5d59e 71 xbus_t xbus_master;
vsluiter 21:6150ca891301 72 //MODSERIAL pc(USBTX,USBRX);
vsluiter 16:bfc7ea6bd1af 73
vsluiter 21:6150ca891301 74 //DigitalOut ploep(PTA13);
vsluiter 21:6150ca891301 75 //#define PLOEP do{ploep = !(ploep);}while(0);
vsluiter 19:8ead12aec8d5 76
vsluiter 1:5e22bf1a3817 77 /* Private function prototypes -----------------------------------------------*/
vsluiter 1:5e22bf1a3817 78 /* Private functions ---------------------------------------------------------*/
vsluiter 1:5e22bf1a3817 79
vsluiter 1:5e22bf1a3817 80 /** void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 81 \brief Object handler, declared from esc.h, as extern function
vsluiter 1:5e22bf1a3817 82 \param index
vsluiter 1:5e22bf1a3817 83 \param subindex
vsluiter 1:5e22bf1a3817 84 */
vsluiter 1:5e22bf1a3817 85 void ESC_objecthandler(uint16 index, uint8 subindex);
vsluiter 1:5e22bf1a3817 86 void TXPDO_update(void);
vsluiter 1:5e22bf1a3817 87 void RXPDO_update(void);
vsluiter 1:5e22bf1a3817 88 void DIG_process(void);
vsluiter 5:6d75f432a41f 89 void sample(void);
vsluiter 1:5e22bf1a3817 90
vsluiter 1:5e22bf1a3817 91 void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 92 {
vsluiter 1:5e22bf1a3817 93 uint8 dummy8;
vsluiter 1:5e22bf1a3817 94 uint16 dummy16;
vsluiter 1:5e22bf1a3817 95 switch (index)
vsluiter 1:5e22bf1a3817 96 {
vsluiter 1:5e22bf1a3817 97 case 0x8000:
vsluiter 1:5e22bf1a3817 98 switch (subindex)
vsluiter 1:5e22bf1a3817 99 {
vsluiter 1:5e22bf1a3817 100 case 0x01:
vsluiter 5:6d75f432a41f 101 dummy8 = 0;//Eb.setting8;//Write value to EEPROM; eeprom_write_byte(&eedat.setting8, Wb.setting8);
vsluiter 1:5e22bf1a3817 102 break;
vsluiter 1:5e22bf1a3817 103 case 0x02:
vsluiter 5:6d75f432a41f 104 dummy16 = 0;//Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16);
vsluiter 1:5e22bf1a3817 105 break;
vsluiter 1:5e22bf1a3817 106 }
vsluiter 1:5e22bf1a3817 107 break;
vsluiter 1:5e22bf1a3817 108 }
vsluiter 1:5e22bf1a3817 109 }
vsluiter 1:5e22bf1a3817 110
vsluiter 1:5e22bf1a3817 111 void TXPDO_update(void)
vsluiter 1:5e22bf1a3817 112 {
vsluiter 1:5e22bf1a3817 113 ESC_write(SM3_sma, &Rb, TXPDOsize, &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 114 }
vsluiter 1:5e22bf1a3817 115
vsluiter 1:5e22bf1a3817 116 void RXPDO_update(void)
vsluiter 1:5e22bf1a3817 117 {
vsluiter 1:5e22bf1a3817 118 ESC_read(SM2_sma, &Wb, RXPDOsize, &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 119 }
vsluiter 1:5e22bf1a3817 120
vsluiter 1:5e22bf1a3817 121 void APP_safeoutput(void)
vsluiter 1:5e22bf1a3817 122 {
vsluiter 1:5e22bf1a3817 123 asm("nop");
vsluiter 1:5e22bf1a3817 124 //Wb.dout = 0;
vsluiter 1:5e22bf1a3817 125 //DOUTPORT = (Wb.dout >> 4) & 0xf0;
vsluiter 1:5e22bf1a3817 126 }
vsluiter 1:5e22bf1a3817 127
vsluiter 1:5e22bf1a3817 128 void DIG_process(void)
vsluiter 1:5e22bf1a3817 129 {
vsluiter 16:bfc7ea6bd1af 130 uint8_t correct_offset;
vsluiter 1:5e22bf1a3817 131 if (APPstate & APPSTATE_OUTPUT) //output enabled
vsluiter 1:5e22bf1a3817 132 {
vsluiter 1:5e22bf1a3817 133 if (ESCvar.ALevent & ESCREG_ALEVENT_SM2) // SM2 trigger ?
vsluiter 1:5e22bf1a3817 134 {
vsluiter 1:5e22bf1a3817 135 ESCvar.ALevent &= ~ESCREG_ALEVENT_SM2;
vsluiter 8:09dcef3ed467 136 RXPDO_update();
vsluiter 1:5e22bf1a3817 137 // dummy output point
vsluiter 5:6d75f432a41f 138 correct_offset = Wb.correct_offset;
vsluiter 5:6d75f432a41f 139 if(correct_offset & 0x01) {
vsluiter 8:09dcef3ed467 140 // led.write(1);
vsluiter 14:e3c49b75fee9 141 asm("nop");
vsluiter 8:09dcef3ed467 142 }
vsluiter 8:09dcef3ed467 143 // else
vsluiter 8:09dcef3ed467 144 // led.write(0);
vsluiter 1:5e22bf1a3817 145 wd_cnt = wd_reset;
vsluiter 1:5e22bf1a3817 146 }
vsluiter 1:5e22bf1a3817 147
vsluiter 5:6d75f432a41f 148 if (!wd_cnt) {
vsluiter 1:5e22bf1a3817 149 ESC_stopoutput();
vsluiter 1:5e22bf1a3817 150 // watchdog, invalid outputs
vsluiter 1:5e22bf1a3817 151 ESC_ALerror(ALERR_WATCHDOG);
vsluiter 1:5e22bf1a3817 152 // goto safe-op with error bit set
vsluiter 1:5e22bf1a3817 153 ESC_ALstatus(ESCsafeop | ESCerror);
vsluiter 1:5e22bf1a3817 154 }
vsluiter 1:5e22bf1a3817 155 }
vsluiter 1:5e22bf1a3817 156 else
vsluiter 1:5e22bf1a3817 157 {
vsluiter 1:5e22bf1a3817 158 //wd_ok = 1;
vsluiter 1:5e22bf1a3817 159 wd_cnt = wd_reset;
vsluiter 1:5e22bf1a3817 160 }
vsluiter 1:5e22bf1a3817 161 if (APPstate) //input or output enabled
vsluiter 5:6d75f432a41f 162 {
vsluiter 1:5e22bf1a3817 163 Rb.timestamp = ESCvar.Time;
vsluiter 1:5e22bf1a3817 164 //just some dummy data to test
vsluiter 4:bb72df6dce33 165 //Rb.counter++;
vsluiter 4:bb72df6dce33 166 //Rb.diginput = diginput;
vsluiter 4:bb72df6dce33 167 //Rb.analog[0] = 1;
vsluiter 4:bb72df6dce33 168 //Rb.analog[1] = 2;
vsluiter 14:e3c49b75fee9 169
vsluiter 1:5e22bf1a3817 170
vsluiter 1:5e22bf1a3817 171 TXPDO_update();
vsluiter 5:6d75f432a41f 172 }
vsluiter 1:5e22bf1a3817 173 }
vsluiter 1:5e22bf1a3817 174
vsluiter 21:6150ca891301 175 //Watch out, this is an uggly fix; for odd numbers of num_bytes, still the trailing byte will be used too.
vsluiter 21:6150ca891301 176 //No memory protection what so ever.
vsluiter 21:6150ca891301 177 void memcpy_byteswap(uint8_t * dest, uint8_t * source, uint16_t num_bytes)
vsluiter 21:6150ca891301 178 {
vsluiter 21:6150ca891301 179 for( int i = 0 ; i < num_bytes ; i+=2 )
vsluiter 21:6150ca891301 180 {
vsluiter 21:6150ca891301 181 dest[i] = source[i+1];
vsluiter 21:6150ca891301 182 dest[i+1] = source[i];
vsluiter 21:6150ca891301 183 }
vsluiter 21:6150ca891301 184 }
vsluiter 17:c5946a0fde83 185
vsluiter 1:5e22bf1a3817 186 int main(void)
vsluiter 1:5e22bf1a3817 187 {
vsluiter 1:5e22bf1a3817 188 /*!< At this stage the microcontroller clock setting is already configured,
vsluiter 1:5e22bf1a3817 189 this is done through SystemInit() function which is called from startup
vsluiter 1:5e22bf1a3817 190 file (startup_stm32f0xx.s) before to branch to application main.
vsluiter 1:5e22bf1a3817 191 To reconfigure the default setting of SystemInit() function, refer to
vsluiter 1:5e22bf1a3817 192 system_stm32f0xx.c file
vsluiter 1:5e22bf1a3817 193 */
vsluiter 1:5e22bf1a3817 194 cpuinit();
vsluiter 1:5e22bf1a3817 195 TXPDOsize = sizeTXPDO();
vsluiter 1:5e22bf1a3817 196 RXPDOsize = sizeRXPDO();
vsluiter 1:5e22bf1a3817 197 wait_ms(200);
vsluiter 1:5e22bf1a3817 198 /*initialize configuration*/
vsluiter 5:6d75f432a41f 199 //Eb.setting16 = 0xABCD;
vsluiter 5:6d75f432a41f 200 //Eb.setting8 = 111;
vsluiter 1:5e22bf1a3817 201 // wait until ESC is started up
vsluiter 1:5e22bf1a3817 202 while ((ESCvar.DLstatus & 0x0001) == 0)
vsluiter 1:5e22bf1a3817 203 ESC_read(ESCREG_DLSTATUS, &ESCvar.DLstatus, sizeof(ESCvar.DLstatus), &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 204
vsluiter 1:5e22bf1a3817 205 // reset ESC to init state
vsluiter 1:5e22bf1a3817 206 ESC_ALstatus(ESCinit);
vsluiter 1:5e22bf1a3817 207 ESC_ALerror(ALERR_NONE);
vsluiter 1:5e22bf1a3817 208 ESC_stopmbx();
vsluiter 1:5e22bf1a3817 209 ESC_stopinput();
vsluiter 1:5e22bf1a3817 210 ESC_stopoutput();
vsluiter 1:5e22bf1a3817 211
vsluiter 1:5e22bf1a3817 212 // application run loop
vsluiter 1:5e22bf1a3817 213 while (1)
vsluiter 1:5e22bf1a3817 214 {
vsluiter 20:ad13d4f5fd98 215 while(xbus_serial.readable())
vsluiter 20:ad13d4f5fd98 216 {
vsluiter 20:ad13d4f5fd98 217 XbusReceiveState(&xbus_master, xbus_serial.getc());
vsluiter 20:ad13d4f5fd98 218 if(xbus_master.rx.checksum_ok)
vsluiter 20:ad13d4f5fd98 219 {
vsluiter 20:ad13d4f5fd98 220 if(xbus_master.rx.buffer[2] == 0x32)
vsluiter 20:ad13d4f5fd98 221 {
vsluiter 21:6150ca891301 222 memcpy_byteswap((uint8_t *)&Rb.first.acc[0],&xbus_master.rx.buffer[6],60);
vsluiter 21:6150ca891301 223 //pc.printf("%u , %u\n", Rb.first.acc[0] , *((uint16_t *)(&xbus_master.rx.buffer[6])) );
vsluiter 20:ad13d4f5fd98 224 }
vsluiter 20:ad13d4f5fd98 225 xbus_master.rx.checksum_ok = 0;
vsluiter 21:6150ca891301 226 //PLOEP;
vsluiter 20:ad13d4f5fd98 227 }
vsluiter 18:6629e8c5d59e 228 }
vsluiter 1:5e22bf1a3817 229 ESC_read(ESCREG_LOCALTIME, &ESCvar.Time, sizeof(ESCvar.Time), &ESCvar.ALevent);
vsluiter 1:5e22bf1a3817 230 ESC_ALevent();
vsluiter 1:5e22bf1a3817 231 ESC_state();
vsluiter 1:5e22bf1a3817 232 if (ESC_mbxprocess())
vsluiter 1:5e22bf1a3817 233 {
vsluiter 1:5e22bf1a3817 234 ESC_coeprocess();
vsluiter 1:5e22bf1a3817 235 ESC_xoeprocess();
vsluiter 1:5e22bf1a3817 236 }
vsluiter 1:5e22bf1a3817 237 DIG_process();
vsluiter 1:5e22bf1a3817 238 }
vsluiter 1:5e22bf1a3817 239 }
vsluiter 1:5e22bf1a3817 240
vsluiter 1:5e22bf1a3817 241 /** void ESC_objecthandler(uint16 index, uint8 subindex)
vsluiter 1:5e22bf1a3817 242 \brief Object handler, declared from esc.h, as extern function
vsluiter 1:5e22bf1a3817 243 \param index
vsluiter 1:5e22bf1a3817 244 \param subindex
vsluiter 1:5e22bf1a3817 245 */
vsluiter 1:5e22bf1a3817 246
vsluiter 1:5e22bf1a3817 247
vsluiter 1:5e22bf1a3817 248 #ifdef USE_FULL_ASSERT
vsluiter 1:5e22bf1a3817 249
vsluiter 1:5e22bf1a3817 250 /**
vsluiter 1:5e22bf1a3817 251 * @brief Reports the name of the source file and the source line number
vsluiter 1:5e22bf1a3817 252 * where the assert_param error has occurred.
vsluiter 1:5e22bf1a3817 253 * @param file: pointer to the source file name
vsluiter 1:5e22bf1a3817 254 * @param line: assert_param error line source number
vsluiter 1:5e22bf1a3817 255 * @retval None
vsluiter 1:5e22bf1a3817 256 */
vsluiter 1:5e22bf1a3817 257 void assert_failed(uint8_t* file, uint32_t line)
vsluiter 1:5e22bf1a3817 258 {
vsluiter 1:5e22bf1a3817 259 /* User can add his own implementation to report the file name and line number,
vsluiter 1:5e22bf1a3817 260 ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
vsluiter 1:5e22bf1a3817 261
vsluiter 1:5e22bf1a3817 262 /* Infinite loop */
vsluiter 1:5e22bf1a3817 263 while (1)
vsluiter 1:5e22bf1a3817 264 {
vsluiter 1:5e22bf1a3817 265 }
vsluiter 1:5e22bf1a3817 266 }
vsluiter 1:5e22bf1a3817 267 #endif
vsluiter 1:5e22bf1a3817 268
vsluiter 1:5e22bf1a3817 269 /**
vsluiter 1:5e22bf1a3817 270 * @}
vsluiter 1:5e22bf1a3817 271 */
vsluiter 1:5e22bf1a3817 272
vsluiter 1:5e22bf1a3817 273 /**
vsluiter 1:5e22bf1a3817 274 * @}
vsluiter 1:5e22bf1a3817 275 */
vsluiter 1:5e22bf1a3817 276
vsluiter 1:5e22bf1a3817 277 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
vsluiter 1:5e22bf1a3817 278