T1 F281x Mcbsp Transmitter Configuration

Emmanuel May 13, 2011 Coded in C for the TI F28x
/**********************************************************************/
#include "DSP281x_Device.h"
/**********************************************************************
Parameter					Condition		Register		Value

Delay between data frames:	1 bit			XCR2.XDATDLY	0b01
Word Length:				16 bits			XCR1.XDLEN1		0b010
Words per frame:			1 word			XCR1.XFRLEN1	0
sample rate:				150 kHz			SRGR1.CLKGDV	0xC7
															(LSPCLK=30 MHz)

************************************************************************/
void InitMcbsp(void)
{

/*** Mcbsp	Reset	*/

	McbspaRegs.SPCR2.bit.XRST= 0;
	McbspaRegs.SPCR2.bit.GRST= 0;
	McbspaRegs.SPCR2.bit.FRST= 0;
	
/*** SPCR2 Register Configuration	*/

	McbspaRegs.SPCR2.all = 0x0000;
	
// bit 15-10 	0's:	Reserved
// bit 9 		0:		FREE Free running mode
// bit 8 		0:		SOFT Soft bit
// bit 7 		0:		FRST Frame sync generator reset
// bit 6 		0:		GRST Sample rate generator reset
// bit 5-4 		00:		XINTM Transmit interrupt mode
// bit 3 		0:		XSYNCERR Transmit synchronization error
// bit 2 		0:		XEMPTY Transmit shift register (XSR) empty
// bit 1 		0:		XRDY Transmitter ready
// bit 0 		0:		XRST Transmitter reset
	
/*** SPCR1 Register Configuration 		*/	

	McbspaRegs.SPCR1.all = 0x0000;

// bit 15 		0:		DLB Digital Loop back mode
// bit 14-13 	0:		RJUST Receive sign extension and justification mode
// bit 12-11 	00:		CLKSTP Clock Stop Mode
// bit 10-8 	0's:	Reserved
// bit 7 		0:		DXENA DX enabler. 
// bit 6 		0:		ABIS ABIS mode
// bit 5-4 		00:		RINTM Receive Interrupt Mode
// bit 3 		0		RSYNCERR Receive Synchronization error
// bit 2 		0		RFULL Receive shift register full
// bit 1 		0		RRDY Receiver ready
// bit 0 		0		RRST Receiver reset

/*** XCR2 Register Configuration 	*/	

	McbspaRegs.XCR2.all = 0x0001;

// bit 15 		0:		XPHASE Transmit phase
// bit 14-8 	0's;	XRFRLEN2 Transmit frame length 2
// bit 7-5 		000:	XWDLEN2 Transmit word length 2
// bit 4-3 		00:		XCOMPAND Transmit companding mode
// bit 2 		0:		XFIG Transmite frame ignore
// bit 1-0 		01:		XDATDLY Data Delay (Delay in bits between two frames)
	
/*** XCR1 Register Configuration 	*/	

	McbspaRegs.XCR1.all = 0x0040;
	
// bit 15 		0		Reserved
// bit 14-8 	0's:	XFRLEN1 Frame Length 1 (Words per frame)
// bit 7-5 		010:	XWDLEN1 Word Length 1  (bits per word)
// bit 4 		0:		Reserved
// bit 3-0 		0's:	Reserved

/*** SRGR2 Register Configuration 	*/

	McbspaRegs.SRGR2.all = 0x2011;

// bit 15 		0;		GSYNC Sample rate generator clock synchronization
// bit 14 		0:		Reserved
// bit 13 		1:		CLKSM Mcbsp sample rate generator clock mode: internal clock
// bit 12 		0:		FSGM Transmit frame synchronization mode
// bit 11-0 	x011:	FPER Frame Period (number of CLKG cycles between sync signals): 18 cycles

/*** SRGR1 Register Configuration 	*/	

	McbspaRegs.SRGR1.all = 0x00E8;

// bit 15-8 	x00:	FWID Frame Width
// bit 7-0 		xC7		CLKGDV Sample rate generator clock divider

/*** MCR2 Register Configuration 	*/	

	McbspaRegs.MCR2.all = 0x0200;
	
// bit 15-10 	0's:	Reserved
// bit 9 		1:		XMCME Enhanced transmit multichannel selection enable
// bit 8-7 		00:		XPBBLK Receive/transmit partition B block
// bit 6-5 		00:		XPABLK Receive/transmit partition A block
// bit 4-2 		000:	XCBLK Receive/transmit current block
// bit 1-0 		00:		XMCM Transmit multichannel selection enable

/*** MCR1 Register Configuration  */	

	McbspaRegs.MCR1.all = 0x0201;
	
// bit 15-10 	0's:	Reserved
// bit 9 		1		RMCME Enhanced receive multichannel selection enable
// bit 8-7 		00		RPBBLK Receive/transmit partition B block
// bit 6-5 		00		RPABLK Receive/transmit partition A block
// bit 4-2 		000		RCBLK Receive transmit current block
// bit 1 		0		Reserved
// bit 0 		1		RMCM Receive multichannel selection enable

/*** PCR Register Configuration */	

	McbspaRegs.PCR.all = 0x0A00;

// bit 15-12 	0's:	Reserved
// bit 11 		1:		FSXM Transmit frame synchronization mode
// bit 10 		0:		FSRM Receive frame synchronization mode
// bit 9 		1:		CLKXM Transmit frame synchronization mode (internal, FSR as output)
// bit 8 		0:		CLKRM Receiver clock mode
// bit 7 		0:		SCLKME Sample clock mode selection bit
// bit 6 		0:		CLKS_STAT Reserved 
// bit 5 		0:		DX_STAT DX pin status
// bit 4 		0:		DR_STAT DR pin status
// bit 3 		0:		FSXP Transmit frame synchronization polarity
// bit 2 		0:		FSRP Receive frame synchronization polarity
// bit 1 		0:		CLKXP Tramsmit clock polarity
// bit 0 		0:		CLKRP Receive clock polarity
	

	
/*** Mcbsp Start-up		*/

	McbspaRegs.SPCR2.bit.XRST= 1;
	McbspaRegs.SPCR2.bit.GRST= 1;
	McbspaRegs.SPCR2.bit.FRST= 1;
	

/*** Interruption Configuration  */

	McbspaRegs.MFFINT.bit.XINT= 1;

	PieCtrlRegs.PIEIER6.bit.INTx6= 1;	
    IER |= 0x0020;   
    
    

}

