jens
290fd5f5e0
git-svn-id: http://moon:8086/svn/software/trunk/libsrc/radio@1 b431acfa-c32f-4a4a-93f1-934dc6c82436
119 lines
2.5 KiB
C
Executable File
119 lines
2.5 KiB
C
Executable File
// --------------------------------------------------------------
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include "nco.h"
|
|
|
|
// --------------------------------------------------------------
|
|
// NCO
|
|
// Scalar version
|
|
// --------------------------------------------------------------
|
|
void NCO_Init(nco_t *pObj, radio_float_t omega, radio_float_t phi)
|
|
{
|
|
pObj->omega = omega;
|
|
pObj->phi = phi;
|
|
memset(&pObj->state,0,sizeof(cpx_t));
|
|
}
|
|
|
|
void NCO_Free(nco_t *pObj)
|
|
{
|
|
(void)pObj;
|
|
}
|
|
|
|
void NCO_Reinit(nco_t *pObj, radio_float_t omega, radio_float_t phi)
|
|
{
|
|
NCO_Free(pObj);
|
|
NCO_Init(pObj, omega, phi);
|
|
}
|
|
|
|
void NCO_SetOmega(nco_t *pObj, radio_float_t omega)
|
|
{
|
|
pObj->omega = omega;
|
|
}
|
|
|
|
void NCO_SetPhi(nco_t *pObj, radio_float_t phi)
|
|
{
|
|
pObj->phi = phi;
|
|
}
|
|
|
|
radio_float_t NCO_GetOmega(nco_t *pObj)
|
|
{
|
|
return pObj->omega;
|
|
}
|
|
|
|
radio_float_t NCO_GetPhi(nco_t *pObj)
|
|
{
|
|
return pObj->phi;
|
|
}
|
|
|
|
void NCO_Process(nco_t *pObj, radio_float_t dOmega, radio_float_t dPhi)
|
|
{
|
|
pObj->phi += (pObj->omega + dOmega);
|
|
|
|
if (pObj->phi < -(radio_float_t)0.5)
|
|
pObj->phi += (radio_float_t)1.0;
|
|
|
|
if (pObj->phi >= +(radio_float_t)0.5)
|
|
pObj->phi -= (radio_float_t)1.0;
|
|
|
|
pObj->state.real = +(radio_float_t)cos(2.0*PI*pObj->phi + dPhi);
|
|
pObj->state.imag = -(radio_float_t)sin(2.0*PI*pObj->phi + dPhi);
|
|
}
|
|
|
|
cpx_t NCO_MixRealComplexS(nco_t *pObj, radio_float_t src, radio_float_t gain)
|
|
{
|
|
cpx_t dst;
|
|
|
|
dst.real = gain*src;
|
|
dst.imag = 0;
|
|
|
|
return CpxMulS(pObj->state, dst);
|
|
}
|
|
|
|
radio_float_t NCO_MixComplexRealS(nco_t *pObj, cpx_t src, cpx_t gain)
|
|
{
|
|
return gain.real*pObj->state.real*src.real + gain.imag*pObj->state.imag*src.imag;
|
|
}
|
|
|
|
cpx_t NCO_MixComplexS(nco_t *pObj, cpx_t src, cpx_t gain)
|
|
{
|
|
return CpxMulS(pObj->state, CpxScaleComplexS(src, gain));
|
|
}
|
|
|
|
void NCO_MixRealComplexV(nco_t *pObj, cpx_t *pDst, radio_float_t *pSrc, radio_float_t gain, uint32_t len)
|
|
{
|
|
|
|
while (len--)
|
|
{
|
|
|
|
*(pDst++) = NCO_MixRealComplexS(pObj, *(pSrc++), gain);
|
|
|
|
// NCO update
|
|
NCO_Process(pObj, 0, 0);
|
|
}
|
|
}
|
|
|
|
void NCO_MixComplexRealV(nco_t *pObj, radio_float_t *pDst, cpx_t *pSrc, cpx_t gain, uint32_t len)
|
|
{
|
|
while (len--)
|
|
{
|
|
|
|
*(pDst++) = NCO_MixComplexRealS(pObj, *(pSrc++), gain);
|
|
|
|
// NCO update
|
|
NCO_Process(pObj, 0, 0);
|
|
}
|
|
}
|
|
|
|
void NCO_MixComplexV(nco_t *pObj, cpx_t *pSrcDst, cpx_t gain, uint32_t len)
|
|
{
|
|
while (len--)
|
|
{
|
|
*(pSrcDst++) = NCO_MixComplexS(pObj, *(pSrcDst++), gain);
|
|
|
|
// NCO update
|
|
NCO_Process(pObj, 0, 0);
|
|
}
|
|
}
|
|
|
|
// --------------------------------------------------------------
|