#include "math.h"
#include "afe4300_app.h"
#include "spi.h"
#include "stdio.h"
#include "lora.h"
#include "app_system.h"
#if 1
int16_t KalmanFilter(int16_t inData)
{
static float prevData = 0; //????
static float p = 1, q = 0.001, r = 0.1, kGain = 0; // q????? r?????
p = p + q;
kGain = p / ( p + r ); //???????
inData = prevData + ( kGain * ( inData - prevData ) ); //?????????
p = ( 1 - kGain ) * p; //??????
prevData = inData;
return inData; //?????
}
#endif
#if 0
void kalman_init(kalman_struct *kalman_lcw, float init_x, float init_p)
{
kalman_lcw->x = init_x;//???????,??????????(????)
kalman_lcw->p = init_p;//????????????????
kalman_lcw->A = 1;
kalman_lcw->H = 1;
kalman_lcw->q = 10e-3;//10e-6;//2e2;////predict noise convariance ??(??)???? ????????????????
kalman_lcw->r = 5e2;//10e-5;//??(??)???????????,?????:
}
float kalman_filter(kalman_struct *kalman_lcw, float measure)
{
/* Predict */
kalman_lcw->x = kalman_lcw->A * kalman_lcw->x;
kalman_lcw->p = kalman_lcw->A * kalman_lcw->A * kalman_lcw->p + kalman_lcw->q; /* p(n|n-1)=A^2*p(n-1|n-1)+q */
/* Measurement */
kalman_lcw->gain = kalman_lcw->p * kalman_lcw->H / (kalman_lcw->p * kalman_lcw->H * kalman_lcw->H + kalman_lcw->r);
kalman_lcw->x = kalman_lcw->x + kalman_lcw->gain * (measure - kalman_lcw->H * kalman_lcw->x);
kalman_lcw->p = (1 - kalman_lcw->gain * kalman_lcw->H) * kalman_lcw->p;
return kalman_lcw->x;
}
#endif
void resetAFE4300(void)
{
SPI_AFE4300_RST_LOW;
tos_sleep_ms(10);
SPI_AFE4300_RST_HIGH;
spiWrite(MISC1_REGISTER,0x0000);
spiWrite(MISC2_REGISTER,0xFFFF);
spiWrite(MISC3_REGISTER,0x0030);
}
void initAFE4300(void)
{
spiWrite(ADC_CONTROL_REGISTER,0x4170);
spiWrite(DEVICE_CONTROL_1,0x6004);//,0x6006
spiWrite(ISW_MATRIX,0x0000);//0x0804
spiWrite(VSW_MATRIX,0x0000);//0x0804
spiWrite(IQ_MODE_ENABLE,0x0000);
spiWrite(WEIGHT_SCALE_CONTROL,0x0000);
spiWrite(BCM_DAC_FREQ,0x0010);//0x0010
spiWrite(DEVICE_CONTROL_2,0x0000);
spiWrite(ADC_CONTROL_REGISTER_2,0x0011);//0x0063
spiWrite(MISC1_REGISTER,0x0000);
spiWrite(MISC2_REGISTER,0xFFFF);
spiWrite(MISC3_REGISTER,0x0030);//0x00C0//0x0030
}
void afe4300_config_test(void)
{
spiWrite(ADC_CONTROL_REGISTER,0x4170); //Differential measurement mode, 32 SPS //0x4120 4130: 64SPS
APP_LOG("ADC_CONTROL_REGISTER : %x\n", spiRead(ADC_CONTROL_REGISTER));
spiWrite(DEVICE_CONTROL_1,0x6006); //Power up BCM signal chain
APP_LOG("DEVICE_CONTROL_1 : %x\n", spiRead(DEVICE_CONTROL_1));
spiWrite(ISW_MATRIX,0x0408); //Channels IOUTP1 and IOUTN0 0x0804
APP_LOG("ISW_MATRIX : %x\n", spiRead(ISW_MATRIX));
spiWrite(VSW_MATRIX,0x0408); //Channels VSENSEP1 and VSENSEN0
APP_LOG("VSW_MATRIX : %x\n", spiRead(VSW_MATRIX));
spiWrite(ADC_CONTROL_REGISTER_2,0x0063); //ADC selects output of BCM-I output
APP_LOG("ADC_CONTROL_REGISTER_2 : %x\n", spiRead(ADC_CONTROL_REGISTER_2));
spiWrite(WEIGHT_SCALE_CONTROL,0x0000); //Gain = 1 DAC Offset = 0
APP_LOG("WEIGHT_SCALE_CONTROL : %x\n", spiRead(WEIGHT_SCALE_CONTROL));
spiWrite(BCM_DAC_FREQ,0x0020);//0x0010
APP_LOG("BCM_DAC_FREQ : %x\n", spiRead(BCM_DAC_FREQ));
spiWrite(DEVICE_CONTROL_2,0x1800);//32khz
APP_LOG("DEVICE_CONTROL_2 : %x\n", spiRead(DEVICE_CONTROL_2));
spiWrite(IQ_MODE_ENABLE,0x0800);
APP_LOG("IQ_MODE_ENABLE : %x\n", spiRead(IQ_MODE_ENABLE));
}
void afe4300_config(void)
{
spiWrite(ADC_CONTROL_REGISTER,0x4170); //Differential measurement mode, 32 SPS //0x4120 4130: 64SPS
spiWrite(DEVICE_CONTROL_1,0x6006); //Power up BCM signal chain
spiWrite(ISW_MATRIX,0x0408); //Channels IOUTP1 and IOUTN0 0x0804
spiWrite(VSW_MATRIX,0x0408); //Channels VSENSEP1 and VSENSEN0
spiWrite(ADC_CONTROL_REGISTER_2,0x0063); //ADC selects output of BCM-I output
spiWrite(WEIGHT_SCALE_CONTROL,0x0000); //Gain = 1 DAC Offset = 0
spiWrite(BCM_DAC_FREQ,0x0020);//0x0010
spiWrite(DEVICE_CONTROL_2,0x1800);//32khz
spiWrite(IQ_MODE_ENABLE,0x0800);
APP_LOG("afe4300 initialization\n\r");
}
//Supported IQ frequencies are 8,16,32,64 and 128 kHz
void set_frequency(int frequency_khz, bool set_IQ_demod)
{
if (frequency_khz == 8) //DAC to 8 kHz
{
spiWrite(BCM_DAC_FREQ, 0x08);
if (set_IQ_demod)
spiWrite(DEVICE_CONTROL_2, 0x2800);
}
else if (frequency_khz == 16)
{
spiWrite(BCM_DAC_FREQ, 0x10);
if (set_IQ_demod)
spiWrite(DEVICE_CONTROL_2, 0x2000);
}
else if (frequency_khz == 32)
{
spiWrite(BCM_DAC_FREQ, 0x20);
if (set_IQ_demod)
spiWrite(DEVICE_CONTROL_2, 0x1800);
}
else if (frequency_khz == 64)
{
spiWrite(BCM_DAC_FREQ, 0x40);
if (set_IQ_demod)
spiWrite(DEVICE_CONTROL_2, 0x1000);
}
else if (frequency_khz == 128)
{
spiWrite(BCM_DAC_FREQ, 0x80);
if (set_IQ_demod)
spiWrite(DEVICE_CONTROL_2, 0x0800);//dfsdfds
}
else
{
if (set_IQ_demod)
spiWrite(DEVICE_CONTROL_2, 0x0000);
spiWrite(BCM_DAC_FREQ, frequency_khz);
}
}
uint16_t read_last(void)
{
return spiRead(ADC_DATA_RESULT);//*(1.7/32768.0);
}
void wait_for_stability(uint16_t cnt) {
uint16_t i = 0;
//APP_LOG("last:\n");
do {
//APP_LOG("%x\t", read_last());
i++;
} while (i < cnt);
//APP_LOG("\n");
}
void IQ_read(uint16_t outchannel, uint16_t inchannel, int16_t *result_I, int16_t *result_Q)
{
const uint16_t modes[] = {0x0063, 0x0065};
volatile short datai_adc=0; int datai=0;
spiWrite(IQ_MODE_ENABLE,0x0800);
set_frequency(32, true);
spiWrite(ISW_MATRIX, outchannel);
spiWrite(VSW_MATRIX, inchannel);
for (uint8_t i = 0; i < 2; i++) {
spiWrite(ADC_CONTROL_REGISTER_2, modes[i]);
//wait_for_stability(1000);
//tos_sleep_ms(500);
HAL_Delay(200);
//volatile uint16_t raw_data = spiRead(ADC_DATA_RESULT);
datai=0;
for(uint8_t k=0;k<20;k++)
{
datai_adc = spiRead(0x00);
datai += datai_adc;
}
datai /= 20;
if (i == 0)
{
*result_I = (int16_t)datai; // I分量
}
else
{
*result_Q = (int16_t)datai; // Q分量
}
}
//spiWrite(0x01,0x4180);
//spiWrite(0x09,0x600C);
}
void FWR_read(uint16_t outchannel, uint16_t inchannel, int16_t *result_I, uint8_t frequency, uint16_t delay)
{
set_frequency(frequency, false);
spiWrite(IQ_MODE_ENABLE, 0x00); //Select FWR Mode
spiWrite(ADC_CONTROL_REGISTER_2, 0x63); //ADCREF to VREF / ADC connected to BCM
spiWrite(ISW_MATRIX, inchannel); //Select VSENSE RN1-RP0
spiWrite(VSW_MATRIX, outchannel); //Select IOUT RN1-RP0
//wait_for_stability(10, 10, quiet);
tos_sleep_ms(delay);
*result_I = spiRead(ADC_DATA_RESULT);//*(1.7/32768.0);
}
const uint16_t ch_cal[] = {0x0201,0x0202,0x0101,0x0102};//RP1RN0,RP1RN1,RP0RN0,RP0RN1
//const uint16_t isw_mux_reg[] = {OUTPUT_DIFF_CHANNEL_13, OUTPUT_DIFF_CHANNEL_13, OUTPUT_DIFF_CHANNEL_10, OUTPUT_DIFF_CHANNEL_40, OUTPUT_DIFF_CHANNEL_15, OUTPUT_DIFF_CHANNEL_45, OUTPUT_DIFF_CHANNEL_05, OUTPUT_DIFF_CHANNEL_24};
//const uint16_t vsense_mux_reg[] = {INPUT_DIFF_CHANNEL_24, INPUT_DIFF_CHANNEL_13, INPUT_DIFF_CHANNEL_10, INPUT_DIFF_CHANNEL_40, INPUT_DIFF_CHANNEL_15, INPUT_DIFF_CHANNEL_45, INPUT_DIFF_CHANNEL_05, INPUT_DIFF_CHANNEL_24 };
const uint16_t isw_mux_reg[] = {OUTPUT_DIFF_CHANNEL_01, OUTPUT_DIFF_CHANNEL_23, OUTPUT_DIFF_CHANNEL_45, OUTPUT_DIFF_CHANNEL_40, OUTPUT_DIFF_CHANNEL_15, OUTPUT_DIFF_CHANNEL_45, OUTPUT_DIFF_CHANNEL_05, OUTPUT_DIFF_CHANNEL_24};
const uint16_t vsense_mux_reg[] = {INPUT_DIFF_CHANNEL_01, INPUT_DIFF_CHANNEL_23, INPUT_DIFF_CHANNEL_45, INPUT_DIFF_CHANNEL_40, INPUT_DIFF_CHANNEL_15, INPUT_DIFF_CHANNEL_45, INPUT_DIFF_CHANNEL_05, INPUT_DIFF_CHANNEL_24 };
uint8_t testbuff[12*4];
void bcm_ref_iq(void)
{
uint8_t i,j=0;
volatile short datai_adc;
volatile short dataq_adc;
volatile short datai_adc1;
volatile short dataq_adc1;
int datai = 0;
int dataq = 0;
uint8_t k=0;
{
spiWrite(IQ_MODE_ENABLE,0x0800);
set_frequency(32, true);
spiWrite(0x0A,ch_cal[i]); //OUTPUT
spiWrite(0x0B,ch_cal[i]); //INPUT
spiWrite(0x10,0x0063); //ADC_FOR_BCM_CHANNELI
HAL_Delay(200);
spiRead(0x00);
}
for(i=0;i<4;i++)
{
spiWrite(0x0A,ch_cal[i]); //OUTPUT
spiWrite(0x0B,ch_cal[i]); //INPUT
spiWrite(0x10,0x0063); //ADC_FOR_BCM_CHANNELI
HAL_Delay(200);
//datai_adc = spiRead(0x00);
datai=0;
for(uint8_t k=0;k<20;k++)
{
datai_adc1 = spiRead(0x00);
datai += datai_adc1;
}
datai_adc = datai / 20;
spiWrite(0x10,0x0065); //ADC_FOR_BCM_CHANNELQ
HAL_Delay(200);
//dataq_adc = spiRead(0x00);
dataq=0;
for(uint8_t k=0;k<20;k++)
{
dataq_adc1 = spiRead(0x00);
dataq += dataq_adc1;
}
dataq_adc = dataq / 20;
testbuff[j++] = datai_adc >> 8;
testbuff[j++] = datai_adc & 0xff;
testbuff[j++] = dataq_adc >> 8;
testbuff[j++] = dataq_adc & 0xff;
}
}
void bcm_measurement_iq(void)
{
//resetAFE4300();
//afe4300_config();
int16_t result_I, result_Q;
uint8_t j=16;
bcm_ref_iq();
for (uint8_t i=0; i<sizeof(isw_mux_reg)/sizeof(isw_mux_reg[0]); i++)
{
result_I = 0;
result_Q = 0;
IQ_read(isw_mux_reg[i], vsense_mux_reg[i], &result_I, &result_Q);
testbuff[j++] = result_I >> 8;
testbuff[j++] = result_I & 0xff;
testbuff[j++] = result_Q >> 8;
testbuff[j++] = result_Q & 0xff;
}
}
void zref_calc(uint8_t* rawdata, float* caldata)
{
uint8_t i,j;
static const float realy[4]={99.1,196.4,699.1,950.2};//{99,195,696,947};//{99.1,196.4,699.1,950.2};//{98.7,196.6,698.1,949.2};//x[4]={29,55,188,255};
float datai, dataq, theta_tmp[12], z_tmp[12];
float lxx, lxy, sum_x, sum_y, avg_x, avg_y, avg_theta;
float aref, bref, theta;
lxx = lxy = sum_x = sum_y = avg_theta = aref = bref = theta = 0.0;
for (i=0, j=0; i < 48; i=i+4, j++)
{
datai = (float)((rawdata[i] << 8) + rawdata[i + 1])* 0.0518799;
dataq = (float)((rawdata[i + 2] << 8) + rawdata[i + 3])* 0.0518799;
//APP_LOG("#####cal: datai =%.2f, dataq =%.2f\n",datai,dataq);
theta_tmp[j] = 57.2957795f * atan2f(dataq, datai); // 使用atan2避免除零错误
z_tmp[j] = sqrtf(datai * datai + dataq * dataq); //* (1.7f / 32768.0f);
//APP_LOG("#####cal: ampli[%d] =%.2f, theta[%d] =%.2f\n", j,z_tmp[j],j, theta_tmp[j]);
}
for (i = 0; i < 4; i++)
{
sum_x += z_tmp[i];
sum_y += realy[i];
avg_theta += theta_tmp[i];
}
avg_x = sum_x / 4.0;
avg_y = sum_y / 4.0;
theta = avg_theta / 4.0;
for (i = 0; i != 4; i++)
{
lxx += (z_tmp[i] - avg_x) * (z_tmp[i] - avg_x);
lxy += (z_tmp[i] - avg_x) * (realy[i] - avg_y);
}
bref = lxy / lxx;
aref = avg_y - bref * avg_x;
APP_LOG("#####cal: Y=%.2fx + %.2f, theta avg =%.2f\n", bref, aref, theta);
for (i=4,j=0; i < 12; i++, j=j+2)
{
caldata[j] = z_tmp[i] * bref + aref;
caldata[j+1] = theta_tmp[i] - theta;
}
}
void bcm_calc_iq(void)
{
float caldata[16];
zref_calc(testbuff, caldata);
for (uint8_t i = 0; i < 16; i=i+2)
{
APP_LOG("#####cal: test amp = %.2f, theta avg =%.2f\n", caldata[i], caldata[i+1]);
}
}
//const uint16_t ch_cal[4] = {0x0201,0x0202,0x0101,0x0102};//RP1RN0,RP1RN1,RP0RN0,RP0RN1
volatile float x[4]={0,0,0,0};//{29,55,188,255};
volatile float angle[4]={0,0,0,0};
static const float y[4]={99.1,196.4,699.1,950.2};
static float aref = 0,bref = 0,theta = 0;
void zref_calc1(void)
{
// int n = sizeof(x) / sizeof(x[0]);
int i;
float lxx, lxy, sum_x, sum_y, avg_x, avg_y;
aref = bref = lxx = lxy = sum_x = sum_y = 0.0;
for (i = 0; i < 4; i++)
{
sum_x += x[i];
sum_y += y[i];
}
avg_x = sum_x / 4.0;
avg_y = sum_y / 4.0;
for (i = 0; i != 4; i++)
{
lxx += (x[i] - avg_x)*(x[i] - avg_x);
lxy += (x[i] - avg_x)*(y[i] - avg_y);
}
bref = lxy / lxx;
aref = avg_y - bref*avg_x;
APP_LOG("cal: Y=%.2fx + %.2f\n",bref,aref);
}
void thetaref_calc(void)
{
float tmp = 0;
for (int i = 0; i < 4; i++)
{
tmp += angle[i];
}
theta = tmp / 4.0;
APP_LOG("#####cal: theta avg =%.2f\n",theta);
}
void bcm_config_measurement_ref_iq(void)
{
uint8_t i;
short datai_adc;
short dataq_adc;
float datai = 0;
float dataq = 0;
for(i=0;i<4;i++)
{
angle[i] = 0;
x[i] = 0;
}
for(i=0;i<4;i++)
{
spiWrite(0x0A,ch_cal[i]); //OUTPUT
spiWrite(0x0B,ch_cal[i]); //INPUT
spiWrite(0x10,0x0063); //ADC_FOR_BCM_CHANNELI
HAL_Delay(200);
datai_adc = spiRead(0x00);
datai = datai_adc * 0.051889;
spiWrite(0x10,0x0065); //ADC_FOR_BCM_CHANNELQ
HAL_Delay(200);
dataq_adc = spiRead(0x00);
dataq = dataq_adc * 0.051889;
angle[i] = 57.3 * atan(dataq / datai);
x[i] = sqrt(datai * datai + dataq * dataq);
}
for(i=0;i<4;i++)
APP_LOG("iq reference: root=%.2f angle=%.2f\n",x[i], angle[i]);
zref_calc1();
thetaref_calc();
}
可是我用STM32WL052测得的数据就很正常啊 #####cal: Y=1.47x + -8.10, theta avg =26.07
#####cal: test amp = 88.08, theta avg =0.93
#####cal: test amp = 188.53, theta avg =-0.32
本文通过一个具体的Java代码示例，详细解析了按位运算符的使用方法及其运算过程，包括取反、按位与等操作，并展示了如何将十六进制转换为二进制进行按位运算。
扫一扫
6058
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