/**End of function*/

TI F281x PWM Configuration

Emmanuel April 18, 2011 Coded in C for the TI F28x
/**********************************************************************/
#include "DSP281x_Device.h"
/***********************************************************************/

void InitPwm(void)
{

asm( "EALLOW");	//Enable write operations in protected registers

/*** Independent Compare Output Enable	*/

	EvaRegs.EXTCONA.bit.INDCOE= 1;

/********************************************************************
*	Write values in the two following registers until all the other
*	registers have been written, as their value depends on the other
*	registers. 							*
********************************************************************/

/*** Timer 1 Period Regiser	*/

	EvaRegs.T1PR = 0xB71A;			//use this value for a frequency of 200 Hz assuming
								    //sysclkout=150 MHz and its pre-scaler=16
								    
/*** CMPR1 Register Configuration */

	EvaRegs.CMPR1= 0x493D;		//to get 60% of duty cycle this value equals the 1-0.6= 40% period
		
/********************************************************************/

	EvaRegs.GPTCONA.all= 0x0050;
	
// bit 15 		0		Reserved 
// bit 14 		0		T2STAT GP timer 2 Status. 
// bit 13 		0		T1STAT GP timer 1 Status. 
// bit 12 		0		T2CTRIPE T2CTRIP Enable. 
// bit 11 		0		T1CTRIPE T1CTRIP Enable. 
// bit 10-9 	00		T2TOADC Start ADC with timer 2 event
// bit 8-7 		00		T1TOADC Start ADC with timer 1 event
// bit 6 		1		TCMPOE Timer compare output enable. 
// bit 5		0		T2CMPOE Timer 2 compare output enable. 
// bit 4 		1		T1CMPOE Timer 1 Compare Output Enable. 
// bit 3-2 		00		T2PIN Polarity of GP timer 2 compare output
// bit 1-0 		00		T1PIN Polarity of GP timer 1 compare output
	

/*** COMCONA Register Configuration*/

	EvaRegs.COMCONA.all = 0x8020;
	
// bit 15 		1:		CENABLE: Compare Enable
// bit 14-13 	00:		CLD1, CLD0: CMPRx Reload Condition
// bit 12 		0:		SVENABLE: PWM Vector Enable
// bit 11-10 	00:		ACTRLD1,ACTRLD0: Action Control Regiser Reload Condition
// bit 9 		0:		FCMPOE: Full Compare Output Enable
// bit 8 		0:		PDPINTA: PDPINTA Status
// bit 7 		0:		FCMP3OE: Compare 3 Output Enable
// bit 6 		0:		FCMP2OE: Compare 2 Output Enable
// bit 5 		1:		FCMP1OE: Compare 1 Output Enable
// bit 4-3 		00:		Reserved
// bit 2 		0:		C3TRIPE: C3TRIP Enable 
// bit 1 		0:		C2TRIPE: C2TRIP Enable 
// bit 0 		0:		C1TRIPE: C1TRIP Enable 

/** ACTRA Register Configuration*/

	EvaRegs.ACTRA.all = 0x0002;
	
// bit 15 		0:		SVRDIR: PWM Vector rotate direction 
// bit 14-12 	000:	D2-D: PWM Vector basic bits
// bit 11-10 	00:		CMP6ACT1-0 Compare Action in pin 6, CMP6.
// bit 9-8 		00:		CMP5ACT1-0 Compare Action in pin 5, CMP5.
// bit 7-6 		00:		CMP4ACT1-0 Compare Action in pin 4, CMP4.
// bit 5-4 		00:		CMP3ACT1-0 Compare Action in pin 3, CMP3
// bit 3-2 		00:		CMP2ACT1-0 Compare Action in pin 2, CMP2
// bit 1-0 		10		CMP1ACT1-0 Compare Action in el pin 1, CMP1

/*** T1CON Register Configuration*/

	EvaRegs.T1CON.all = 0x9442;
	
// bit 15:14 	10:		Emulation Control bits
// bit 13 		0:		Reserved
// bit 12-11 	10:		TMODE1-TMODE0 Counter Mode Selection
// bit 10-8 	100:	TPS2-TPS0 Pre-scaler: 100= HSPCLK/16
// bit 7 		0:		T2SWT1 Timer 2 trigger by timer 1
// bit 6 		1:		TENABLE Timer Enable
// bit 5-4 		00:		TCLKS(1,0) Clock Source Selection
// bit 3-2 		00:		TCLD(1,0) Timer Compare Register Reload Condition
// bit 1 		1:		TECMPR Compare Operations Enable
// bit 0		0:		SELT1PR: Period Register Selection
						

asm( "EDIS");	//Protected Registers Write Disabled

}

