/**
  ******************************************************************************
  * @file    lt_fdc2214.c
  ******************************************************************************
  * @attention
  *	FDC2214的SD引脚，拉高关机，拉低开机。
  *	
  ******************************************************************************
  */
#include "lt_fdc2214.h"
#include "stdlib.h"
#include "math.h"
#include "main.h"
#include "queue.h"


uint8_t CHx_FIN_SEL[4];
double fREFx[4];
double FRE[4];
uint32_t ch0,ch1,ch2,ch3;

/*!
 *  @brief      IIC写FDC2214
 *  @param      Slve_Addr     器件地址
 *  @param      reg           寄存器
 *  @param      data          数据
 *  @since      v1.0
 *  Sample usage:       FDC2214_Write16(FDC2214_Addr, FDC2214_CONFIG, 0xFFFF);
 */
void FDC2214_Write16(SoftI2C_HandleTypeDef *iic, uint8_t Slve_Addr, uint8_t reg, uint16_t data)
{
    uint8_t dat;
    FDC2214_IIC_Start(iic);

    FDC2214_IIC_Send_Byte(iic, Slve_Addr << 1);
    FDC2214_IIC_Wait_Ack(iic);

    FDC2214_IIC_Send_Byte(iic, reg);
    FDC2214_IIC_Wait_Ack(iic);

    dat=(data >> 8);
    FDC2214_IIC_Send_Byte(iic, dat);
    FDC2214_IIC_Wait_Ack(iic);

    dat=data & 0xFF;
    FDC2214_IIC_Send_Byte(iic, dat);
    FDC2214_IIC_Wait_Ack(iic);

    FDC2214_IIC_Stop(iic);
}

/*!
 *  @brief      IIC读FDC2214
 *  @param      Slve_Addr     器件地址
 *  @param      reg           寄存器
 *  @return     寄存器值
 *  @since      v1.0
 *  Sample usage:       FDC2214_Read16(FDC2214_Addr, FDC2214_CONFIG);
 */
uint16_t FDC2214_Read16(SoftI2C_HandleTypeDef *iic, uint8_t Slve_Addr, uint8_t reg)
{
    uint16_t temp;

    FDC2214_IIC_Start(iic);
    FDC2214_IIC_Send_Byte(iic, Slve_Addr << 1);
    FDC2214_IIC_Wait_Ack(iic);
    FDC2214_IIC_Send_Byte(iic, reg);
    FDC2214_IIC_Wait_Ack(iic);
    FDC2214_IIC_Start(iic);
    FDC2214_IIC_Send_Byte(iic,((Slve_Addr << 1) | 0x01));
    FDC2214_IIC_Wait_Ack(iic);
    temp = (uint16_t)(FDC2214_IIC_Read_Byte(iic,1) << 8);
    temp |= FDC2214_IIC_Read_Byte(iic,0);
    FDC2214_IIC_Stop(iic);
    return temp;
}

/*!
 *  @brief      读取FDC2214一个通道的转换值
 *  @param      channel     通道
 *  @param      *data       读取的数据
 *  @return     0：失败；1：成功
 *  @since      v1.0
 *  Sample usage:       FDC2214_GetChannelData(FDC2214_Channel_0, &CH0_DATA);
 */
uint8_t FDC2214_GetChannelData(SoftI2C_HandleTypeDef *iic, FDC2214_channel_t channel, uint32_t *data)
{
    int16_t timeout = 100;
    uint8_t dateReg_H;
    uint8_t dateReg_L;
    uint8_t bitUnreadConv;
    uint16_t status;
    uint32_t dataRead;
    switch(channel)
    {
    case FDC2214_Channel_0:
        dateReg_H = FDC2214_DATA_CH0;
        dateReg_L = FDC2214_DATA_LSB_CH0;
        bitUnreadConv = 0x0008;
        break;

    case FDC2214_Channel_1:
        dateReg_H = FDC2214_DATA_CH1;
        dateReg_L = FDC2214_DATA_LSB_CH1;
        bitUnreadConv = 0x0004;
        break;

    case FDC2214_Channel_2:
        dateReg_H = FDC2214_DATA_CH2;
        dateReg_L = FDC2214_DATA_LSB_CH2;
        bitUnreadConv = 0x0002;
        break;

    case FDC2214_Channel_3:
        dateReg_H = FDC2214_DATA_CH3;
        dateReg_L = FDC2214_DATA_LSB_CH3;
        bitUnreadConv = 0x0001;
        break;
    }
    status = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_STATUS);//读取状态寄存器的值
    while(timeout && !(status & bitUnreadConv))//检查是否有未读数据
    {
        status = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_STATUS);
        timeout--;
    }
    if(timeout)
    {
        dataRead = (uint32_t)(FDC2214_Read16(iic, FDC2214_Addr, dateReg_H) << 16);
        dataRead |= FDC2214_Read16(iic, FDC2214_Addr, dateReg_L);
        *data = dataRead;
        return 1;
    }
    else
    {
        //超时
        return 0;
    }
}

