rudism
/
pinebuds
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
pinebuds/platform/drivers/bt/best2300p/iqcorrect.c

986 lines
30 KiB
C
Raw Blame History

#ifdef RTOS
#include <math.h>
#include <stdio.h>
#include "hal_dma.h"
#include "hal_timer.h"
#include "hal_trace.h"
#include "heap_api.h"
#include "string.h"
#include "bt_drv.h"
#include "cmsis_os.h"
#include "hal_btdump.h"
#include "iqcorrect.h"
#include "nvrecord.h"
#include "nvrecord_env.h"
#include "nvrecord_dev.h"
#ifdef TX_IQ_CAL
#define LOG_MODULE HAL_TRACE_MODULE_APP
#define NTbl (1024)
#define fftSize 1024
#define bitshift 10
#define pi (3.1415926535898)
#define min(a,b) (((a) < (b)) ? (a) : (b))
#define READ_REG(b,a) *(volatile uint32_t *)(b+a)
#define WRITE_REG(v,b,a) *(volatile uint32_t *)(b+a) = v
#define BUF_SIZE (1024)
#define MAX_COUNT 5
volatile int iqimb_dma_status = 0;
//short M0data[BUF_SIZE];
#define MED_MEM_POOL_SIZE (20*1024)
static uint8_t *g_medMemPool = NULL;
extern void *rt_malloc(unsigned int size);
extern void rt_free(void *rmem);
typedef struct ComplexInt_
{
int re;
int im;
} ComplexInt;
typedef struct Complexflt_
{
float re;
float im;
} ComplexFlt;
typedef struct ComplexShort_
{
short re;
short im;
} ComplexShort;
#ifdef DCCalib
static void Tblgen(ComplexShort*w0, ComplexShort*w1, ComplexShort*w2,int len)
{
for (int i=0; i<len; i++)
{
w0[i].re = (short)(cos(2*pi*i/(2048.0/32))*32767);
w0[i].im = (short)(sin(2*pi*i/(2048.0/32))*32767);
w1[i].re = (short)(cos(2*pi*i/(2048.0/31))*32767);
w1[i].im = (short)(sin(2*pi*i/(2048.0/31))*32767);
w2[i].re = (short)(cos(2*pi*i/(2048.0/33))*32767);
w2[i].im = (short)(sin(2*pi*i/(2048.0/33))*32767);
}
}
#endif
static void Tblgen_iq(ComplexShort*w0, ComplexShort*w1, ComplexShort*w2,int len)
{
for (int i=0; i<len; i++)
{
w0[i].re = (short)(cos(2*pi*i/(2048.0/64))*32767);
w0[i].im = (short)(sin(2*pi*i/(2048.0/64))*32767);
w1[i].re = (short)(cos(2*pi*i/(2048.0/63))*32767);
w1[i].im = (short)(sin(2*pi*i/(2048.0/63))*32767);
w2[i].re = (short)(cos(2*pi*i/(2048.0/65))*32767);
w2[i].im = (short)(sin(2*pi*i/(2048.0/65))*32767);
}
}
/*
typedef struct IQMismatchPreprocessState_
{
short *M0data;
ComplexShort *Table0;
ComplexShort *Table1;
ComplexShort *Table2;
} IQMismatchPreprocessState;
IQMismatchPreprocessState st_g;
short st_M0data[fftSize];
short st_Table0[2*NTbl];
short st_Table1[2*NTbl];
short st_Table2[2*NTbl];
IQMismatchPreprocessState *st;
IQMismatchPreprocessState *IQMismatchPreprocessState_init(int Cohcnt)
{
st_g.M0data = st_M0data;
st_g.Table0= st_Table0;
st_g.Table1= st_Table1;
st_g.Table2= st_Table2;
return &st_g;
}
*/
typedef struct IQMismatchPreprocessState_
{
short *M0data;
ComplexShort *Table0;
ComplexShort *Table1;
ComplexShort *Table2;
} IQMismatchPreprocessState;
IQMismatchPreprocessState *IQMismatchPreprocessState_init(int fft_size)
{
BT_DRV_TRACE(0,"malloc ini");
IQMismatchPreprocessState *st = (IQMismatchPreprocessState *)med_calloc(1, sizeof(IQMismatchPreprocessState));
st->M0data = (short*)med_calloc(fft_size, sizeof(short));
st->Table0 = (ComplexShort*)med_calloc(fft_size, sizeof(ComplexShort));
st->Table1 = (ComplexShort*)med_calloc(fft_size, sizeof(ComplexShort));
st->Table2 = (ComplexShort*)med_calloc(fft_size, sizeof(ComplexShort));
BT_DRV_TRACE(5,"st:%p, st->M0data:%p, st->Table0:%p, st->Table1:%p, st->Table2:%p", st, st->M0data, st->Table0, st->Table1, st->Table2);
BT_DRV_TRACE(0,"malloc ok");
return st;
}
int32_t IQMismatchPreprocessState_destroy(IQMismatchPreprocessState *st)
{
BT_DRV_TRACE(5,"st:%p, st->M0data:%p, st->Table0:%p, st->Table1:%p, st->Table2:%p", st, st->M0data, st->Table0, st->Table1, st->Table2);
med_free(st->M0data);
med_free(st->Table0);
med_free(st->Table1);
med_free(st->Table2);
med_free(st);
return 0;
}
int IQMismatchParameterCalc_ex(short *M0data,
short Cohcnt, ComplexShort *Table0, ComplexShort *Table1, ComplexShort *Table2,int fftsize)
{
int i,j,k;
float M0 = 0;
//ComplexInt tmp0;
ComplexFlt tmp0;
ComplexFlt tmp1;
ComplexFlt tmp2;
for (j = 0; j < Cohcnt; j++)
{
tmp0.re = 0.0f;
tmp0.im = 0.0f;
tmp1.re = 0.0f;
tmp1.im = 0.0f;
tmp2.re = 0.0f;
tmp2.im = 0.0f;
k = 0;
for (i=0; i<fftsize; i++)
{
if(k>=fftsize)
k = 0;
tmp0.re = tmp0.re + (((int)M0data[i] *(int)Table0[k].re) >> 15) ;
tmp0.im = tmp0.im + (((int)M0data[i] * (int)Table0[k].im) >> 15) ;
tmp1.re = tmp1.re + (((int)M0data[i] *(int)Table1[k].re) >> 15) ;
tmp1.im = tmp1.im + (((int)M0data[i] * (int)Table1[k].im) >> 15) ;
tmp2.re = tmp2.re + (((int)M0data[i] *(int)Table2[k].re) >> 15) ;
tmp2.im = tmp2.im + (((int)M0data[i] * (int)Table2[k].im) >> 15) ;
k = k + 1;
}
tmp0.re = tmp0.re/ fftsize;// >> bitshift;
tmp0.im = tmp0.im/ fftsize;// >>bitshift;
tmp1.re = tmp1.re/ fftsize;// >> bitshift;
tmp1.im = tmp1.im/ fftsize;// >>bitshift;
tmp2.re = tmp2.re/ fftsize;// >> bitshift;
tmp2.im = tmp2.im/ fftsize;// >>bitshift;
M0 = M0 + tmp0.re*tmp0.re + tmp0.im*tmp0.im;
M0 = M0 + tmp1.re*tmp1.re + tmp1.im*tmp1.im;
M0 = M0 + tmp2.re*tmp2.re + tmp2.im*tmp2.im;
}
return (int)(M0/Cohcnt);
}
void caculate_energy_main_test(IQMismatchPreprocessState *st,int* Energy,int* Energy1,int fftsize)
{
short Cohcnt = 1;
*Energy1 = IQMismatchParameterCalc_ex(st->M0data, Cohcnt, st->Table0, st->Table1, st->Table2,fftsize);
}
static struct HAL_DMA_DESC_T iqimb_dma_desc[1];
static void iqimb_dma_dout_handler(uint32_t remains, uint32_t error, struct HAL_DMA_DESC_T *lli)
{
hal_audma_free_chan(0);
hal_btdump_disable();
iqimb_dma_status = 0;
return;
}
int bt_iqimb_dma_enable ( short * dma_dst_data, uint16_t size)
{
struct HAL_DMA_CH_CFG_T iqimb_dma_cfg;
// BT_DRV_TRACE(0,"bt_iqimb_dma_enable :");
iqimb_dma_status = 1;
memset(&iqimb_dma_cfg, 0, sizeof(iqimb_dma_cfg));
//iqimb_dma_cfg.ch = hal_audma_get_chan(HAL_AUDMA_DSD_RX,HAL_DMA_HIGH_PRIO);
iqimb_dma_cfg.ch = hal_audma_get_chan(HAL_AUDMA_BTDUMP,HAL_DMA_HIGH_PRIO);
iqimb_dma_cfg.dst_bsize = HAL_DMA_BSIZE_16;
iqimb_dma_cfg.dst_periph = 0; //useless
iqimb_dma_cfg.dst_width = HAL_DMA_WIDTH_WORD;
iqimb_dma_cfg.handler = (HAL_DMA_IRQ_HANDLER_T)iqimb_dma_dout_handler;
iqimb_dma_cfg.src = 0; // useless
iqimb_dma_cfg.src_bsize = HAL_DMA_BSIZE_4;
//iqimb_dma_cfg.src_periph = HAL_AUDMA_DSD_RX;
iqimb_dma_cfg.src_periph = HAL_AUDMA_BTDUMP;
iqimb_dma_cfg.src_tsize = size;//1600; //1600*2/26=123us
iqimb_dma_cfg.src_width = HAL_DMA_WIDTH_WORD;
//iqimb_dma_cfg.src_width = HAL_DMA_WIDTH_HALFWORD;
iqimb_dma_cfg.try_burst = 1;
iqimb_dma_cfg.type = HAL_DMA_FLOW_P2M_DMA;
iqimb_dma_cfg.dst = (uint32_t)(dma_dst_data);
hal_audma_init_desc(&iqimb_dma_desc[0], &iqimb_dma_cfg, 0, 1);
hal_audma_sg_start(&iqimb_dma_desc[0], &iqimb_dma_cfg);
//configed after mismatch parameter done, or apb clock muxed.
//wait
for(volatile int i=0; i<5000; i++);
hal_btdump_enable();
return 1;
}
int bt_iqimb_ini ()
{
uint32_t val;
BT_DRV_TRACE(0,"bt_iqimb_ini :");
//config gpadc 26m
val = READ_REG(0xd0350208,0x0);
val &= 0xffefffff; //bit20
val |= (1<<18); //reg_gclk_en(BTM_CLK_26RXNFMI)
val &= 0xffbfffff; //bit22
val |= (1<<20); //reg_gclk_en(BTM_CLK_26RXDUMP)
val &= 0xfdffffff; //bit25
val |= (1<<25); //reg_gclk_en(BTM_CLK_26RXADC)
WRITE_REG(val,0xd0350208,0x0);
BT_DRV_TRACE(1,"0xd0350208 : 0x%08x",val);
val = READ_REG(0xd0350228,0x0);
val &= 0xffefffff; //bit20
val |= (1<<18); //reg_gclk_mode(BTM_CLK_26RXNFMI)
val &= 0xffbfffff; //bit22
val |= (1<<20); //reg_gclk_mode(BTM_CLK_26RXDUMP)
val &= 0xfdffffff; //bit25
val |= (1<<25); //reg_gclk_mode(BTM_CLK_26RXADC)
WRITE_REG(val,0xd0350228,0x0);
BT_DRV_TRACE(1,"0xd0350228 : 0x%08x",val);
//for rx mux to gpadc
val = READ_REG(0xd0350218,0x0);
val &= 0xfffffff0; //bit3:0 1110 reg_diagcntl(3 downto 0)
val |= (0xe<<0);
val &= 0xfffffbff; //bit10 1 reg_diagcntl(10)
val |= (0x1<<10);
WRITE_REG(val,0xd0350218,0x0);
BT_DRV_TRACE(1,"0xd0350218 : 0x%08x",val);
val = READ_REG(0xd0330024,0x0);
val &= 0xfffffff7; //bit3:0 1110 reg_diagcntl(3 downto 0)
val |= (0x1<<15);
WRITE_REG(val,0xd0330024,0x0);
BT_DRV_TRACE(1,"0xd0330024 : 0x%08x",val);
val = READ_REG(0xd0330034,0x0);
val &= 0x03ffffff; //bit3:0 1110 reg_diagcntl(3 downto 0)
val |= (0x1b<<26);
WRITE_REG(val,0xd0330034,0x0);
BT_DRV_TRACE(1,"0xd0330034 : 0x%08x",val);
WRITE_REG(0x002eb948,0xd0350364,0x0);
val = READ_REG(0xd0330038,0x0);
val |= (0x1<<24);
WRITE_REG(val,0xd0330038,0x0);
WRITE_REG(0,0xc000033c,0x0);
return 1;
}
void check_mem_data(void* data, int len)
{
short* share_mem = (short*)data;
BT_DRV_TRACE(3,"check_mem_data :share_mem= %p, 0x%x, 0x%x",share_mem,share_mem[0],share_mem[1]);
int16_t i =0;
int16_t m =0;
int16_t remain = len;
for(m=0; m<32; m++)
{
for(i=0; i<32; i++)
{
if (remain >16)
{
DUMP16("%04x ",share_mem,16);
share_mem +=16;
remain -= 16;
}
else
{
DUMP16("%04x ",share_mem,remain);
remain =0;
return;
}
}
// BT_DRV_TRACE(0,"\n");
//BT_DRV_TRACE(1,"addr :0x%08x\n",share_mem);
hal_sys_timer_delay(MS_TO_TICKS(200));
}
}
int bt_Txdc_cal_set(int ch_num, int dc_add)
{
uint32_t val;
uint32_t tmp = (uint32_t)dc_add;
//sel apb clock
val = READ_REG(0xd0350348,0x0);
if(ch_num==0)
{
val &= 0xfffffc00; //bit31 1 int_en_mismatch
val |= tmp & 0x3ff;
}
else
{
// BT_DRV_TRACE(1,"bt_Txdc_cal_set, dcadd : 0x%08x",tmp);
val &= 0xfc00ffff; //bit31 1 int_en_mismatch
val |= (tmp & 0x3ff) <<16;
}
WRITE_REG(val,0xd0350348,0x0);
// BT_DRV_TRACE(1,"bt_Txdc_cal_set, 0xd0350348 : 0x%08x",val);
return 1;
}
//g/p mismatch base addr 0xd0310000
//dc_i: 0xd0350348 9:0
//dc_q: 0xd0350348 25:16
int bt_iqimb_add_mismatch(int ch_num, int gain_mis, int phase_mis, int dc_i, int dc_q, uint32_t addr)
{
uint32_t val;
//sel apb clock
val = READ_REG(0xd0350348,0x0);
val &= 0x7fffffff; //bit31 1 int_en_mismatch
val |= (0x0<<31);
WRITE_REG(val,0xd0350348,0x0);
val = (phase_mis << 16) | (gain_mis & 0x0000ffff);
WRITE_REG(val,addr,ch_num*4);
//tval = READ_REG(addr,0x0);
//BT_DRV_TRACE(1,"bt_iqimb_add_mismatch, iq : 0x%08x",tval);
WRITE_REG(0x400000,0xd0350220,0);
WRITE_REG(0x400000,0xd0350224,0);
// bt_Txdc_cal_set(0,dc_i);
//bt_Txdc_cal_set(1,dc_q);
/*
val = READ_REG(0xd0350348,0x0);
val &= 0xfffffc00; //bit9:0
val |= dc_i;
val &= 0xfc00ffff; //bit25:16
val |= (dc_q<<16);
WRITE_REG(val,0xd0350348,0x0);
*/
//sel 26m clock
val = READ_REG(0xd0350348,0x0);
val &= 0x7fffffff; //bit31 1 int_en_mismatch
val |= (0x1<<31);
WRITE_REG(val,0xd0350348,0x0);
return 1;
}
void DC_correction(IQMismatchPreprocessState *st,int *dc_i_r,int *dc_q_r,int fftsize)
{
uint8_t k;
int Energy,Energy1,tmp;
int dc_iters = 2;
int dc_i;
int dc_q;
int dc_step = 4;
int dc_i_base;
int dc_q_base;
int P0,PIplus,PIneg,PQplus,PQneg,PIPQ;
int CI,CQ,tmp_D;
dc_i_base =0;
dc_q_base = 0;
for(k=0; k<dc_iters; k++)
{
dc_i = dc_i_base;
dc_q = dc_q_base;
bt_Txdc_cal_set(0,dc_i);
bt_Txdc_cal_set(1,dc_q);
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
P0 = Energy1;
dc_i = dc_i_base + dc_step;
dc_q = dc_q_base;
bt_Txdc_cal_set(0,dc_i);
bt_Txdc_cal_set(1,dc_q);
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
PIplus = Energy1;
dc_i = dc_i_base - dc_step;
dc_q = dc_q_base;
bt_Txdc_cal_set(0,dc_i);
bt_Txdc_cal_set(1,dc_q);
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
PIneg = Energy1;
dc_i = dc_i_base;
dc_q = dc_q_base + dc_step;
bt_Txdc_cal_set(0,dc_i);
bt_Txdc_cal_set(1,dc_q);
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
PQplus = Energy1;
dc_i = dc_i_base;
dc_q = dc_q_base - dc_step;
bt_Txdc_cal_set(0,dc_i);
bt_Txdc_cal_set(1,dc_q);
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
PQneg = Energy1;
dc_i = dc_i_base + dc_step;
dc_q = dc_q_base + dc_step;
bt_Txdc_cal_set(0,dc_i);
bt_Txdc_cal_set(1,dc_q);
bt_iqimb_dma_enable(st->M0data,(BUF_SIZE/2));
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
PIPQ = Energy1;
tmp = 2*PIplus*PIplus + 2*PQplus*PQplus + 2*PIPQ*PIPQ;
tmp = tmp - 6*P0*P0;
tmp = tmp + 4*P0*(PIneg+PQneg+PIPQ);
tmp = tmp + 2*(PIplus*(PQplus - PQneg)-PIneg*(PQplus+PQneg));
tmp_D = tmp - 4*PIPQ*(PIplus+PQplus);
tmp = P0*(2*(PIplus - PIneg)+PQplus -PQneg);
tmp = tmp - PIPQ*(PQneg-PQplus)+PIneg*(PQplus+PQneg);
tmp = tmp + PQplus*(PQneg-PQplus-2*PIplus);
CI = -dc_step*tmp;
tmp = P0*(2*(PQplus - PQneg)+PIplus-PIneg);
tmp = tmp - PIPQ*(PIneg-PIplus)+PQneg*(PIplus+PIneg);
tmp = tmp + PIplus*(PIneg-PIplus-2*PQplus);
CQ = -dc_step*tmp;
dc_i_base = dc_i_base + CI/tmp_D;
dc_q_base = dc_q_base + CQ/tmp_D;
//dc_step = dc_step/2;
}
*dc_i_r = dc_i_base;
*dc_q_r = dc_q_base;
}
int IQ_GAIN_Mismatch_Correction(IQMismatchPreprocessState *st,int phase_mis_base,int fftsize,uint32_t addr)
{
uint8_t k = 0;
int phase_mis_tmp =0;
int gain_mis_tmp =0;
int tmp = 0;
int energy_ret0_last = 1048576;
int energy_ret0 = 0;
int energy_ret1 = 0;
int energy_ret2 = 0;
int Energy,Energy1 ;
int iters = 4;
int gainstep =8;
int gain_mis_base = 0;
phase_mis_tmp = phase_mis_base;
energy_ret0_last = 1048576;
for(k=0; k<iters+2; k++)
{
gain_mis_tmp = gain_mis_base;
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr);
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
energy_ret0 = Energy1;
if(energy_ret0<50)
break;
if(k>0)
{
if(energy_ret0_last < energy_ret0)
{
gain_mis_base = gain_mis_base + tmp;
gain_mis_tmp = gain_mis_base;
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr);
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
energy_ret0 = Energy1;
}
}
//BT_DRV_TRACE(1,"IQ gain correct energy_ret0 = %d",energy_ret0);
gainstep = 4;///////////////////
gain_mis_tmp = gain_mis_base + gainstep;
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr);
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
energy_ret2 = Energy1;
gain_mis_tmp = gain_mis_base - gainstep;
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr);
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
energy_ret1 = Energy1;
tmp = energy_ret2 -2*energy_ret0 + energy_ret1;
//BT_DRV_TRACE(1,"IQ gain correct energy_ret2 -2*energy_ret0 + energy_ret1 = %d",tmp);
if(tmp>0)
{
tmp = (energy_ret2-energy_ret1)*gainstep/tmp/2;
tmp = min(tmp,4*gainstep);
gainstep = gainstep/2;
}
else
{
tmp = 0;
iters = iters+1;
}
if(iters>8)
break;
if((gain_mis_base - tmp>60)||(gain_mis_base - tmp<-60))
tmp = 0;
//BT_DRV_TRACE(2,"IQ gain correct gain_mis = %d ,gain_adj = %d",gain_mis_base,tmp);
gain_mis_base = gain_mis_base - tmp;
energy_ret0_last = energy_ret0;
}
return gain_mis_base;
}
int IQ_Phase_Mismatch_Correction(IQMismatchPreprocessState *st,int gain_mis_base,int fftsize,uint32_t addr)
{
uint8_t k = 0;
int phase_mis_tmp =0;
int gain_mis_tmp =0;
int tmp = 0;
int energy_ret0_last = 1048576;
int energy_ret0 = 0;
int energy_ret1 = 0;
int energy_ret2 = 0;
int Energy,Energy1 ;
int iters = 4;
int phasestep =4;
int phase_mis_base = 0;
tmp = 0;
energy_ret0_last = 1048576;
gain_mis_tmp = gain_mis_base;
for(k=0; k<iters; k++)
{
phase_mis_tmp = phase_mis_base;
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr); //no mismatch
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
energy_ret0 = Energy1;
if(k>0)
{
if(energy_ret0_last < energy_ret0)
{
phase_mis_base = phase_mis_base + tmp;
phase_mis_tmp = phase_mis_base;
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr); //no mismatch
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
energy_ret0 = Energy1;
}
}
if(energy_ret0<50)
break;
// BT_DRV_TRACE(1,"IQ phase correct energy_ret0 = %d",energy_ret0);
phasestep = 4;//////////////////////////
phase_mis_tmp = phase_mis_base + phasestep;
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr); //no mismatch
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
energy_ret2 = Energy1;
// BT_DRV_TRACE(1,"IQ phase correct energy_ret2 = %d",energy_ret2);
phase_mis_tmp = phase_mis_base - phasestep;
bt_iqimb_add_mismatch(0,gain_mis_tmp,phase_mis_tmp,0,0,addr); //no mismatch
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
energy_ret1 = Energy1;
//BT_DRV_TRACE(1,"IQ phase correct energy_ret1 = %d",energy_ret1);
tmp = energy_ret2 -2*energy_ret0 + energy_ret1;
// BT_DRV_TRACE(1,"IQ phase correct energy_ret2 -2*energy_ret0 + energy_ret1 = %d",tmp);
if(tmp>0)
{
tmp = (energy_ret2-energy_ret1)*phasestep/tmp/2;
tmp = min(tmp,4*phasestep);
phasestep = phasestep/2;
}
else
{
tmp = 0;
iters = iters + 1;
}
if(iters>8)
break;
if((phase_mis_base - tmp>60)||(phase_mis_base - tmp<-60))
tmp = 0;
// BT_DRV_TRACE(2,"IQ phase correct phase_mis = %d ,phase_adj = %d",phase_mis_base,tmp);
phase_mis_base = phase_mis_base - tmp;
energy_ret0_last = energy_ret0;
}
return phase_mis_base;
}
static BT_IQ_CALIBRATION_CONFIG_T config;
static void bt_update_local_iq_calibration_val(uint32_t freq_range, uint16_t gain_cal_val, uint16_t phase_cal_val)
{
config.validityMagicNum = BT_IQ_VALID_MAGIC_NUM;
config.gain_cal_val[freq_range] = gain_cal_val;
config.phase_cal_val[freq_range] = phase_cal_val;
}
// do QA calibration and get the calibration value
static POSSIBLY_UNUSED void btIqCalibration(void)
{
btdrv_tx_iq_cal();
BT_DRV_TRACE(0,"Acquired calibration value:");
for (uint32_t range = 0;range < BT_FREQENCY_RANGE_NUM;range++)
{
BT_DRV_TRACE(3,"%d: 0x%x - 0x%x", range,
config.gain_cal_val[range],
config.phase_cal_val[range]);
}
}
// get IQ calibration from NV
static bool bt_get_iq_calibration_val_from_nv(BT_IQ_CALIBRATION_CONFIG_T* pConfig)
{
if (nv_record_get_extension_entry_ptr() &&
(BT_IQ_VALID_MAGIC_NUM ==
nv_record_get_extension_entry_ptr()->btIqCalConfig.validityMagicNum))
{
*pConfig = nv_record_get_extension_entry_ptr()->btIqCalConfig;
return true;
}
else
{
return false;
}
}
void bt_iq_calibration_setup(void)
{
if (bt_get_iq_calibration_val_from_nv(&config))
{
BT_DRV_TRACE(0,"Calibration value in NV:");
for (uint32_t range = 0;range < BT_FREQENCY_RANGE_NUM;range++)
{
BT_DRV_TRACE(3,"%d: 0x%x - 0x%x", range,
config.gain_cal_val[range],
config.phase_cal_val[range]);
}
uint32_t addr = 0xd0310000;
int chs = 27;
for (uint32_t range = 0;range < 3;range++)
{
if (0 == range)
{
addr = 0xd0310034;
chs= 27;
addr = addr-13*4;
}
for (uint32_t ch=0;ch < chs;ch++)
{
bt_iqimb_add_mismatch(0,
config.gain_cal_val[range],
config.phase_cal_val[range],
0,0,addr);
addr = addr +4;
}
}
}
else
{
btIqCalibration();
uint32_t lock = nv_record_pre_write_operation();
nv_record_get_extension_entry_ptr()->btIqCalConfig = config;
nv_record_post_write_operation(lock);
nv_record_extension_update();
}
}
void bt_IQ_DC_Mismatch_Correction_Release()
{
int phase_mis_base;
int gain_mis_base;
int Energy,Energy1;
uint32_t time_start = hal_sys_timer_get();
int fftsize;
uint32_t addr = 0xd0310000;
int chs = 27;
IQMismatchPreprocessState *st;
syspool_init();
syspool_get_buff(&g_medMemPool, MED_MEM_POOL_SIZE);
med_heap_init(&g_medMemPool[0], MED_MEM_POOL_SIZE);
//config gpadc 26m
READ_REG(0xd0310000,0x0);
#ifdef DCCalib
int dc_i_base,dc_q_base;
fftsize = 1024;
st = IQMismatchPreprocessState_init(fftsize);
Tblgen(st->Table0,st->Table1,st->Table2,fftsize);
DC_correction(st,&dc_i_base,&dc_q_base,fftsize);
bt_Txdc_cal_set(0,dc_i_base); //set cal DC I
bt_Txdc_cal_set(1,dc_q_base);
//val = READ_REG(0xd0350348,0x0);
//BT_DRV_TRACE(1,"0xd0350348 = 0x%08x",val);
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
// BT_DRV_TRACE(3,"dc cal done!!! dc_i = %d ,dc_q = %d,Energy1 = %d",dc_i_base,dc_q_base,Energy1);
if(Energy1>200)
{
DC_correction(st,&dc_i_base,&dc_q_base,fftsize);
bt_Txdc_cal_set(0,dc_i_base); //set cal DC I
bt_Txdc_cal_set(1,dc_q_base);
//val = READ_REG(0xd0350348,0x0);
//BT_DRV_TRACE(1,"0xd0350348 = 0x%08x",val);
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
}
if(Energy1>200)
{
DC_correction(st,&dc_i_base,&dc_q_base,fftsize);
bt_Txdc_cal_set(0,dc_i_base); //set cal DC I
bt_Txdc_cal_set(1,dc_q_base);
//val = READ_REG(0xd0350348,0x0);
//BT_DRV_TRACE(1,"0xd0350348 = 0x%08x",val);
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
}
#endif
// BT_DRV_TRACE(1,"use time: %d ticks", (hal_sys_timer_get()-time_start));
// BT_DRV_TRACE(3,"dc cal done!!! dc_i = %d ,dc_q = %d,Energy1 = %d",dc_i_base,dc_q_base,Energy1);
fftsize = 1024;
st = IQMismatchPreprocessState_init(fftsize);
Tblgen_iq(st->Table0,st->Table1,st->Table2,fftsize);
BT_DRV_TRACE(1,"use time: %d ms", __TICKS_TO_MS(hal_sys_timer_get()-time_start));
for (int k = 0; k<3; k++)
{
if(k==0)
{
WRITE_REG(0x0,0xD02201E4,0x0);
WRITE_REG(0x000A000D,0xD02201E4,0x0);
//osDelay(1000);
addr = 0xd0310034;
}
else if(k==1)
{
WRITE_REG(0x0,0xD02201E4,0x0);
WRITE_REG(0x000A0027,0xD02201E4,0x0);
addr = 0xd031009c;
}
else
{
WRITE_REG(0x0,0xD02201E4,0x0);
WRITE_REG(0x000A0041,0xD02201E4,0x0);
addr = 0xd0310104;
}
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,0,fftsize,addr);
phase_mis_base = IQ_Phase_Mismatch_Correction(st,gain_mis_base,fftsize,addr);
bt_iqimb_add_mismatch(0,gain_mis_base,phase_mis_base,0,0,addr); //no mismatch
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
// BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr,phase_mis_base,gain_mis_base,Energy1);
if(Energy1>500)
{
if(ABS(gain_mis_base)>ABS(phase_mis_base))
{
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,0,fftsize,addr);
phase_mis_base = IQ_Phase_Mismatch_Correction(st,gain_mis_base,fftsize,addr);
}
else
{
phase_mis_base = IQ_Phase_Mismatch_Correction(st,0,fftsize,addr);
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,phase_mis_base,fftsize,addr);
}
bt_iqimb_add_mismatch(0,gain_mis_base,phase_mis_base,0,0,addr); //no mismatch
// BT_DRV_TRACE(1,"use time: %d ticks", (hal_sys_timer_get()-time_start));
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
// BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr,phase_mis_base,gain_mis_base,Energy1);
}
if(Energy1>500)
{
if(ABS(gain_mis_base)>ABS(phase_mis_base))
{
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,0,fftsize,addr);
phase_mis_base = IQ_Phase_Mismatch_Correction(st,gain_mis_base,fftsize,addr);
}
else
{
phase_mis_base = IQ_Phase_Mismatch_Correction(st,0,fftsize,addr);
gain_mis_base = IQ_GAIN_Mismatch_Correction(st,phase_mis_base,fftsize,addr);
}
// BT_DRV_TRACE(1,"use time: %d ticks", (hal_sys_timer_get()-time_start));
bt_iqimb_dma_enable(st->M0data,fftsize);
while(1)
{
if(iqimb_dma_status==0)
break;
}
caculate_energy_main_test(st,&Energy,&Energy1,fftsize);
// BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr,phase_mis_base,gain_mis_base,Energy1);
}
if(k==0)
{
chs= 27;
addr = addr-13*4;
}
else if(k==1)
{
chs = 26;
addr = addr - 12*4;
}
else
{
chs = 26;
addr = addr -12*4;
}
bt_update_local_iq_calibration_val(k, gain_mis_base, phase_mis_base);
for (int ch=0; ch<chs; ch++)
{
bt_iqimb_add_mismatch(0,gain_mis_base,phase_mis_base,0,0,addr); //no mismatch
// osDelay(1);
// BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr,phase_mis_base,gain_mis_base,Energy1);
addr = addr +4;
}
BT_DRV_TRACE(1,"use time: %d ms", __TICKS_TO_MS(hal_sys_timer_get()-time_start));
//BT_DRV_TRACE(4,"IQ phase correct addr = 0x%08x,phase_mis = %d ,gain_mis = %d,Energy = %d",addr-4,phase_mis_base,gain_mis_base,Energy1);
}
}
int bt_iqimb_test_ex (int mismatch_type)
{
bt_IQ_DC_Mismatch_Correction_Release();
return 1;
}
#endif//TX_IQ_CAL
#endif
Reference in New Issue View Git Blame Copy Permalink