/**End of function*/

Discrete Cosine Transform 1D Matrix Processing

Emmanuel April 16, 2011 Coded in Matlab
function [Sdct] = matrix_dct(P)    %where P is the vector to apply the dct
N=length(P);
Cf=[1/sqrt(2),ones(1,N-1)];
S=zeros(1,N);
for f=0: N-1;
    for x=0: N-1;
        W(f+1, x+1)=cos((2*x+1)*pi*f/(2*N));  %matrix kernel
    end
end
Sdct=W*P';   
Sdct=sqrt(2/N)*Sdct.*Cf';     %constant product

Discrete Cosine Transform 2D Serial Processing

Emmanuel April 16, 2011 Coded in Matlab
function [S] = dct_2d(P)    %where P is the matrix to apply the dct
N=length(P);
Cu=[1/sqrt(2),ones(1,N-1)];   %Constant coeficients d1
Cv=[1/sqrt(2),ones(1,N-1)];   %Constant coeficients d2
S=zeros(N,N);
for v=0: N-1;
    for y=0: N-1;
        for u=0: N-1;
            for x=0: N-1;    %serial processing
                S(u+1,v+1)=S(u+1,v+1)+Cu(u+1)*Cv(v+1)*P(x+1,y+1)*cos((2*x+1)*pi*u/(2*N))*cos((2*y+1)*pi*v/(2*N));
            end;
        end;
    end;
end;
S=(2/N)*S;  %final result

TI F281x and F2833x ADC Configuration

Emmanuel February 17, 2011 Coded in C for the TI F28x
/**********************************************************************/
#include "DSP281x_Device.h"		//if using F2812
//#include "DSP2833x_Device.h" //if using F28335
/**********************************************************************
void InitAdc(void)
{
	
/*** ADC Reset*/

	AdcRegs.ADCTRL1.bit.RESET = 1;

	asm(" RPT #22 || NOP");		//Wait 2 clocks assuming ADCCLK=12.5 Mhz and SYSCLKOUT=150 Mhz
	
																			