/*!
 *  @brief      设置FDC2214一个通道的转换时间，转换时间(tCx)=(CHx_RCOUNT?16) / fREFx。
 *  @param      channel     通道
 *  @param      rcount      CHx_RCOUNT
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetRcount(FDC2214_Channel_0, 0x0FFF);
 */
void FDC2214_SetRcount(SoftI2C_HandleTypeDef *iic, FDC2214_channel_t channel, uint16_t rcount)
{
    if(rcount <= 0x00FF) return;

    switch(channel)
    {
    case FDC2214_Channel_0:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_RCOUNT_CH0, rcount);
        break;

    case FDC2214_Channel_1:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_RCOUNT_CH1, rcount);
        break;

    case FDC2214_Channel_2:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_RCOUNT_CH2, rcount);
        break;

    case FDC2214_Channel_3:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_RCOUNT_CH3, rcount);
        break;
    }
}

/*!
 *  @brief      设置FDC2214一个通道的建立时间，使LC传感器可以在通道x开始转换之前稳定下来，建立时间(tSx) = (CHx_SETTLECOUNT?16) ÷ fREFx。
                CHx_SETTLECOUNT = 0x00, 0x01时(tSx) = 32 ÷ fREFx。
 *  @param      channel     通道
 *  @param      count       CHx_SETTLECOUNT
 *  @since      v1.0
 *  @note       CHx_SETTLECOUNT > Vpk × fREFx × C × π^2 / (32 × IDRIVEX)
 *  Sample usage:       FDC2214_SetSettleCount(FDC2214_Channel_0, 0x00FF);
 */
void FDC2214_SetSettleCount(SoftI2C_HandleTypeDef *iic, FDC2214_channel_t channel, uint16_t count)
{
    switch(channel)
    {
    case FDC2214_Channel_0:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_SETTLECOUNT_CH0, count);
        break;

    case FDC2214_Channel_1:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_SETTLECOUNT_CH1, count);
        break;

    case FDC2214_Channel_2:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_SETTLECOUNT_CH2, count);
        break;

    case FDC2214_Channel_3:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_SETTLECOUNT_CH3, count);
        break;
    }
}

/*!
 *  @brief      设置FDC2214一个通道的的时钟频率,
                差分传感器配置：
                0x01 -- 传感器频率0.01MHz和8.75MHz
                0x02 -- 传感器频率5MHz和10MHz
                单端传感器配置：
                0x01 -- 传感器频率0.01MHz和10MHz
 *  @param      channel               通道
 *  @param      frequency_select      时钟选择
 *  @param      divider               分频器，fREFx = fCLK / CHx_FREF_DIVIDER，
 *  @note       fREFx is > 4 × fSENSOR(外部LC振荡器频率)
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetChannelClock(FDC2214_Channel_0, 0x01, 0xFF);
 */
void FDC2214_SetChannelClock(SoftI2C_HandleTypeDef *iic, FDC2214_channel_t channel, uint8_t frequency_select, uint16_t divider)
{
    uint16_t temp = 0;

    temp = (uint16_t)(frequency_select << 12) | (divider & 0x03FF);

    CHx_FIN_SEL[channel] = frequency_select;//记录时钟选择
		FDC_PRINTF("Frequency_select  : %d \r\n",frequency_select);
    fREFx[channel] = (double)ClockFrequency / divider;//记录时钟频率

    switch(channel)
    {
    case FDC2214_Channel_0:
//				printf("CHx_FIN_SEL[0] %d \r\n",CHx_FIN_SEL[0]);
//				printf("FREFx[0]        : [%f] \r\n",fREFx[0]);
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CLOCK_DIVIDERS_CH0, temp);
        break;

    case FDC2214_Channel_1:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CLOCK_DIVIDERS_CH1, temp);
        break;

    case FDC2214_Channel_2:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CLOCK_DIVIDERS_CH2, temp);
        break;

    case FDC2214_Channel_3:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CLOCK_DIVIDERS_CH3, temp);
        break;
    }
}

