File content as of revision 42:ef78d11f2bd7:

/*
 * SOES Simple Open EtherCAT Slave
 *
 * File    : soes.c
 * Version : 0.9.2
 * Date    : 22-02-2010
 * Copyright (C) 2007-2010 Arthur Ketels
 *
 * SOES is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License version 2 as published by the Free
 * Software Foundation.
 *
 * SOES is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * for more details.
 *
 * As a special exception, if other files instantiate templates or use macros
 * or inline functions from this file, or you compile this file and link it
 * with other works to produce a work based on this file, this file does not
 * by itself cause the resulting work to be covered by the GNU General Public
 * License. However the source code for this file must still be made available
 * in accordance with section (3) of the GNU General Public License.
 *
 * This exception does not invalidate any other reasons why a work based on
 * this file might be covered by the GNU General Public License.
 *
 * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual
 * property of, and protected by Beckhoff Automation GmbH.
 */

/****************************************************
Chip type           : STM32F051R8
Clock frequency     : 48 MHz
*****************************************************/

/* Includes ------------------------------------------------------------------*/
#include "mbed.h"
#include "cpuinit.h"
#include "utypes.h"
#include "esc.h"
#include "MODSERIAL.h"
#include "xbus.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define wd_reset 1000

/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
_ESCvar         ESCvar;
uint8			APPstate;
_MBX            MBX[MBXBUFFERS];
_MBXcontrol     MBXcontrol[MBXBUFFERS];
uint8           MBXrun=0;
uint16          SM2_sml,SM3_sml;
_Rbuffer        Rb;
_Wbuffer        Wb;
_Ebuffer        Eb; //EEprom
uint8           TXPDOsize,RXPDOsize;
uint16          wd_ok = 1, wd_cnt = wd_reset;

_Rbuffer local_Rb; //to prevent issues when updating
DigitalOut led(LED_PIN);
DigitalOut et1100_ss(ET1100_SS);
DigitalIn  et1100_miso(ET1100_MISO);
SPI et1100_spi(ET1100_MOSI,ET1100_MISO,ET1100_SCK);
MODSERIAL xbus_serial(PTA2,PTA1, 512);
xbus_t xbus_master;
//MODSERIAL pc(USBTX,USBRX,512);

DigitalOut ploep(PTA13);
//#define PLOEP do{ploep = !(ploep);}while(0);

/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/

/** void ESC_objecthandler(uint16 index, uint8 subindex)
    \brief Object handler, declared from esc.h, as extern function
    \param index
    \param subindex
*/
void ESC_objecthandler(uint16 index, uint8 subindex);
void TXPDO_update(void);
void RXPDO_update(void);
void DIG_process(void);
void sample(void);

void ESC_objecthandler(uint16 index, uint8 subindex)
{
    uint8 dummy8;
    uint16 dummy16;
    switch (index)
    {
    case 0x8000:
        switch (subindex)
        {
        case 0x01:
        {
            uint32_t rates[] = {460800,230400,115200,76800,57600,38400,28800,19200,14400,9600};
            if(Eb.setting8 < (sizeof(rates)/sizeof(uint32_t))  )
            {	
            	XbusGoToConfig();
            	XbusSetBaudRate(Eb.setting8);
            	XbusReset();
            	wait(2);
            	xbus_serial.baud(rates[Eb.setting8]);
            	XbusInitializeXbusMaster();
            	
            }
            else if(Eb.setting8 == 0x80)
            {
            	XbusGoToConfig();
            	XbusSetBaudRate(0x80);
            	XbusReset();
            	wait(2);
            	xbus_serial.baud(960800);
            	XbusInitializeXbusMaster();
            	
            }
            break;
        }
        case 0x02:
            dummy16 = 0;//Eb.setting16;//Write value to EEPROM; eeprom_write_word(&eedat.setting16, Wb.setting16);
            break;
        }
        break;
    }
}

void TXPDO_update(void)
{
  ESC_write(SM3_sma, &Rb, TXPDOsize, &ESCvar.ALevent);
}

void RXPDO_update(void)
{
  ESC_read(SM2_sma, &Wb, RXPDOsize, &ESCvar.ALevent);
}

void APP_safeoutput(void)
{
  asm("nop");
  //Wb.dout = 0;
  //DOUTPORT = (Wb.dout >> 4) & 0xf0;
}

void DIG_process(void)
{
  uint8_t correct_offset;
  if (APPstate & APPSTATE_OUTPUT) //output enabled
    {
        if (ESCvar.ALevent & ESCREG_ALEVENT_SM2) // SM2 trigger ?
        {
          ESCvar.ALevent &= ~ESCREG_ALEVENT_SM2;
          RXPDO_update();
            // dummy output point
            correct_offset = Wb.correct_offset;
            if(correct_offset & 0x01) {
            //    led.write(1);
  				asm("nop");
           }
           // else
           // 	led.write(0);
            wd_cnt = wd_reset;
        }

        if (!wd_cnt) {
            ESC_stopoutput();
            // watchdog, invalid outputs
            ESC_ALerror(ALERR_WATCHDOG);
            // goto safe-op with error bit set
            ESC_ALstatus(ESCsafeop | ESCerror);
        }
    }
    else
    {
        //wd_ok = 1;
        wd_cnt = wd_reset;
    }
  if (APPstate) //input or output enabled
    {	
        //Rb.timestamp = ESCvar.Time;
        //just some dummy data to test
        //Rb.counter++;
        //Rb.diginput = diginput;
        //Rb.analog[0] = 1;
        //Rb.analog[1] = 2;
 
		memcpy(&Rb, &local_Rb, sizeof(Rb));
        TXPDO_update();
    } 
}