/*** ADC Power-On	*/

	AdcRegs.ADCTRL3.all = 0x00ED;
			
// bit 15-9      0's:    reserved
// bit 8		 0:		 EXTREF
// bit 7-6       11:     ADCBGRFDN, Reference voltage, 00=off, 11=on
// bit 5         1:      ADCPWDN, Main Power Source, 0=off, 1=on
// bit 4-1       0110:   ADCCLKPS, clock prescaler, FCLK=HSPCLK/(2*ADCCLKPS)
// bit 0         1:      SMODE_SEL, 0=sequencial sampling, 1=simultaneous sampling

	DelayUs(5000);			//TI's asm function	
							//Wait 5 ms

/*** ADCMAXCONV	Register configuration*/

	AdcRegs.ADCMAXCONV.all = 0x0000;
	
// bit 15-7      0's:    reserved
// bit 6-4       000:    MAX_CONV2 
// bit 3-0       0000:   MAX_CONV1  (0 = 1 conversion)

/*** Select channels for sampling in SEQ1	*/

	AdcRegs.ADCCHSELSEQ1.bit.CONV00 = 0;	//Channels A0 and B0
		
/***  ADCTRL1 Register configuration	*/

	AdcRegs.ADCTRL1.all = 0x0700;
	
// bit 15        0:      reserved
// bit 14        0:      RESET
// bit 13-12     00:     SUSMOD, 00=ignore emulation suspend
// bit 11-8      0111:   ACQ_PS Acquisition window pre-scaler
// bit 7         0:      CPS Core clock
// bit 6         0:      CONT_RUN
// bit 5         0:      SEQ_OVRD
// bit 4         0:      SEQ_CASC
// bit 3-0       0000:   reserved

/*** ADCTRL2 Register configuration	*/

	AdcRegs.ADCTRL2.all = 0x0800;
	
// bit 15        0:      EVB_SOC_SEQ: SOC trigger by EVB
// bit 14        0:      RST_SEQ1: SEQ1 reset
// bit 13        0:      SOC_SEQ1: trigger signal for SEQ1
// bit 12        0:      reserved
// bit 11        1:      INT_ENA_SEQ1: SEQ1 interruption enable
// bit 10        0:      INT_MOD_SEQ1: interruption mode
// bit 9         0:      reserved
// bit 8         0:      EVA_SOC_SEQ1, 1=SEQ1 starts with EVA_SOC trigger
// bit 7         0:      EXT_SOC_SEQ1, 1=SEQ1 starts with ADCSOC input signal
// bit 6         0:      RST_SEQ2
// bit 5         0:      SOC_SEQ2
// bit 4         0:      reserved
// bit 3         0:      INT_ENA_SEQ2: SEQ1 interruption enabled
// bit 2         0:      INT_MOD_SEQ2: interruption mode
// bit 1         0:      reserved
// bit 0         0:      EVB_SOC_SEQ2

/*** ADC interruption enable	*/
PieCtrlRegs.PIEIER1.bit.INTx6= 1;	                 
IER |= 0x0001;              						

}

CRC calculation

Emmanuel February 15, 2011 Coded in C
unsigned char crc(unsigned char data[],int L)
{
	
    unsigned char P=0x83;
    unsigned char *data_ptr;
	unsigned char crc_result;
	int bit;
	crc_result=0;
	for(data_ptr=data;data_ptr<data+L;data_ptr++)
	{
		for(bit=7;bit>=0;bit--)
		{
            crc_result=(crc_result<<1)+((*data_ptr&0x1<<bit)>>bit);
			if (crc_result>=128)
			{
				crc_result=crc_result^P;
			}
		}
	}
	//multiply by 2^n-k;
	for(bit=6;bit>=0;bit--)
		{
            crc_result=(crc_result<<1);
			if (crc_result>=128)
			{
				crc_result=crc_result^P;
			}
		}

	return crc_result;
		
}

TI F281x SCI Configuration

