#include "afe4300.h"
#include <stdio.h>
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "driver/spi_master.h"
#include "esp_log.h"
// --- AFE4300寄存器地址 ---
#define ADC_DATA_RESULT 0x00
#define ADC_CONTROL_REGISTER 0x01
#define MISC1_REGISTER 0x02
#define MISC2_REGISTER 0x03
#define DEVICE_CONTROL_1 0x09
#define ISW_MATRIX 0x0A
#define VSW_MATRIX 0x0B
#define IQ_MODE_ENABLE 0x0C
#define WEIGHT_SCALE_CONTROL 0x0D
#define BCM_DAC_FREQ 0x0E
#define DEVICE_CONTROL_2 0x0F
#define ADC_CONTROL_REGISTER_2 0x10
#define MISC3_REGISTER 0x1A
static const char* TAG = "AFE4300";
static spi_device_handle_t afe4300_spi;
uint8_t testbuff[12*4];
//const uint16_t isw_mux_reg[] = {0x0408, 0x1020, 0x4080};
//const uint16_t vsense_mux_reg[] = {0x0408, 0x1020, 0x4080};
static esp_err_t spiWrite(uint8_t addr, uint16_t data)
{
uint8_t tx[3] = {addr, (uint8_t)(data >> 8), (uint8_t)data};
spi_transaction_t t = {.length = 24, .tx_buffer = tx};
esp_err_t ret = spi_device_transmit(afe4300_spi, &t);
if (ret != ESP_OK) ESP_LOGE(TAG, "SPI写失败 addr=0x%02X", addr);
return ret;
}
static uint16_t spiRead(uint8_t addr)
{
uint8_t tx[3] = {addr | 0x20, 0, 0};
uint8_t rx[3] = {0};
spi_transaction_t t = {.length = 24, .tx_buffer = tx, .rx_buffer = rx};
esp_err_t ret = spi_device_transmit(afe4300_spi, &t);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "SPI读失败 addr=0x%02X", addr);
return 0xFFFF;
}
return (rx[1] << 8) | rx[2];
}
static void gpio_init(void)
{
gpio_config_t out_cfg = {
.pin_bit_mask = ((1ULL << AFE4300_SCLK_PIN) | (1ULL << AFE4300_MOSI_PIN) | (1ULL << AFE4300_CS_PIN) | (1ULL << AFE4300_RESET_PIN)),
.mode = GPIO_MODE_OUTPUT,
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_DISABLE
};
gpio_config(&out_cfg);
gpio_config_t in_cfg = {
.pin_bit_mask = ((1ULL << AFE4300_MISO_PIN) | (1ULL << AFE4300_RDY_PIN)),
.mode = GPIO_MODE_INPUT,
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_DISABLE,
.intr_type = GPIO_INTR_DISABLE
};
gpio_config(&in_cfg);
gpio_set_level(AFE4300_CS_PIN, 1);
gpio_set_level(AFE4300_RESET_PIN, 1);
}
static esp_err_t spi_init(void)
{
spi_bus_config_t buscfg = {
.mosi_io_num = AFE4300_MOSI_PIN,
.miso_io_num = AFE4300_MISO_PIN,
.sclk_io_num = AFE4300_SCLK_PIN,
.quadwp_io_num = -1,
.quadhd_io_num = -1
};
ESP_ERROR_CHECK(spi_bus_initialize(AFE4300_SPI_HOST, &buscfg, SPI_DMA_DISABLED));
spi_device_interface_config_t devcfg = {
.clock_speed_hz = AFE4300_SPI_FREQ_HZ,
.mode = 1,
.spics_io_num = AFE4300_CS_PIN,
.queue_size = 7,
.cs_ena_pretrans = 1,
.cs_ena_posttrans = 1
};
return spi_bus_add_device(AFE4300_SPI_HOST, &devcfg, &afe4300_spi);
}
void afe4300_reset(void)
{
gpio_set_level(AFE4300_RESET_PIN, 0);
vTaskDelay(pdMS_TO_TICKS(10));
gpio_set_level(AFE4300_RESET_PIN, 1);
vTaskDelay(pdMS_TO_TICKS(100));
spiWrite(MISC1_REGISTER, 0);
spiWrite(MISC2_REGISTER, 0xFFFF);
spiWrite(MISC3_REGISTER, 0x30);
}
static void afe4300_config(void)
{
spiWrite(ADC_CONTROL_REGISTER, 0x4170);
spiWrite(DEVICE_CONTROL_1, 0x6006);
spiWrite(ISW_MATRIX, 0x0408);
spiWrite(VSW_MATRIX, 0x0408);
spiWrite(ADC_CONTROL_REGISTER_2, 0x0063);
spiWrite(WEIGHT_SCALE_CONTROL, 0x0000);
spiWrite(BCM_DAC_FREQ, 0x0020);
spiWrite(DEVICE_CONTROL_2, 0x1800);
spiWrite(IQ_MODE_ENABLE, 0x0800);
ESP_LOGI(TAG, "AFE4300 初始化完成");
}
static void set_frequency(int freq_khz, bool enable_iq)
{
uint16_t freq_val = 0;
uint16_t dev_ctrl2_val = 0;
switch (freq_khz) {
case 8: freq_val = 0x08; dev_ctrl2_val = enable_iq ? 0x2800 : 0; break;
case 16: freq_val = 0x10; dev_ctrl2_val = enable_iq ? 0x2000 : 0; break;
case 32: freq_val = 0x20; dev_ctrl2_val = enable_iq ? 0x1800 : 0; break;
case 64: freq_val = 0x40; dev_ctrl2_val = enable_iq ? 0x1000 : 0; break;
case 128: freq_val = 0x80; dev_ctrl2_val = enable_iq ? 0x0800 : 0; break;
default: freq_val = freq_khz; dev_ctrl2_val = enable_iq ? 0x0000 : 0; break;
}
spiWrite(BCM_DAC_FREQ, freq_val);
spiWrite(DEVICE_CONTROL_2, dev_ctrl2_val);
}
static uint16_t read_average(uint8_t addr, uint8_t count)
{
int sum = 0;
for (uint8_t i = 0; i < count; i++) {
sum += (int16_t)spiRead(addr);
}
return sum / count;
}
static void IQ_read(uint16_t out_channel, uint16_t in_channel, int16_t *result_I, int16_t *result_Q)
{
const uint16_t modes[] = {0x0063, 0x0065};
spiWrite(IQ_MODE_ENABLE, 0x0800);
set_frequency(32, true);
spiWrite(ISW_MATRIX, out_channel);
spiWrite(VSW_MATRIX, in_channel);
for (uint8_t i = 0; i < 2; i++) {
spiWrite(ADC_CONTROL_REGISTER_2, modes[i]);
vTaskDelay(pdMS_TO_TICKS(200));
int data = read_average(ADC_DATA_RESULT, 20);
if (i == 0) *result_I = (int16_t)data;
else *result_Q = (int16_t)data;
}
}
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=0,j=0;
volatile short datai_adc;
volatile short dataq_adc;
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
vTaskDelay(pdMS_TO_TICKS(200));
datai_adc = read_average(0x00, 20);
spiWrite(0x10,0x0065); //ADC_FOR_BCM_CHANNELQ
vTaskDelay(pdMS_TO_TICKS(200));
dataq_adc = read_average(0x00, 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)
{
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_test(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 = (int16_t)((rawdata[i] << 8) + rawdata[i + 1])* 0.0518799;
dataq = (int16_t)((rawdata[i + 2] << 8) + rawdata[i + 3])* 0.0518799;
//ESP_LOGI("AFE4300","#####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);
//ESP_LOGI("AFE4300","#####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;
ESP_LOGI("AFE4300","__linear_regression: 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;
}
}
static void bcm_iq_test_task(void* arg)
{
float caldata[16];
while (1) {
bcm_measurement_iq();
zref_calc_test(testbuff, caldata);
for (uint8_t i = 0; i < 16; i=i+2)
{
ESP_LOGI("AFE4300","__aftercal_iq: Z = %.2f, theta = %.2f\n", caldata[i], caldata[i+1]);
}
vTaskDelay(pdMS_TO_TICKS(2000));
}
}
esp_err_t afe4300_init(void)
{
gpio_init();
esp_err_t ret = spi_init();
if (ret != ESP_OK) {
ESP_LOGE(TAG, "SPI 初始化失败");
return ret;
}
afe4300_reset();
afe4300_config();
return ESP_OK;
}
void afe4300_start_task(void)
{
xTaskCreate(bcm_iq_test_task, "bcm_iq_test", 4096, NULL, 5, NULL);
}
优化
最新发布