|
|
发表于 2016-10-26 21:20:50
|
显示全部楼层
- static uint8_t sendInfos[122] = {0};
- void AlgorithmProcessFun(uint8_t* frameBuf, uint16_t bufLen, uint16_t nChirpIndex)
- {
- uint16_t nSamplesPerChip = (DataProc_GetRadarDataLen() - 8) / 4;
- uint8_t* dataBuf = frameBuf + 12;
- if((RadarPara.dataType == DATA_TYPE_FFT) ||
- (RadarPara.dataType == DATA_TYPE_DSRAW))
- {
- for(int j = 0; j < nSamplesPerChip; j ++)
- {
- uint8_t temp = dataBuf[4 * j];
- dataBuf[4 * j] = dataBuf[4 * j + 1];
- dataBuf[4 * j + 1] = temp;
-
- temp = dataBuf[4 * j + 2];
- dataBuf[4 * j + 2] = dataBuf[4 * j + 3];
- dataBuf[4 * j + 3] = temp;
- }
-
- COMPLEX16_T* pData = (COMPLEX16_T*)dataBuf;
-
- if(nChirpIndex==0 && IsFinished == 0)
- {
- IsFinished=1;
- flagFullFrame = 0;
- //maxPower = 0;
- memset(flagfullFrameIndex, 0, sizeof(flagfullFrameIndex));
- flagfullFrameIndex[0] = 1;
- memset(RadarReal, 0, sizeof(RadarReal));
- memset(RadarImag, 0, sizeof(RadarImag));
- for(int j=0;j<10;j++)
- {
- RadarReal[j][nChirpIndex] = pData[j].real;
- RadarImag[j][nChirpIndex] = pData[j].imag;
- }
- }
- else if(nChirpIndex>0 &&nChirpIndex<64 && IsFinished==1)
- {
- flagFullFrame++;
- flagfullFrameIndex[nChirpIndex] = 1;
- for(int j=0;j<10;j++)
- {
- RadarReal[j][nChirpIndex] = pData[j].real;
- RadarImag[j][nChirpIndex] = pData[j].imag;
- }
- memset(RadarSumRPower, 0, sizeof(RadarSumRPower));
- memset(RadarSumRPowerM, 0, sizeof(RadarSumRPowerM));
- memset(RadarBreathReal, 0, sizeof(RadarBreathReal));
- memset(RadarBreathImag, 0, sizeof(RadarBreathImag));
- memset(sendInfos, 0, sizeof(sendInfos));
- }
- uint8_t sum = 0;
- for(int j=0;j<64;j++)
- {
- sum = sum+flagfullFrameIndex[j];
- }
- if(sum==64 && flagFullFrame>=63)
- {
- for(int j=0;j<10;j++)
- {
- int32_t sumReal = 0;
- int32_t sumImag = 0;
- uint32_t sumPower = 0;
- uint32_t sumPower2 = 0;
- for(int i=0;i<64;i++)
- {
- int16_t realv = RadarReal[j][i];
- int16_t imagv = RadarImag[j][i];
- sumReal = sumReal + realv;
- sumImag = sumImag + imagv;
- uint32_t sumAmp = (uint32_t)((int32_t)(realv*realv) + (int32_t)(imagv*imagv));
- sumPower2 = sumPower2 + sumAmp;
- sumPower = sumPower + (uint32_t)sqrt_fast(sumAmp);
- }
- RadarBreathReal[j] = (int16_t)(sumReal/64);
- RadarBreathImag[j] = (int16_t)(sumImag/64);
- RadarSumRPower[j] = (uint32_t)(sumPower/64);
- // Çó·½²î, std
- //Ampliptude[j] = (sumPower2/64)-RadarSumRPower[j]*RadarSumRPower[j];
-
- // fft - speed
- int16_t fft_buffer[128];
- int16_t fft_mag_buffer[64];
- for(int i=0;i<64;i++)
- {
- fft_buffer[2*i] = (int16_t)(RadarReal[j][i] - RadarBreathReal[j]);
- fft_buffer[2*i+1] = (int16_t)(RadarImag[j][i] - RadarBreathImag[j]);
- }
- arm_cfft_q15(&arm_cfft_sR_q15_len64, fft_buffer, 0, 1);
- arm_cmplx_mag_q15(fft_buffer, fft_mag_buffer, 64);
- // ¼ÆËãfft ƽ¾ùÖµ
- int32_t sumMotion = 0;
- for(int i=0;i<64;i++)
- {
- sumMotion = sumMotion + fft_mag_buffer[i];
- }
- RadarSumRPowerM[j] = (uint32_t)(sumMotion/64);
- }
- // send
- sendInfos[0] = 0xFA;
- memcpy(&sendInfos[1], &RadarBreathReal[0], 20); // int16_t -- 2 x uint8_t
- memcpy(&sendInfos[21], &RadarBreathImag[0], 20); // int16_t -- 2 x uint8_t
- memcpy(&sendInfos[41], &RadarSumRPower[0], 40); // uint32_t -- 4 x uint8_t
- memcpy(&sendInfos[81], &RadarSumRPowerM[0], 40); // uint32_t -- 4 x uint8_t
- sendInfos[121] = 0xF1;
- //
- //Delay(40);
- while(!uart_is_Ready()){};
- uart_transmit(sendInfos, 122);
- //
- flagFullFrame = 0;
- memset(flagfullFrameIndex, 0, sizeof(flagfullFrameIndex));
- IsFinished=0;
- }
-
- }
- }
复制代码
|
|