Emmanuel February 4, 2011 Coded in C for the TI F28x
/**********************************************************************/
#include "DSP281x_Device.h"
/**********************************************************************
* Function: InitSci()
*
* Description:  This function initializes F281x SCI. In this case the desired rate
*				is 9600 bps, 8 databits, 1 stop bit and no parity.
**********************************************************************/
void InitSci(void)
{

/*** SCI reset	*/

	ScibRegs.SCICTL1.bit.SWRESET= 0;
	
/*** Setting the baud rate to 9600 bps assuming LSPCLK=150 Mhz */

	ScibRegs.SCIHBAUD = 0x07;		
	ScibRegs.SCILBAUD = 0xA0;
	

/*** Configuration for no stop bits, no parity and 8 data bits		*/

	ScibRegs.SCICCR.all = 0x07;
	
// bit 7 		0:		STOP BITS
// bit 6 		0:		PARITY
// bit 5 		0:		PARITY ENABLE
// bit 4 		0:		LOOP BACK ENA
// bit 3 		0:		ADDR/IDLE MODE
// bit 2-0 		111:	SCI CHAR

/*** Transmitter and receiver enabled	*/

	ScibRegs.SCICTL1.all = 0x03;
	
// bit 7 		0:		Reserved
// bit 6 		0:		RX ERR INT
// bit 5 		0:		SW RESET
// bit 4 		0		Reserved
// bit 3 		0:		TXWAKE
// bit 2 		0:		SLEEP
// bit 1 		1:		TXENA, Habilitación del transmisor
// bit 0 		1:		RXENA, Habilitación del receptor

/*** Tx and Rx interruption enabled 	*/

	ScibRegs.SCICTL2.all = 0x0003;
	
// bit 7 		0:		TXRDY
// bit 6 		0:		TX EMPTY
// bit 5-2 		0's:	Reserved
// bit 1 		1:		RX/BK INT
// bit 0 		1:		TX INT
	
/***Emulation mode configuration	*/

	ScibRegs.SCIPRI.all = 0x0010;

// bit 7-5 		0's:	Reserved
// bit 4-3 		10:		Emulation Mode
// bit 2-0 		0's:	Reserved

/*** SCI Reset		*/

	ScibRegs.SCICTL1.bit.SWRESET= 1;
	
/*** SCI interruptions enabled in PIE */

	PieCtrlRegs.PIEIER9.bit.INTx4= 1;	  
	PieCtrlRegs.PIEIER9.bit.INTx3= 1;	                
    IER |= 0x0100;              						

}

/**End of file*/

TI C281x Timer Configuration

Emmanuel January 20, 2011 Coded in C for the TI F28x
**********************************************************************/
#include "DSP281x_Device.h"
/**********************************************************************
* Function: InitTimer()
*
* Description: Initialize the CPU Timer 0 at a rate of 2 Hz
**********************************************************************/
void InitTimer(void)
{	
	//CpuTimer0Regs.TIM.all = 0x0;        //Timer0 Counter Register
	
	CpuTimer0Regs.PRD.all = 0x047868C0;       //Period Register
	
	CpuTimer0Regs.TCR.all = 0x4830;       //Control Register
/*
bit 15		0:		CPU-Timer Interupt Flag
bit 14		1:		Timer Interrupt Enable
bit 13-12   0's:	Reserved
bit 11-10	10:		CPU-Timer Emulation Modes
bit 9-6		0's:	Reserved
bit 5		0:		CPU-Timer Reload bit
bit 4		0:		CPU-Timer Status Bit
bit 3-0		0's:	Reserved
*/
	
	CpuTimer0Regs.TPR.all = 0x0000;       //Prescale Register
	CpuTimer0Regs.TPRH.all = 0x0000;       //Prescale Register High
	
	
	PieCtrlRegs.PIEIER1.bit.INTx7 = 1; 		//Interruption enabled
	IER |= 0x0001;							//PIE group enabled
	
	StartCpuTimer0();//Reset of the Timer
	
}

FFT with down-sampling in time domain for C++

Emmanuel November 10, 20103 comments Coded in C++
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>

#define N 128
#define PI 3.14159265358979

struct COMPLEX
{float real,imag;};

void qfft(struct COMPLEX *X, int n);   //FFT function
int q_log2(int n);
int qflip(int i, int M);     //self developed function like fliplr() in Matlab for bits.

struct COMPLEX w[N/2];       	    //twiddle factors
struct COMPLEX samples[N];          //input signal
float x1[N];                        //output signal
int i;
float s;		           