/*!
 *  @brief      配置INTB引脚中断功能
 *  @param      mode      1：开中断；0：关中断
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetDriveCurrent(FDC2214_Channel_0, FDC2214_Drive_Current_0_025);
 */
void FDC2214_SetINTB(SoftI2C_HandleTypeDef *iic, uint8_t mode)
{
    uint16_t temp = 0;

    temp = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_CONFIG);
    temp &= 0xFF7F;
    if(mode)
        temp |= 0x0080;
    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CONFIG, temp);

    temp = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_ERROR_CONFIG);
    temp &= 0xFFFE;
    if(mode)
        temp |= 0x0001;
    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_ERROR_CONFIG, temp);
}

/*!
 *  @brief      激活FDC2214的一个通道，单通道模式
 *  @param      channel     通道
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetActiveChannel(FDC2214_Channel_0);
 */
void FDC2214_SetActiveChannel(SoftI2C_HandleTypeDef *iic, FDC2214_channel_t channel)
{
    uint16_t temp = 0;
    temp = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_CONFIG);
    temp &= 0x3FFF;
    temp |= (uint16_t)(channel << 14);

    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CONFIG, temp);
}

/*!
 *  @brief      设置FDC2214的工作模式：正常、睡眠。
 *  @param      mode     0：正常工作；1：睡激活模式选择。眠
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetSleepMode(0);
 */
void FDC2214_SetSleepMode(SoftI2C_HandleTypeDef *iic, uint8_t mode)
{
    uint16_t temp = 0;
    temp = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_CONFIG);
    temp &= 0xDFFF;
    if(mode)temp |= 0x2000;

    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CONFIG, temp);
}

/*!
 *  @brief      FDC2214激活模式选择。
 *  @param      mode     0：全电流激活模式；1：低功率激活模式
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetCurrentMode(1);
 */
void FDC2214_SetCurrentMode(SoftI2C_HandleTypeDef *iic, uint8_t mode)
{
    uint16_t temp = 0;
    temp = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_CONFIG);
    temp &= 0xF7FF;
    if(mode)
        temp |= 0x800;

    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CONFIG, temp);
}

/*!
 *  @brief      设置FDC2214时钟源
 *  @param      src     0：内部时钟源；1：外部时钟源
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetClockSource(1);
 */
void FDC2214_SetClockSource(SoftI2C_HandleTypeDef *iic, uint8_t src)
{
    uint16_t temp = 0;
    temp = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_CONFIG);
    temp &= 0xFDFF;
    if(src)
        temp |= 0x200;

    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CONFIG, temp);
}

/*!
 *  @brief      高电流传感器驱动，只适用于单通道模式。
 *  @param      mode     0：正常（最大1.5mA）；1：高电流传感器驱动（>1.5mA）
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetHighCurrentMode(0);
 */
void FDC2214_SetHighCurrentMode(SoftI2C_HandleTypeDef *iic, uint8_t mode)
{
    uint16_t temp = 0;
    temp = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_CONFIG);
    temp &= 0xFFBF;
    if(mode)
        temp |= 0x40;

    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_CONFIG, temp);
}

/*!
 *  @brief      设置FDC2214的MUX CONFIG寄存器
 *  @param      autoscan      自动扫描模式 0：关闭，1：打开
 *  @param      channels      自动扫描通道
 *  @param      bandwidth     数字滤波器带宽，大于外部LC振荡器频率
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetMUX_CONFIG(1, FDC2214_Channel_Sequence_0_1_2, FDC2214_Bandwidth_10M);
 */
void FDC2214_SetMUX_CONFIG(SoftI2C_HandleTypeDef *iic, uint8_t autoscan, FDC2214_channel_sequence_t channels, FDC2214_filter_bandwidth_t bandwidth)
{
    uint16_t temp = 0;

    if(autoscan)
    {
        temp = (uint16_t)(autoscan << 15) | (channels << 13) | 0x0208 | bandwidth;
    }
    else
    {
        temp = 0x0208 | bandwidth;
    }

    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_MUX_CONFIG, temp);
}

/*!
 *  @brief      传感器复位
 *  @since      v1.0
 *  Sample usage:       FDC2214_Reset();
 */
void FDC2214_Reset(SoftI2C_HandleTypeDef *iic)
{
    FDC2214_Write16(iic, FDC2214_Addr, FDC2214_RESET_DEV, 0x8000);
}

