10+20=1020

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace ConsoleApplication1
{
    class Program
    {
        static void Main(string[] args)
        {
            int i1 = 10;
            int i2 = 20;

            Console.WriteLine(i1 + "+" + i2 + "=" + i1 + i2); // 10+20=1020

            Console.ReadKey();// 按一个键继续执行
        }
    }
}


clc; clear; close all; Pn = 1;%噪声功率 SNR = 10; Ps = 10.^(SNR/10);%信号功率 JNR = 15; Pj = 10.^(JNR/10);%信号功率 ISR = 10*log10(Pj / Ps); %% 生成有用信号 Fs=1000000; N =1000000; t=(0:N-1)/Fs; f=(0:N-1)*Fs/N; mA=0.3; f1=30; s1=mA*sin(2*pi*f1*t); mB=0.3; fs=9960; mf=16; a0=0;a1=0.5*pi;a2=pi;a4=1.5*pi; s2=mB*cos(2*pi*fs*t-mf*cos(2*pi*f1*t+a1)); mC=0.1; fm=(t>=0&t<8.1).*0+(t>=8.1&t<8.4).*1+(t>=8.4&t<8.7).*0+(t>=8.7&t<8.8).*1+(t>=8.8&t<8.9).*0+(t>=8.9&t<9.2).*1+(t>=9.2&t<9.3).*0+(t>=9.3&t<9.6).*1+(t>=9.6&t<9.7).*0+(t>=9.7&t<10).*1 ... +(t>=10&t<18.1).*0+(t>=18.1&t<18.4).*1+(t>=18.4&t<18.7).*0+(t>=18.7&t<18.8).*1+(t>=18.8&t<18.9).*0+(t>=18.9&t<19.2).*1+(t>=19.2&t<19.3).*0+(t>=19.3&t<19.6).*1+(t>=19.6&t<19.7).*0+(t>=19.7&t<20).*1 ... +(t>=20&t<28.1).*0+(t>=28.1&t<28.4).*1+(t>=28.4&t<28.7).*0+(t>=28.7&t<28.8).*1+(t>=28.8&t<28.9).*0+(t>=28.9&t<29.2).*1+(t>=29.2&t<29.3).*0+(t>=29.3&t<29.6).*1+(t>=29.6&t<29.7).*0+(t>=29.7&t<30).*1 ... +(t>=30&t<38.1).*0+(t>=38.1&t<38.4).*1+(t>=38.4&t<38.7).*0+(t>=38.7&t<38.8).*1+(t>=38.8&t<38.9).*0+(t>=38.9&t<39.2).*1+(t>=39.2&t<39.3).*0+(t>=39.3&t<39.6).*1+(t>=39.6&t<39.7).*0+(t>=39.7&t<40).*1 ... +(t>=40&t<48.1).*0+(t>=48.1&t<48.4).*1+(t>=48.4&t<48.7).*0+(t>=48.7&t<48.8).*1+(t>=48.8&t<48.9).*0+(t>=48.9&t<49.2).*1+(t>=49.2&t<49.3).*0+(t>=49.3&t<49.6).*1+(t>=49.6&t<49.7).*0+(t>=49.7&t<50).*1 ; g=cos(2*pi*1020*t);%1020hz音频信号 s3=mC*g.*fm; u_t=1+s1+s2+s3; fc=100000; VOR_re=u_t.*cos(2*pi*fc*t); VOR_im=u_t.*sin(2*pi*fc*t); VOR=VOR_re+1i.*VOR_im; average_power_VOR = sum(abs(VOR).^2) / length(VOR); scaling_factor = sqrt(Ps / average_power_VOR);% 计算比例系数 VOR = VOR * scaling_factor; 写一段代码对我的VOR 信号利用过零点检测求载波频率
03-26
#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); } 优化
最新发布
07-10
#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 += 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 }; 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 = (float)((rawdata[i] << 8) + rawdata[i + 1])* 0.0518799; dataq = (float)((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); } 测试得到I (462) AFE4300: AFE4300 初始化完成 I (462) main_task: Returned from app_main() I (3262) AFE4300: __linear_regression: Y = -3.56x + 12072.35, theta(avg) = 84.73  I (3262) AFE4300: __aftercal_iq: Z = 72.75, theta = 4.30  I (3262) AFE4300: __aftercal_iq: Z = 208.02, theta = 3.31 其中 Y = -3.56x + 12072.35明显不对,12072.35太大了
07-10
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值