main(void)
{	
	for (i = 0 ; i<N/2 ; i++)	          // set up twiddle constants in w
    {
      	   w[i].real = cos(2*PI*i/N);  //Re component of twiddle constants
      	   w[i].imag =-sin(2*PI*i/N);  //Im component of twiddle constants
    }
	
/*Generating test signal******************/	
    for (i=0; i<N; i++)
	{
        s=0+i;
		samples[i].real = -1+(s*(2/128));
		samples[i].imag = 0; 		
	}
	printf("%f,%f",samples[50].real,s);
	
/************************************************/	
	system("PAUSE");
	//fft calculation
	qfft(samples,N);
        //magnitude calculation
	x1[i] = sqrt(samples[i].real*samples[i].real
	         + samples[i].imag*samples[i].imag);
	
    printf("El resultado es:\n");
	for (i=0; i<N; i++)
	{
		printf("%f\t%fi\n",samples[i].real,samples[i].imag);
	}
	
    system("PAUSE");
}

void qfft(struct COMPLEX *X, int n) 
{
	int k,l,m,k2,k21,Mk,index;
	struct COMPLEX op1,op2;
	struct COMPLEX X1;
	//nuber of stages
	int M=q_log2(n);
	//to array elements of the input signal
	for (k=0; k<N/2; k++)
	{
        index=qflip(k,M);
        X1=X[k];
        X[k]=X[index];
        X[index]=X1;
        printf("%d\n",index);
        }
	//entering to graph
	//k holds the stage
	for (k=1; k<=M; k++)
	{
		k2=(int)pow(2,k);
		k21=(int)pow(2,k-1);
		Mk=(int)pow(2,M-k);
		//l holds the butterfly
		for (l=1; l <= (n/k2); l++)
		{
			//m holds the upper element of the butterfly
			for (m=1; m<=k21; m++)
			{
				op1=X[(l-1)*k2+m-1];
				op2=X[(l-1)*k2+m+k21-1];
				//Butterfly equations
				//Complex product
				//(a+jb)(c+jd)=ac+j(ad+bc)-bd
				X[(l-1)*k2+m-1].real = op1.real + w[Mk*(m-1)].real*op2.real 
									 - w[Mk*(m-1)].imag*op2.imag ;
				X[(l-1)*k2+m-1].imag = op1.imag + w[Mk*(m-1)].real*op2.imag
									 + w[Mk*(m-1)].imag*op2.real ;
				//su elemento complemento:
				X[(l-1)*k2+m+k21-1].real = op1.real - w[Mk*(m-1)].real*op2.real 
									     + w[Mk*(m-1)].imag*op2.imag ;
				X[(l-1)*k2+m+k21-1].imag = op1.imag - w[Mk*(m-1)].real*op2.imag
									     - w[Mk*(m-1)].imag*op2.real ;
			}//end m
		}//end l
	}//end k
	return;
}//end function

int q_log2(int n)
{
	int res=0;
	while(n>1)
	{
		n=n/2;
		res=res+1;
	}
	return res;
}

/***Implementation of the qflip function*****/

/**************************************************/

FFT with down-sampling in time domain

Emmanuel November 9, 20102 comments Coded in Matlab
clc, clear all, close all
N=128;
x=-1:2/128:1-2/128;
%number of stages
M=q_log2(N);
%to array elements in a proper way for the butterfly
for k=0:N-1;
    x1(k+1)=x(bi2de(fliplr(de2bi(k,M)))+1);
end
%to set up twiddle factors
for k=0:N/2-1;
    W(k+1)=exp(-1i*2*pi*k/N);
end
%entering to graph
%k holds the stage
for k=1:M;
    k2=2^k;
    k21=2^(k-1);
    Mk=2^(M-k);
    %l holds the butterfly
    for l=1:N/k2;
        %l=1:N/pow(2,k)
        %m holds the upper element of the butterfly
        for m=1:k21;
        op1 = x1((l-1)*(k2)+m);
        op2 = x1((l-1)*(k2)+m+k21);
        %butterfly equation
        x1((l-1)*(k2)+m) = op1+W(Mk*(m-1)+1)*op2;
        x1((l-1)*(k2)+m+k21) = op1-W(Mk*(m-1)+1)*op2;
        end
    end
end