/*!
 *  @brief      设置FDC2214的一个通道传感器驱动电流，确保振荡幅度在1.2V和1.8V之间。
 *  @param      channel      通道
 *  @param      current      电流大小
 *  @since      v1.0
 *  Sample usage:       FDC2214_SetDriveCurrent(FDC2214_Channel_0, FDC2214_Drive_Current_0_025);
 */
void FDC2214_SetDriveCurrent(SoftI2C_HandleTypeDef *iic, FDC2214_channel_t channel, FDC2214_drive_current_t current)
{
    uint16_t temp = 0;
    temp = (uint16_t)(current << 11);

    switch(channel)
    {
    case FDC2214_Channel_0:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_DRIVE_CURRENT_CH0, temp);
        break;

    case FDC2214_Channel_1:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_DRIVE_CURRENT_CH1, temp);
        break;

    case FDC2214_Channel_2:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_DRIVE_CURRENT_CH2, temp);
        break;

    case FDC2214_Channel_3:
        FDC2214_Write16(iic, FDC2214_Addr, FDC2214_DRIVE_CURRENT_CH3, temp);
        break;
    }
}

/*!
 *  @brief      计算传感器频率
 *  @param      channel      通道
 *  @param      datax        读取的转换值
 *  @return     频率，单位Hz
 *  @since      v1.0
 *  Sample usage:       FDC2214_CalculateFrequency(FDC2214_Channel_0, 0xFF);
 */
double FDC2214_CalculateFrequency(FDC2214_channel_t channel, uint32_t datax)
{
    double frequency = 0.0;
	
    frequency = (double)(CHx_FIN_SEL[channel] * fREFx[channel] * datax) / 268435456.0;//2^28 0.298*data
//		printf("CHx_FIN_SEL[0] %d \r\n",CHx_FIN_SEL[0]);
    return frequency;
}

/*!
 *  @brief      计算外部电容大小
 *  @param      frequency      频率大小，单位Hz
 *  @param      inductance     板载电感大小，单位uH
 *  @param      capacitance    板载电容大小，单位pF
 *  @return     外部电容大小，单位pF
 *  @since      v1.0
 *  Sample usage:       FDC2214_CalculateCapacitance(FDC2214_Channel_0, 6000000, 18, 33);
 */
double FDC2214_CalculateCapacitance(double frequency, float inductance, float capacitance)
{
    double cap = 0.0;

    cap = (double)(1000000000000000000/ (inductance * (2 * PI * frequency) * (2 * PI * frequency)))-capacitance; // 
    return cap;
}

/*!
 *  @brief      FDC2214初始化函数
 *  @return     1：成功；0：失败
 *  @since      v1.0
 *  Sample usage:       FDC2214_Init();
 *	RCOUNT：值越大，分辨率越高，但速度越慢。5000 合理，建议范围 2000 ~ 10000
 *	SETTLECOUNT：不能太小。建议 settleCount = fSensor / fRef * 10，可先设为 500 或 1000 测试。实际电路中电容越大、阻抗越高需要越大的 settleCount。
 *	FDC2214_Drive_Current_0_081：电容值小/感应距离短时，低电流更稳定。电容大或探测距离远时，可尝试 0.146mA、0.206mA 等更大电流
 */
