📄 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;
float SinTable[FFT_SIZE/2], sin_b[FFT_SIZE], cos_b[FFT_SIZE], sin_b_5[FFT_SIZE], cos_b_5[FFT_SIZE];
Data_In datalist[DATA_CALCULATE_SIZE];
//struct _DATA_AD2DSP DATA_CALCULATE[DATA_CALCULATE_SIZE];/*存采样后的值*/
//TSampleInputs DATA_DSP2CPU;
//#define PER_TEST
#ifdef PER_TEST
float cap[100];
float caap;
int cap_i = 0;
float sum = 0, avg = 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 float *tempAdd;
register float tempA;
register float tempB;
register float tempC,tempD,tempE,tempF;
register float tempSin;
register float tempCos;
float pOut[FFT_SIZE*2];
// 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 = &datalist[index].IA;
#if 1
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 */
#endif
}
*( JDInputsAdd+off_IA_Real) = tempA = pOut[0];
*( JDInputsAdd+off_IA_Imag) = tempB = pOut[1];
//cap[cap_i%100] = *( JDInputsAdd+off_IA) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
*( JDInputsAdd+off_IA) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
linerAdjust(14.46279, *( 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(14.53279, *( 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(14.73351, *( 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;
linerAdjust(14.53279, *( JDInputsAdd+off_I0));
*( 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(33.5597933, *( JDInputsAdd+off_UA));
if (*(JDInputsAdd+off_UA) < 0.7458271)
{
linerAdjust(33.5597933, *( JDInputsAdd+off_UA));
}
else
{
linerAdjust(33.6597933, *( 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(14.53279, *( JDInputsAdd+off_UB));
if (*(JDInputsAdd+off_UB) < 0.7458271)
{
linerAdjust(33.5597933, *( JDInputsAdd+off_UB);
}
else
{
linerAdjust(33.6597933, *( 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)) / 10.0;
//linerAdjust(14.53279, *( JDInputsAdd+off_UC));
if (*(JDInputsAdd+off_UC) < 0.7458271)
{
linerAdjust(33.5597933, *( JDInputsAdd+off_UC));
}
else
{
linerAdjust(33.6597933, *( 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 ; //B c 实部
tempF = (pOut[3] + pOut[5])*0.5; //B C 虚部
*( JDInputsAdd+off_I1_Real) = tempA = (pOut[0] - tempE + tempC)*0.33333333;
*( JDInputsAdd+off_I1_Imag )= tempB = (pOut[1]- tempF + tempD)*0.33333333;
*( JDInputsAdd+off_I1) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
linerAdjust(14.46279, *( JDInputsAdd+off_I1));
*( JDInputsAdd+off_I2_Real) = tempA =( pOut[0] - tempE - tempC)*0.33333333;
*( JDInputsAdd+off_I2_Imag )= tempB = (pOut[1]- tempF - tempD)*0.33333333;
*( JDInputsAdd+off_I2) = (sqrt(tempA * tempA + tempB * tempB)) *D_HALF_SIZE;
linerAdjust(14.46279, *( JDInputsAdd+off_I2));
#ifdef PER_TEST
{
sum +=cap[cap_i%100] = *( JDInputsAdd+off_UA);
avg = sum / (cap_i+1);
cap_i++;
}
#endif
readIndex += READ_INDEX_INCREASE;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -