motortest/TC234_startup.c

/*****************************************************
 *
 * TC234_startup.c







 *
 * Description : Hello World in C, ANSI-style
 *
 */



#include "TC234_startup.h"

GPIO_config test_pin;


static TimerVal tOn[3] = {700,700,700}; // container for TRANSISTOR ON TIMES
static open_loop_driver open_loop;		// container for open loop dirver

static spi_config config_SPI;			// container for SPI configuration


static qspi_dma_rx_stucture TLI_dma_rec_structure;  // container for raw_current values

static qspi_dma_rx_stucture TLI_dma_rec_structure_copy; // copy of container for raw_current values


static uint32 test_value[10];
static uint32 test_rec_value[10]; 			// test array for DMA


static sint32 position_raw;					// raw position - from incremental encoder
static sint16 pos_electrical;				// electrical -position

static uint32 torque_raw;
static float32 torque;



static uint32 linked_list_buffer[9];		// source address buffer for dma transmit buffer

IfxGpt12_IncrEnc Inc_Enc_struct;			// init struct for Incremental Encoder

static float I_a,I_b,I_c ;					// Phase Currents I_a I_b I_c
static float32 I_imag,I_real;				// I_d = I_real,   I_q= I_imag

//static float32 elAngleConst;

static TFoc fo_controller;					// current control container

static float32 U_a,U_b,U_c;					// T_on A,B,C proportional to UA UB UC

static IfxVadc_Adc vadc_module;
static IfxVadc_Adc_Channel Vadc_input_channel;




IFX_INTERRUPT(vadcgroup0ISR, 0, CONTROL_ISR_PRIO);
IFX_INTERRUPT(dma_tx_empty_isr, 0, ISR_PRIORITY_TX_EMPTY);
IFX_INTERRUPT(qspi3_er_isr, 0, ISR_PRIORITY_QSPI3_ER);
IFX_INTERRUPT(qspi3_rx_empty,0, ISR_PRIORITY_RX_EMPTY);
IFX_INTERRUPT(qspi3_rx_interrupt,0, 101);

IFX_INTERRUPT(ISR_Encoder_Gpt12, 0, INTERRUPT_ENCODER_GPT12);

IFX_INTERRUPT(dma_test_isr, 0, DMA_TEST_PRIO);


void ISR_Encoder_Gpt12(void)
{
    IfxGpt12_IncrEnc_isrOnZeroPulse(&Inc_Enc_struct);
    __enable();
}

void dma_test_isr() {
	//set_GPIO(&test_pin) ;
	//clear_GPIO(&test_pin) ;


    IfxCpu_enableInterrupts();



}


void vadcgroup0ISR(void) {
	IfxCpu_disableInterrupts();
	//set_GPIO(&test_pin) ;



	TLI_dma_rec_structure_copy=TLI_dma_rec_structure; // copy Current raw data in working copy
	I_a=avarage_sensor_values(&(TLI_dma_rec_structure_copy.sensor1)); // get average current of last 50us
	I_b=avarage_sensor_values(&(TLI_dma_rec_structure_copy.sensor2)); // get average current of last 50us
	I_c=avarage_sensor_values(&(TLI_dma_rec_structure_copy.sensor3)); // get average current of last 50us

	trigger_vadc(&vadc_module,IfxVadc_ChannelId_0);

	T3Phase phase_currents= {I_a,I_b,I_c};



	position_raw=get_inc_encoder_position(&Inc_Enc_struct); // get raw position (mechanical) from incremental encoder
	pos_electrical = (sint16)get_electrical(position_raw);	// convert mechanical position to electrical position


	if(Inc_Enc_struct.base.status.B.notSynchronised == TRUE ){
		// if incremental encoder has not passed the zero marker
		do_open_loop_step(&open_loop); //
		SpaceVectorModulation(open_loop.complex_ptr, open_loop.pwm_period_ticks, open_loop.tOn);
		set_three_channel_PWM(&Timer_driver,&PWM_driver, open_loop.tOn);
		CplxStdReal I_rot=fo_current_ctrl_step(&fo_controller, pos_electrical,&phase_currents); // dummy calculation
		I_imag=I_rot.imag;
		I_real=I_rot.real;
	}
	else {
		//FoC Step

		CplxStdReal I_rot=own_fo_current_ctrl_step(&fo_controller, pos_electrical,&phase_currents); // field oriented control
		SpaceVectorModulation(I_rot, open_loop.pwm_period_ticks, tOn);
		set_three_channel_PWM(&Timer_driver,&PWM_driver,tOn);
		I_imag=I_rot.imag;

		I_real=I_rot.real;

	}

	torque_raw= return_raw(&Vadc_input_channel);

	torque=convert_raw_to_voltage(torque_raw);

	U_a=open_loop.tOn[0];
	U_b=open_loop.tOn[1];
	U_c=open_loop.tOn[2];




	IfxCpu_enableInterrupts();


}

void dma_tx_empty_isr() {
	//set_GPIO(&test_pin);

    IfxCpu_enableInterrupts();
    //dma_Daisy_chain_Transaction_test();
    //clear_GPIO(&test_pin);
    //Qspi_SpiMaster_write_input_fifo(&(config_SPI.spiMasterChannel[0]));
   // IfxQspi_SpiMaster_isrTransmit(&config_SPI.spiMaster);
    //chHandle->base.txHandler(&chHandle->base);
    //handle->base.txCount++;






}

void qspi3_rx_empty() {
	//set_GPIO(&test_pin);
    IfxCpu_enableInterrupts();
    IfxQspi_SpiMaster_isrReceive(&config_SPI.spiMaster);
   // clear_GPIO(&test_pin);


}

void qspi3_er_isr() {

	IfxCpu_enableInterrupts();
	IfxQspi_SpiMaster_isrError(&config_SPI.spiMaster);


}

void qspi3_rx_interrupt() {
	Qspi_SpiMaster_write_input_fifo(&(config_SPI.spiMasterChannel[0]));
	uint8 max=Qspi_SpiMaster_get_rx_fill_level(&config_SPI.spiMasterChannel[0]);

	for(int i=0;i<max;i++) {
		test_value[i]= *(uint32*)Qspi_SpiMaster_get_rx_addr(&config_SPI.spiMasterChannel[0]) ;
	}

	if(test_value[0] >=0xA000) {
	set_GPIO(&test_pin);

	clear_GPIO(&test_pin);
	}

}


int main(void)
{

	//test pin for debugging an measurement purposes
	test_pin.port=&MODULE_P02;
	test_pin.pin=0;

	LookUp_Init(); // init sin cos lookup table

	static TimerVal pwm_period_in_ticks;

//	static boolean LS_Status;		//test for lockstepping - not implemented



	MYIfxScuCcu_init(&IfxScuCcu_defaultClockConfig); // Clock configuration - default clock config not working properly


	set_IO_start();	// set pins to default settings

	initGpio(&test_pin);	// set testpin to Gernal purpose output

	set_GPIO(&test_pin);	// testpin = 1

	clear_GPIO(&test_pin);	//testpin = 0


	// Dummy values for SPI output
	// values dont matter
	config_SPI.spi3TxBuffer[0]=0x1111;
	config_SPI.spi3TxBuffer[1]=0x5555;
	config_SPI.spi3TxBuffer[2]=0x1111;
	config_SPI.spi3RxBuffer[0]=0x1234;
	config_SPI.spi3RxBuffer[1]=0x1234;
	config_SPI.spi3RxBuffer[2]=0x1234;





// initialize UART
	init_uart_poll(UART_BAUDRATE);

	initPwm() ; // initiaize PWM wit GTM Module
	initSpi_TLI4970(&config_SPI);	// initilize SPI interface
	pwm_period_in_ticks= get_pwm_period_ticks(&Timer_driver,20000.0);
	init_open_loop_driver (&open_loop,10.0,20, 1/20000.0, pwm_period_in_ticks);	// initialize open Loop driver


	init_encoder(&Inc_Enc_struct);	// initialize incremental encoder



	init_fo_controller(&fo_controller,20000); // init PI controller for I_d and I_q
	set_Iq_ref_current(&fo_controller,4.0);	  // set Iq in Ampere
//	const TimerVal period_in_ticks=PWM_driver.base.timer->period;
	// init vadc
	init_vadc_for_torque_sensor(&vadc_module,&Vadc_input_channel);


	spi_write_Dma_src_addr_buffer(linked_list_buffer,&config_SPI);	// Fill linked list Buffer with values
	uint32 beacon_addr=Qspi_SpiMaster_get_bacon_entry(&config_SPI.spiMasterChannel[0]);	// get BEACON-Register Addr
	uint32	data_enty_addr=Qspi_SpiMaster_get_data_entry(&config_SPI.spiMasterChannel[0]);// get DATAENTRY-Register Addr

	uint32 data_exit= Qspi_SpiMaster_get_rx_addr(&config_SPI.spiMasterChannel[0]);	// get DATAEXIT-REGISTER ADDR


	init_dma_linked_list(IfxDma_ChannelId_5, linked_list_buffer,beacon_addr,data_enty_addr);	// init dma transmit FSM
	init_dma_linked_list_read(IfxDma_ChannelId_6,&TLI_dma_rec_structure, data_exit);			// init dma receive FSM


	//wait for current sensors
	polling_wait_ms(0.1);

	clear_GPIO(&test_pin);
	int iterator=0;


//	LS_Status=check_if_ls_enabled();

//	polling_wait_ms(0.1);
//	LS_Status=check_if_ls_occured();
//	polling_wait_ms(0.1);
//	inject_ls_error();
//	polling_wait_ms(0.1);
//	LS_Status=check_if_ls_occured();


	__enable();		// enable interrupts

	//Qspi_SpiMaster_write_input_fifo(&(config_SPI.spiMasterChannel[0]));
	init_dma_startup_linked_list(IfxDma_ChannelId_5);  // start DMA FSM
	while(1) {

		//printf( "Hello world\n" );
		//Qspi_SpiMaster_write_input_fifo(&(config_SPI.spiMasterChannel[0]));

		if(iterator >=4) {
		//	dma_daisy_chain_reset();

			iterator=0;
		}
		iterator++;

		_out_uart('O');
		polling_wait_ms(0.1);



		_out_uart('O');
		_out_uart('k');
		//__enable();
	}
}

void execute_dma_transmit(spi_config * config) {
	printf("Hello World\n");

	uint32 *test_addr = &test_value;

  //  while (IfxQspi_SpiMaster_getStatus(&config->spiMasterChannel[0]) == SpiIf_Status_busy){
  //  printf("Hello World\n");
	//}


    DMAIfxQspi_SpiMaster_exchange(&config->spiMasterChannel[0],test_rec_value,&config->spi3RxBuffer[0], 2,test_addr);
}

mystr test_komplex(mystr m) {
	mystr k;
	k.a= m.a*2;
	k.b= m.b*2;
	return k;
}


static void polling_wait_ms(float32 time)
{
   uint32 stmCount      = (uint32)(IfxScuCcu_getStmFrequency() * time);
  uint32 stmCountBegin = STM0_TIM0.U;

    while ((uint32)(STM0_TIM0.U - stmCountBegin) < stmCount)
    {
    	__nop();
    }
}