uint8_t FDC2214_Init(SoftI2C_HandleTypeDef *iic )
{
		delay_us(10);
		FDC2214_IIC_Stop(iic);
		delay_us(10);
		FDC2214_IIC_Start(iic);
		delay_us(10);
		FDC_PRINTF("\r\n=== FDC2214_deviceID ===\r\n");
		
    uint16_t deviceID = 0;
    deviceID = FDC2214_Read16(iic, FDC2214_Addr, FDC2214_DEVICE_ID);//读器件ID
		FDC_PRINTF("FDC2214_deviceID  : 0x%02x \r\n" ,deviceID );
		uint16_t RCOUNT = 20000; // 5000对应2000us 转换周期
		uint16_t SETTLECOUNT = 4000; // 200对应200 cycles 等待时间
    if(deviceID == FDC2214_ID)
    {
				
        FDC2214_Reset(iic);
				// 通道0
        FDC2214_SetRcount(iic, FDC2214_Channel_0, RCOUNT);// 转换周期
        FDC2214_SetSettleCount(iic, FDC2214_Channel_0, SETTLECOUNT);// cycles 等待时间
				FDC2214_SetChannelClock(iic, FDC2214_Channel_0, 0x02, 1);//设置时钟分频，1分频，40MHz
//				// 通道1
//				FDC2214_SetRcount(FDC2214_Channel_1, RCOUNT);//2000us 转换周期
//				FDC2214_SetSettleCount(FDC2214_Channel_1, SETTLECOUNT);//200 cycles 等待时间
//				FDC2214_SetChannelClock(FDC2214_Channel_1, 1, 1);//设置时钟分频，1分频，40MHz
//				// 通道2
//        FDC2214_SetRcount(FDC2214_Channel_2, RCOUNT);//2000us 转换周期
//				FDC2214_SetSettleCount(FDC2214_Channel_2, SETTLECOUNT);//200 cycles 等待时间
//				FDC2214_SetChannelClock(FDC2214_Channel_2, 1, 1);//设置时钟分频，1分频，40MHz
//				// 通道3			
//        FDC2214_SetRcount(FDC2214_Channel_3, RCOUNT);//2000us 转换周期
//				FDC2214_SetSettleCount(FDC2214_Channel_3, SETTLECOUNT);//200 cycles 等待时间
//				FDC2214_SetChannelClock(FDC2214_Channel_3, 1, 1);//设置时钟分频，1分频，40MHz

        FDC2214_SetINTB(iic, 0);//关闭INTB中断       
        FDC2214_SetCurrentMode(iic, 1);//全电流激活模式
        FDC2214_SetClockSource(iic, 1);//1外部时钟源 0内部晶振
        FDC2214_SetHighCurrentMode(iic, 0);//0：正常（最大1.5mA）；1：高电流传感器驱动（>1.5mA）
				
				FDC2214_SetMUX_CONFIG(iic, 1, FDC2214_Channel_Sequence_0_1, FDC2214_Bandwidth_3_3M); // 0关闭自动扫描，设置带宽， 带宽调小，抗干扰性更强
				FDC2214_SetActiveChannel(iic, FDC2214_Channel_0);//激活通道0
				
//				FDC2214_SetMUX_CONFIG(1, FDC2214_Channel_Sequence_0_1, FDC2214_Bandwidth_10M);//开启01通道采集，打开自动扫描，10MHz带宽
//        FDC2214_SetMUX_CONFIG(1, FDC2214_Channel_Sequence_0_1_2_3, FDC2214_Bandwidth_3_3M);//开启0123通道采集，打开自动扫描，10MHz带宽

        FDC2214_SetDriveCurrent(iic, FDC2214_Channel_0, FDC2214_Drive_Current_0_356);//通道0驱动电流
//        FDC2214_SetDriveCurrent(FDC2214_Channel_1, FDC2214_Drive_Current_1_006);//通道1驱动电流
//        FDC2214_SetDriveCurrent(FDC2214_Channel_2, FDC2214_Drive_Current_1_006);//通道2驱动电流
//        FDC2214_SetDriveCurrent(FDC2214_Channel_3, FDC2214_Drive_Current_1_006);//通道3驱动电流
				
        FDC2214_SetSleepMode(iic, 0);//退出睡眠状态，开始工作
				FDC_PRINTF("FDC2214 Init OK!\r\n");
				FDC_PRINTF("============================\r\n");				
        osDelay(100);
        return 0;
    }
    else
    {  
				FDC_PRINTF("FDC2214 Init Failed!! \r\n");
				FDC_PRINTF("============================\r\n");
				return 1;     
    }
}

long filterread(SoftI2C_HandleTypeDef *iic, FDC2214_channel_t channel, uint32_t *data)
{
    uint16_t i = 0, try_count = 0;
    long result = 0;
    long Data[20], Sum = 0, max = 0, min = 0;

    while (i < 20 && try_count < 100)
    {
        try_count++;
        if (FDC2214_GetChannelData(iic, channel, data) == 1)
        {
            Data[i++] = *data;
        }
        delay_us(100);
    }

    if (i < 20) {
        DEBUG_PRINTF("FDC2214 data read timeout! Only %d samples.\r\n", i);
        return -1; // 读取失败
    }

    Sum = Data[0];
    max = Data[0];
    min = Data[0];
    for (i = 1; i < 20; i++)
    {
        if (max < Data[i]) max = Data[i];
        if (min > Data[i]) min = Data[i];
        Sum += Data[i];
    }

    result = (Sum - max - min) / 18;
    return result;
}

double getcap(SoftI2C_HandleTypeDef *iic, uint8_t channel,double dg,double dr)
{
    double Cap=0.0;
    switch(channel)
    {
    case 0:
        ch0=filterread(iic, FDC2214_Channel_0,&ch0);
        FRE[0]=FDC2214_CalculateFrequency(FDC2214_Channel_0,ch0);
				DEBUG_PRINTF("\r\nCH0 RAW: [%u], Frequency: [%lf] MHz", ch0, FRE[0]/1000000);
        Cap=FDC2214_CalculateCapacitance(FRE[0], dg, dr);
        break;
    case 1:
        ch1=filterread(iic, FDC2214_Channel_1,&ch1);
        FRE[1]=FDC2214_CalculateFrequency(FDC2214_Channel_1,ch1);
				DEBUG_PRINTF("CH1 RAW = %u, Frequency = %lf KHz\r\n", ch1, FRE[1]/1000);
        Cap=FDC2214_CalculateCapacitance(FRE[1], dg, dr);
        break;
    case 2:
        ch2=filterread(iic, FDC2214_Channel_2,&ch2);
        FRE[2]=FDC2214_CalculateFrequency(FDC2214_Channel_2,ch2);
				DEBUG_PRINTF("CH2 RAW = %u, Frequency = %lf KHz\r\n", ch2, FRE[2]/1000);
        Cap=FDC2214_CalculateCapacitance(FRE[2], dg, dr);
        break;
    case 3:
        ch3=filterread(iic, FDC2214_Channel_3,&ch3);
        FRE[3]=FDC2214_CalculateFrequency(FDC2214_Channel_3,ch3);
				DEBUG_PRINTF("CH3 RAW = %u, Frequency = %lf KHz\r\n", ch3, FRE[3]/1000);
        Cap=FDC2214_CalculateCapacitance(FRE[3], dg, dr);
        break;
    }
    return Cap;
}

void ProcessFDC2214(SoftI2C_HandleTypeDef* iic, osMessageQueueId_t queue, uint8_t* ready_flag, uint8_t* valid_flag, uint8_t channel, uint8_t inductor, uint8_t par_capacitor, float zeroValue, uint8_t device_index) 
{
//  delay_us(10);
		FDC2214_IIC_Stop(iic);
		delay_us(10);
		FDC2214_IIC_Start(iic);
		delay_us(10);
	FDC2214_Reset(iic);
		if (!(*ready_flag)) {
        *ready_flag = (FDC2214_Init(iic) == 0) ? 1 : 0;
        if (!(*ready_flag)) {
            FDC_PRINTF("\r\nRetrying %d FDC2214 init  in 2s ...", device_index);
            osDelay(2000);
            return;
        } else {
            *valid_flag = 1;
        }
    }

    if (*valid_flag) {
				float cap = 0.0f ;
        cap = getcap(iic, channel, inductor, par_capacitor) - zeroValue; // 采集数据 通道0,电感，电容 /* 并联 18uh - 13pf : 测量47pf , -30.6 , 并联 18uh - 33pf : 测量47pf , -43.1 */
				
			if(device_index == 1)
				{
					Filter_msg1 msg1;				
					msg1.data = cap;
					
					if (xQueueSend(queue, &msg1, pdMS_TO_TICKS(1)) != pdPASS) {
							FDC_PRINTF("\r\nQueue full, cap=%.3f not sent", cap);
					}
					return;
				}
				
				if(device_index == 2)
				{
					Filter_msg2 msg2;				
					msg2.data = cap;
					
					if (xQueueSend(queue, &msg2, pdMS_TO_TICKS(1)) != pdPASS) {
							FDC_PRINTF("\r\nQueue full, cap=%.3f not sent", cap);
					}
					return;
				}

		FDC2214_IIC_Stop(iic);
		delay_us(10);

    }
}

void CheckFDC2214_ID(SoftI2C_HandleTypeDef* iic, uint8_t device_index, uint8_t* deviceValid)
{
	
		FDC2214_IIC_Stop(iic);
		osDelay(1);
		FDC2214_IIC_Start(iic);
		osDelay(1);
		FDC2214_Reset(iic);
	
    uint16_t id = 0;
    id = FDC2214_Read16(iic, FDC2214_Addr, 0x7F);

    if (id == FDC2214_ID) {
        FDC_PRINTF("[FDC2214-%d] ok: ID=0x%04X\r\n", device_index, id);
        *deviceValid = 1;
    } else {
        *deviceValid = 0;
				FDC_PRINTF("[FDC2214-%d] error: ID=0x%04X\r\n", device_index, id);
    }
		FDC2214_IIC_Stop(iic);
		osDelay(1);
		FDC2214_IIC_Start(iic);
		osDelay(1);
}