float ReverseFloat( const float inFloat )
{
   float retVal;
   char *floatToConvert = ( char* ) & inFloat;
   char *returnFloat = ( char* ) & retVal;

   // swap the bytes into a temporary buffer
   returnFloat[0] = floatToConvert[3];
   returnFloat[1] = floatToConvert[2];
   returnFloat[2] = floatToConvert[1];
   returnFloat[3] = floatToConvert[0];

   return retVal;
}

//Watch out, this is an uggly fix; for odd numbers of num_bytes, still the trailing byte will be used too.
//No memory protection what so ever.
void memcpy_byteswap(uint8_t * dest, uint8_t * source, uint16_t num_bytes)
{
	for( int i = 0 ; i < num_bytes ; i+=2 )
	{
		dest[i]   = source[i+1];
		dest[i+1] = source[i];
	}
}

//Watch out, this is an uggly fix; for odd numbers of num_bytes, still the trailing byte will be used too.
//No memory protection what so ever.
void memcpy_floatswap(uint8_t * dest, uint8_t * source, uint16_t num_bytes)
{
	for( int i = 0 ; i < num_bytes ; i+=4 )
	{
		dest[i]   = source[i+3];
		dest[i+1] = source[i+2];
		dest[i+2]   = source[i+1];
		dest[i+3] = source[i];
	}
}


int main(void)
{

    /*!< At this stage the microcontroller clock setting is already configured,
         this is done through SystemInit() function which is called from startup
         file (startup_stm32f0xx.s) before to branch to application main.
         To reconfigure the default setting of SystemInit() function, refer to
         system_stm32f0xx.c file
       */
    cpuinit();
	TXPDOsize = sizeTXPDO();
	RXPDOsize = sizeRXPDO();
    wait_ms(200);
    /*initialize configuration*/
    //Eb.setting16 = 0xABCD;
    Eb.setting8  = 1;
    // wait until ESC is started up
    while ((ESCvar.DLstatus & 0x0001) == 0)
        ESC_read(ESCREG_DLSTATUS, &ESCvar.DLstatus, sizeof(ESCvar.DLstatus), &ESCvar.ALevent);

// reset ESC to init state
    ESC_ALstatus(ESCinit);
    ESC_ALerror(ALERR_NONE);
    ESC_stopmbx();
    ESC_stopinput();
    ESC_stopoutput();
	
//	pc.baud(115200);
// application run loop
    while (1)
    {
        while(xbus_serial.readable())
        {
        	XbusReceiveState(&xbus_master, xbus_serial.getc());
        	if(xbus_master.rx.checksum_ok)
        	{
        		if(xbus_master.rx.buffer[2] == 0x32)
        		{
        			for(int sensor = 0; sensor < 3 ; sensor++)
        			{
        				const int num_bytes_quat = 4*4;//sizeof(float);//4 floats * 4 bytes
        				const int num_bytes_timestamp = 2;
        				const int num_bytes_msg = num_bytes_quat + num_bytes_timestamp; //timestamp
	        			memcpy_byteswap((uint8_t *)&local_Rb.xsens_imu[sensor].samplecounter   ,&xbus_master.rx.buffer[6]+num_bytes_quat, num_bytes_timestamp);
	        			memcpy_floatswap((uint8_t *)&local_Rb.xsens_imu[sensor].q0,&xbus_master.rx.buffer[6+(sensor*num_bytes_msg)], num_bytes_quat);
	        			//memcpy_byteswap((uint8_t *)&local_Rb.xsens_imu[sensor].samplecounter,&xbus_master.rx.buffer[6+num_bytes+(sensor*num_bytes)],2);
	        			//pc.printf("%x %x\n",Rb.timestamp, *((uint16_t *)(&xbus_master.rx.buffer[4])));
	        			//PLOEP;
        			}
        		}
        		xbus_master.rx.checksum_ok = 0;
        	}
    	}
        ESC_read(ESCREG_LOCALTIME, &ESCvar.Time, sizeof(ESCvar.Time), &ESCvar.ALevent);
        ESC_ALevent();
        ESC_state();
        if (ESC_mbxprocess())
        {
            ESC_coeprocess();
            ESC_xoeprocess();
        }
        DIG_process();
    }
}

/** void ESC_objecthandler(uint16 index, uint8 subindex)
    \brief Object handler, declared from esc.h, as extern function
    \param index
    \param subindex
*/


#ifdef  USE_FULL_ASSERT

/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t* file, uint32_t line)
{
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

    /* Infinite loop */
    while (1)
    {
    }
}
#endif

/**
  * @}
  */

/**
  * @}
  */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/