📄 fix_dsp.c
字号:
#include <math.h>
#include <string.h>
#include "Fix_DSP.h"
#include "MOTOType.h"
//#include "bspGarfield/inc/hardware_reg.h"
#include "Applications/inlcudes/hardware_reg.h"
int readIndex = READ_INDEX_INCREASE;
int writeIndex = 0;
int SinTable[FFT_SIZE/2], sin_b[FFT_SIZE], cos_b[FFT_SIZE], sin_b_5[FFT_SIZE], cos_b_5[FFT_SIZE];
unsigned long ChannelA;
unsigned long ChannelB;
unsigned long ChannelC;
unsigned long ChannelD;/* 存AD采样的四个通道值 */
struct _DATA_AD2DSP DATA_CALCULATE[DATA_CALCULATE_SIZE];/*存采样后的值*/
TSampleInputs DATA_DSP2CPU;
#ifdef PER_TEST
int cap[100];
int cap_i = 0;
#endif
void InitCalculate (void)
{
int i;
float theta = 2 * PI / FFT_SIZE;
int quartsize = FFT_SIZE / 4;
for (i=0; i<quartsize; i++) /* fill sin table in memory */
{
/*SinTable[1 + (2 * i)] = sin(i * theta) * FIX_POINT + 0.5;
SinTable[2 * i] = cos(i * theta) * FIX_POINT + 0.5;*/
SinTable[1 + (2 * i)] = sin(i * theta);
SinTable[2 * i] = cos(i * theta);
}
for (i=0; i<FFT_SIZE; i++)
{
/* sin_b[i] = sin(i * theta) * FIX_POINT + 0.5;
cos_b[i] = cos(i * theta) * FIX_POINT + 0.5;
sin_b_5[i] = sin(i * theta * 5) * FIX_POINT + 0.5;
cos_b_5[i] = cos(i * theta * 5) * FIX_POINT + 0.5; */
sin_b[i] = sin(i * theta);
cos_b[i] = cos(i * theta);
sin_b_5[i] = sin(i * theta * 5);
cos_b_5[i] = cos(i * theta * 5);
}
memset(DATA_CALCULATE, 1, sizeof(DATA_CALCULATE));
}
void CALCULATE (void) //Ua,Ia需要5次谐波的采用FFT，其它简化
{
register int i, index;
register int *tempAdd;
register float tempA;
register float tempB;
register float tempC,tempD,tempE,tempF;
register float tempSin;
register float tempCos;
int pOut[FFT_SIZE*2];
ReadAD( ) ;
// DATA_CALCULATE = DATA_CALCULATE_ReadAddress;
/* 其他值简化计算 */
tempAdd= pOut; /* 代替原pOut清0 */
for(i = 0; i < FFT_SIZE; i++)
{
*tempAdd = 0;
tempAdd++;
}
// for(i=0;i<64;i++) pOut[i] = 0;
for(i=0; i<FFT_SIZE; i++)
{
index = i + readIndex;
if(index >= DATA_CALCULATE_SIZE) index -= DATA_CALCULATE_SIZE;
tempSin = sin_b[i];
tempCos = cos_b[i];
// tempAdd = &DATA_CALCULATE[index].Ua;
pOut[0] += FIX_MUL((*(tempAdd + IA_Offset)), tempSin, Q11); /* IA */
pOut[1] += FIX_MUL((*(tempAdd + IA_Offset)), tempCos, Q11); /* IA */
pOut[2] += FIX_MUL((*(tempAdd + IB_Offset)), tempSin, Q11); /* IB */
pOut[3] += FIX_MUL((*(tempAdd + IB_Offset)), tempCos, Q11); /* IB */
pOut[4] += FIX_MUL((*(tempAdd + IC_Offset)), tempSin, Q11); /* IC */
pOut[5] += FIX_MUL((*(tempAdd + IC_Offset)), tempCos, Q11); /* IC */
pOut[6] += FIX_MUL((*(tempAdd + I0_Offset)), tempSin, Q11); /* I0 */
pOut[7] += FIX_MUL((*(tempAdd + I0_Offset)), tempCos, Q11); /* I0 */
pOut[8] += FIX_MUL((*(tempAdd + Ua_Offset)), tempSin, Q11); /* Ua */
pOut[9] += FIX_MUL((*(tempAdd + Ua_Offset)), tempCos, Q11); /* Ua */
pOut[10] += FIX_MUL((*(tempAdd + Ub_Offset)), tempSin, Q11); /* Ub */
pOut[11] += FIX_MUL((*(tempAdd + Ub_Offset)), tempCos, Q11); /* Ub */
pOut[12] += FIX_MUL((*(tempAdd + Uc_Offset)), tempSin, Q11); /* Uc */
pOut[13] += FIX_MUL((*(tempAdd + Uc_Offset)), tempCos, Q11); /* Uc */
}
*( JDInputsAdd+off_IA_Real) = tempA = pOut[0];
*( JDInputsAdd+off_IA_Imag) = tempB = pOut[1];
*( JDInputsAdd+off_IA) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
linerAdjust(0.03384524, 0.7749741508, 21.8085234322, -0.0021816135, *( JDInputsAdd+off_IA));
*( JDInputsAdd+off_IA_Phase) = atan(tempB / tempA);
phaseConvert(tempA, tempB, *( JDInputsAdd+off_IA_Phase));
*( JDInputsAdd+off_IB_Real) = tempA = pOut[2];
*( JDInputsAdd+off_IB_Imag) = tempB = pOut[3];
*( JDInputsAdd+off_IB) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
linerAdjust(0.0163960189, 0.7047940106, 21.64926757, -0.00105438392, *( JDInputsAdd+off_IB));
*( JDInputsAdd+off_IB_Phase) = atan(tempB / tempA);
phaseConvert(tempA, tempB, *( JDInputsAdd+off_IB_Phase));
*( JDInputsAdd+off_IC_Real) = tempA = pOut[4];
*( JDInputsAdd+off_IC_Imag )= tempB = pOut[5];
*( JDInputsAdd+off_IC) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
linerAdjust(0.0617891915, 0.7223773071, 21.9106379364, -0.0038924769, *( JDInputsAdd+off_IC));
*( JDInputsAdd+off_IC_Phase) = atan(tempB / tempA);
phaseConvert(tempA, tempB, *( JDInputsAdd+off_IC_Phase));
*( JDInputsAdd+off_I0_Real) = tempA = pOut[6];
*( JDInputsAdd+off_I0_Imag )= tempB = pOut[7];
*( JDInputsAdd+off_I0) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
*( JDInputsAdd+off_UA_Real) = tempA = pOut[8];
*( JDInputsAdd+off_UA_Imag) = tempB = pOut[9];
*( JDInputsAdd+off_UA) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE ;
linerAdjust(0.0045405839, -0.1000368677, 23.2626541159, 0.0193734706, *( JDInputsAdd+off_UA));
*( JDInputsAdd+off_UA_Phase) = atan(tempB / tempA);
phaseConvert(tempA, tempB, *( JDInputsAdd+off_UA_Phase));
*( JDInputsAdd+off_UB_Real) = tempA = pOut[10];
*( JDInputsAdd+off_UB_Imag) = tempB = pOut[11];
*( JDInputsAdd+off_UB )= (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
linerAdjust(-0.0084763785, 0.0264693961, 22.7935839842, -0.1200517272, *( JDInputsAdd+off_UB));
*( JDInputsAdd+off_UB_Phase) = atan(tempB / tempA);
phaseConvert(tempA, tempB, *( JDInputsAdd+off_UB_Phase));
*( JDInputsAdd+off_UC_Real) = tempA = pOut[12];
*( JDInputsAdd+off_UC_Imag )= tempB = pOut[13];
*( JDInputsAdd+off_UC) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
linerAdjust(0.0125296299, -0.1629001930, 23.4579313316, 0.1578952797, *( JDInputsAdd+off_UC));
*( JDInputsAdd+off_UC_Phase) = atan(tempB / tempA);
phaseConvert(tempA, tempB, *( JDInputsAdd+off_UC_Phase));
tempC = (pOut[5] - pOut[3])*0.866025404;
tempD = (pOut[2] - pOut[4])*0.866025404;
tempE = (pOut[2] + pOut[4])*0.5 ;
tempF = (pOut[3] + pOut[5])*0.5;
*( JDInputsAdd+off_I1_Real) = tempA = pOut[0] - tempE + tempC;
*( JDInputsAdd+off_I1_Imag )= tempB = pOut[1]- tempF + tempD;
*( JDInputsAdd+off_I1) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
*( JDInputsAdd+off_I2_Real) = tempA = pOut[0] - tempE - tempC;
*( JDInputsAdd+off_I2_Imag )= tempB = pOut[1]- tempF - tempD;
*( JDInputsAdd+off_I2) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
}
BOOL JudgeDATA( void ) //读写指针判断是否丢数
{
if( (readIndex - writeIndex > READ_INDEX_INCREASE) \
|| ((readIndex - writeIndex) <= (int)(READ_INDEX_INCREASE - FFT_SIZE))) /* 读指针领先写指针超过3或读指针回零并加过而写指针在末端则等待*/
{
if (readIndex >= DATA_CALCULATE_SIZE)
{
readIndex = readIndex - DATA_CALCULATE_SIZE;
}
return FALSE;
}
/* 读指针领先写指针则返回1 */
if (readIndex - writeIndex == READ_INDEX_INCREASE)
{
/* 如果写指针到MidAddress，则减66将其复位 */
if (readIndex >= DATA_CALCULATE_SIZE)
{
readIndex = readIndex - DATA_CALCULATE_SIZE;
}
return (TRUE);
}
else
{
/* 读指针领先写指针少于READ_INDEX_INCREASE则系统出错，返回0 */
if (readIndex - writeIndex < READ_INDEX_INCREASE)
{
readIndex = writeIndex + (READ_INDEX_INCREASE<<1);
return (FALSE);
}
}
}
void ReadAD( void ) /* 根据BUSY进入中断 */
{
// *(RP)PORTE_DATA |= 0x80 ; //pull high
Read7865( ); /* 4路AD数据读出到原始数组*/
//*(RP)PORTE_DATA |= 0x80 ; //pull high
DATA_AD2DSP.U0 = CONVERT(ChannelA) ;
DATA_AD2DSP.I0 = CONVERT(ChannelB) ;
DATA_AD2DSP.Ux = CONVERT(ChannelC) ;
DATA_AD2DSP.Ix = CONVERT(ChannelD) ;
/* FSX0/DX0输出10,SEL01 */
//*(RP)PORTE_DATA &= ~0x80 ; //pull low
DATA_AD2DSP.Ia = CONVERT(ChannelA) ;
DATA_AD2DSP.Ib = CONVERT(ChannelB) ;
DATA_AD2DSP.Ic = CONVERT(ChannelC) ;
DATA_AD2DSP.Ubp = CONVERT(ChannelD) ;
// memcpy(&DATA_CALCULATE[writeIndex].Ua, &DATA_AD2DSP, sizeof(DATA_AD2DSP)); /* 采集16通道同步输出 */
/* if (writeIndex >= DATA_CALCULATE_SIZE)
{
writeIndex = (writeIndex + WRITE_INDEX_INCREASE) - DATA_CALCULATE_SIZE;
}
else
{
writeIndex = (writeIndex + WRITE_INDEX_INCREASE);
}
// *(RP)PORTE_DATA &= ~0x80 ; //pull low */
}
void Read7865( void ) /* 根据BUSY进入中断 */
{
int i = 0;
ChannelA = 1; /* 读取VIN1 */
i=0;
ChannelB = 3123; /* 读取VIN2 */
i=0;
ChannelC = 21; /* 读取VIN3 */
i=0;
ChannelD = 32; /* 读取VIN4 */
i=0;
}
// #if 0
// float CONVERT(unsigned int input) /* 14bitAD转为浮点数 */
/*{
if(input & 0x00002000)
{
input &= 0x00003fff;
input = 0x00003fff - input;
return (0.0 - input*LSB - LSB);
}
else
{
input &= 0x00003fff;
return (input*LSB);
}
} */
void TEST (void) //Ua,Ia需要5次谐波的采用FFT，其它简化
{
register int i ;
for(i=0;i<100000;i++);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -