📄 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 cap1[100];
float cap2[100];
float cap3[100];
int cap_i = 0;
float sum1 = 0, sum2=0, sum3 = 0,avg1 = 0,avg2,avg3;
#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));
}
float yxjtest[100];
float yxjtest1[100];
float yxjtest2[100];
float yxjtest3[100];
float yxjtest4[100];
float yxjtest5[100];
int yxjtestcal = 0;
int diandu_int = 0;
int diandu_float_to_int = 0;
void CALCULATE (void) //Ua,Ia需要5次谐波的采用FFT，其它简化
{
/*************************电度******************************/
static double power_total= 0;
float power1 = 0;
float power2 = 0;
float power3 = 0;
static double diandu = 0 ;
static float diandu_float = 0;
static int diandu_flag = 0;
static long power_temp = 0;
/*--------------------------------------------------------*/
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(IA_linerAdjust, *( JDInputsAdd+off_IA));
/* if ((*( JDInputsAdd+off_IA)>30.5)&& (*( JDInputsAdd+off_IA)<38.0))
{
linerAdjust(1.063, *( JDInputsAdd+off_IA));
}
else if (*( JDInputsAdd+off_IA) >38.1)
{
linerAdjust(1.13, *( 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(IB_linerAdjust, *( JDInputsAdd+off_IB));
/*if ((*( JDInputsAdd+off_IB)>30.5)&& (*( JDInputsAdd+off_IB)<38.0))
{
linerAdjust(1.063, *( JDInputsAdd+off_IB));
}
else if (*( JDInputsAdd+off_IB) >38.1)
{
linerAdjust(1.13, *( 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(IC_linerAdjust, *( JDInputsAdd+off_IC));
/*if ((*( JDInputsAdd+off_IC)>30.5)&& (*( JDInputsAdd+off_IC)<38.0))
{
linerAdjust(1.063, *( JDInputsAdd+off_IC));
}
else if (*( JDInputsAdd+off_IC) >38.1)
{
linerAdjust(1.13, *( 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(I0_linerAdjust, *( 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 ;
#ifdef temppt
linerAdjust(285, *( JDInputsAdd+off_UA));
#else
if (*(JDInputsAdd+off_UA) < 0.7458271)
{
linerAdjust(UA_linerAdjust_I, *( JDInputsAdd+off_UA));
//linerAdjust(10.3448275862, *( JDInputsAdd+off_UA));
}
else
{
linerAdjust(UA_linerAdjust_II, *( JDInputsAdd+off_UA));
}
#endif
*( 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;
#ifdef temppt
linerAdjust(287.3, *( JDInputsAdd+off_UB));
#else
if (*(JDInputsAdd+off_UB) < 0.7458271)
{
linerAdjust(UB_linerAdjust_I, *( JDInputsAdd+off_UB));
}
else
{
linerAdjust(UB_linerAdjust_II, *( JDInputsAdd+off_UB));
}
#endif
*( 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;
#ifdef temppt
linerAdjust(289, *( JDInputsAdd+off_UC));
#else
if (*(JDInputsAdd+off_UC) < 0.7458271)
{
linerAdjust(UC_linerAdjust_I, *( JDInputsAdd+off_UC));
}
else
{
linerAdjust(UC_linerAdjust_II, *( JDInputsAdd+off_UC));
}
#endif
*( 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(I1_linerAdjust, *( 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(I2_linerAdjust, *( JDInputsAdd+off_I2));
#ifdef PER_TEST
{/*
if( (*( JDInputsAdd+off_IA_Phase) - *( JDInputsAdd+off_IC_Phase) ) > 0.00000001)
{
sum +=cap[cap_i%100] = *( JDInputsAdd+off_IA_Phase) - *( JDInputsAdd+off_IC_Phase) ;
avg = sum / (cap_i+1);
cap_i++;
}
else
{
sum +=cap[cap_i%100] = *( JDInputsAdd+off_IA_Phase) - *( JDInputsAdd+off_IC_Phase)+2*PI ;
avg = sum / (cap_i+1);
cap_i++;
}
*/
sum1 +=cap1[cap_i%100] = *( JDInputsAdd+off_UA) ;
avg1 = sum1 / (cap_i+1);
sum2 +=cap2[cap_i%100] = *( JDInputsAdd+off_UB) ;
avg2= sum2 / (cap_i+1);
sum3 +=cap3[cap_i%100] = *( JDInputsAdd+off_UC) ;
avg3 = sum3 / (cap_i+1);
cap_i++;
}
#endif
*( JDInputsAdd+off_IA) *= SystemSetting.CT;
*( JDInputsAdd+off_IB) *= SystemSetting.CT;
*( JDInputsAdd+off_IC) *= SystemSetting.CT;
*( JDInputsAdd+off_I1) *= SystemSetting.CT;
*( JDInputsAdd+off_I2) *= SystemSetting.CT;
readIndex += READ_INDEX_INCREASE;
/******************电流相位貌似超前于电压,需要处理*************/
if(*( JDInputsAdd+off_IA_Phase) > PI)
{
*( JDInputsAdd+off_IA_Phase) -= PI;
}
else
{
*( JDInputsAdd+off_IA_Phase) += PI;
}
if(*( JDInputsAdd+off_IB_Phase) > PI)
{
*( JDInputsAdd+off_IB_Phase) -= PI;
}
else
{
*( JDInputsAdd+off_IB_Phase) += PI;
}
if(*( JDInputsAdd+off_IC_Phase) > PI)
{
*( JDInputsAdd+off_IC_Phase) -= PI;
}
else
{
*( JDInputsAdd+off_IC_Phase) += PI;
}
/*************************功率*******************/
//Pa_temp += (data_in.IA-data_in.VREF) *( data_in.Ua - data_in.VREF);
/*-----------------------------------------------------------*/
//*( JDInputsAdd+off_IA)=10;
//*( JDInputsAdd+off_UA)= 6000;
/*
*( JDInputsAdd+off_IB)=10;
*( JDInputsAdd+off_UB)= 2200;
*( JDInputsAdd+off_IC)=100;
*( JDInputsAdd+off_UC)= 1200;
*/
if(diandu_flag == 0)
{
//power_total += (*( JDInputsAdd+off_IA)**( JDInputsAdd+off_UA)+ *( JDInputsAdd+off_IB)**( JDInputsAdd+off_UB)+ *( JDInputsAdd+off_IC)**( JDInputsAdd+off_UC))*0.4;
if( (power1 = *( JDInputsAdd+off_IA)**( JDInputsAdd+off_UA))<0)
{
power1 = 0;
}
if( (power2 = *( JDInputsAdd+off_IB)**( JDInputsAdd+off_UB))<0)
{
power2 = 0;
}
if( (power2 = *( JDInputsAdd+off_IC)**( JDInputsAdd+off_UC))<0)
{
power2 = 0;
}
power_total += (power1+power2+power3)*0.4;
}
diandu_flag++;
diandu = power_total*0.001;
diandu *= 0.001;
diandu*= 0.2777777778;
diandu_flag %= 100;
diandu_int = diandu;
diandu_float = diandu-diandu_int;
diandu_float_to_int = diandu_float*10;
// yxjtest[1] = cos(3.1415926/3);
yxjtest[yxjtestcal] = *( JDInputsAdd+off_UA_Phase)*57.29577951 ;
NOP();
NOP();
yxjtest1[yxjtestcal] = *( JDInputsAdd+off_UB_Phase)*57.29577951 ;
yxjtest2[yxjtestcal] = *( JDInputsAdd+off_UC_Phase)*57.29577951 ;
yxjtest3[yxjtestcal] = *( JDInputsAdd+off_IA_Phase)*57.29577951 ;
yxjtest4[yxjtestcal] = *( JDInputsAdd+off_IB_Phase)*57.29577951 ;
yxjtest5[yxjtestcal] = *( JDInputsAdd+off_IC_Phase)*57.29577951 ;
yxjtest2[yxjtestcal] = cos( *( JDInputsAdd+off_UA_Phase) - *( JDInputsAdd+off_IA_Phase)) ;
NOP();
yxjtestcal++;
NOP();
yxjtestcal %= 100;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -