#include "board.h" #include "tdc_tof.h" #define ANZ_FIRE 12u //fire发送脉冲数 #define DIV_FIRE 3u //内部时钟产生脉冲的分频 #define DIV_CLKHS 0u //高速时钟分频 #define START_CLKHS 2u //高速启动后测量前晶振启动间隔时间,120us #define NEG_START 0u //start通道边缘 #define NEG_STOP 0u //stop通道边缘 #define HITIN 9u //stop预期脉冲数 #define SEL_TIMO_MB 0u //暂未确定数值 #define SEL_TSTO2 3u //32khz //和MS1030不同 #define SEL_TSTO1 0u //暂时不用这个引脚，配置为高速时钟输入模式 #define DFLT_WREG0 ((ANZ_FIRE<<25) | \ ( DIV_FIRE<<19) | \ ( DIV_CLKHS<<17) | \ ( START_CLKHS<<14) | \ ( NEG_START<<13) | \ ( NEG_STOP<<12) | \ ( HITIN<< 8) | \ ( SEL_TSTO2<< 2) | \ ( SEL_TSTO1<< 0)) #define EN_INT_TO 1 //TIMOUT中断触发位 #define EN_INT_HIT 0 //END HITS中断触发位 #define EN_INT_ALU 1 //alu中断触发位 #define RFEDGE 0 //stop通道边沿敏感性 #define UP_DELVAL1 0 //当EN_FIST_WAVE=1时,为第一波屏蔽窗口 BIT12~27 暂未设置 #define OFFSET 0 //当en_hc=1时, 62~-64mV 暂未设置 #define DFLT_WREG1 (( EN_INT_TO<<31) | \ ( EN_INT_HIT<<30) | \ ( EN_INT_ALU<<29) | \ ( RFEDGE<<28) | \ ( UP_DELVAL1<<12) | \ ( OFFSET<< 0)) #define EN_FIRST_WAVE 0u //BIT31, 为1时后面的定义有效 #define EDGE_FW 0u //第一波边缘敏感性 #define DELREL1 6u //第1个stop接收第几个回波周期 #define DELREL2 8u //第2个stop接收第几个回波周期 #define DELREL3 10u //第3个stop接收第几个回波周期 #define DELREL4 12u //第4个stop接收第几个回波周期 #define DELREL5 14u //第5个stop接收第几个回波周期 #define DFLT_WREG2 ((EDGE_FW<<30) | \ (DELREL1<<24) | \ (DELREL2<<18) | \ (DELREL3<<12) | \ (DELREL4<< 6) | \ (DELREL5<< 0)) #define DELREL6 16u //第3个stop接收第几个回波周期 #define DELREL7 18u //第4个stop接收第几个回波周期 #define DELREL8 20u //第5个stop接收第几个回波周期 #define DFLT_WREG3 ((DELREL6<<26) | \ (DELREL7<<20) | \ (DELREL8<<14)) #define CONF_FIRE 0 //暂时设置为0 bit29~31 #define PHASE_FIREUP 0 #define PHASE_FIREDOWN 0 #define SEL_START_FIRE 1 #define EN_ANALOG 1 #define HZ60 0 #define TCYCLE 1 #define ANZ_FAKE 0 #define SEL_ECLK_TMP 1 #define TW2 1 //120us充电时间，与1030不同 #define EN_PEAK 0 //峰值检波使能关闭，1030不同 #define EN_DISCHARGE 0 //检波放电使能关闭，1030不同 #define CYCLE_TOF 3 //顺流逆流自动检测模式下,顺流逆流测量间隔时间,初步确定为3=20ms 1030不同 #define EN_ERR_VAL 1 #define EN_DC 1 //stop端口充电模式 这里设置为直流 1030不同 #define FIRE0_DEF 1 //fire通道默认电平 #define DOUBLE_RES 0 #define QUAD_RES 1 #define DFLT_WREG4 (( PHASE_FIREUP<<28) | \ ( PHASE_FIREDOWN<<27) | \ ( SEL_START_FIRE<<26) | \ ( EN_ANALOG<<25) | \ ( HZ60<<24) | \ ( TCYCLE<<23) | \ ( ANZ_FAKE<<22) | \ ( SEL_ECLK_TMP<<21) | \ ( TW2<<17) | \ ( EN_PEAK<<16) | \ ( EN_DISCHARGE<<15) | \ ( CYCLE_TOF<<11) | \ ( EN_ERR_VAL<<10) | \ ( EN_DC<<9) | \ ( FIRE0_DEF<<8) | \ ( DOUBLE_RES<<7) | \ ( QUAD_RES<<6)) #define CYCLE_AUTOTOF (250<<20) //自动测量循环时间 500ms #define ANZ_AUTOTOF (1<<16) //循环次数 一直开启 #define AUTOTOF_MODE (0<<15) //顺逆顺逆 #define PHASE_FIRENUM (3<<6) //插入相位位置 #define ANZ_PHASE (8) //插入周期数 #define DFLT_WREG5 0 #define DOWN_DELVAL1 (0<<12)//第一波屏蔽窗口 #define SEL_SRC_CLK (0<<7)//使用晶振产生时钟 #define EN_HC (1<<6) #define CURR32K (0<<5) #define DIS_Vref (1<<4) #define ADJ_LDO_ROUGH (0<<3) #define ADJ_LDO_FINE (4<<0) #define DFLT_WREG6 (DOWN_DELVAL1|SEL_SRC_CLK|EN_HC|CURR32K|DIS_Vref|ADJ_LDO_ROUGH|ADJ_LDO_FINE) uint16_t tof_state = TOF_ST_INIT; uint32_t tof_flag; //uint16_t CntSync = 0; void tdc_tof_init(void) { //MUX ADDA : PA08; ADDB : PC09 mux_init(); // MS1030 RSTN : PD02 GpioInit(PD02_AF0, GPIO_DIG_OUT_WEAK_PUSH_NONEUP_NONEDOWN); GpioClrPins(GpioPortD, GpioBitMsk2); // reset active low us_delay(10); // 10us > tPH(30ns) GpioSetPins(GpioPortD, GpioBitMsk2); // reset release us_delay(10); // 10us > tRFS(30ns) tdc_spi_init(); // peripheral init PeriClk_MutEnable(PeriClk_Spi1); DMA_ReqClkOn(); // TDC POR tdc_spi_wr_code(0x50); us_delay(1000); // 1000us > specification(500us) // tdc_spi_wr_d32(0x80, 0x1E18884E); // 修正：SEL_TIMO_MB = 8（合法） // tdc_spi_wr_d32(0x81, 0xA00C8005); // tdc_spi_wr_d32(0x82, 0x83105187); // tdc_spi_wr_d32(0x83, 0x20928400); // tdc_spi_wr_d32(0x84, 0x66E00540); // tdc_spi_wr_d32(0x85, 0x07D10000); // tdc_spi_wr_d32(0x86, 0x000C8004); tdc_spi_wr_d32(0x80, DFLT_WREG0); uint32_t read_a0=tdc_spi_rd_d32(0xA0); read_a0=read_a0&0xFFu; #if 1 tof_flag = TOF_FLAG_UNALGND_CHOP; if (read_a0!=(DFLT_WREG0&0xFFu)){ tof_flag |= TOF_FLAG_TDC_FAULT; } else { // write_reg0 tdc_spi_wr_d32(0x80, DFLT_WREG0 | ( 2u<< 4) // SEL_TIMO_MB, 0:64us; 1:128us; 2:256us; ~ 6:4096us; ); // write_reg1 tdc_spi_wr_d32(0x81, DFLT_WREG1 | (5760u<< 12) // DELVAL1=45us, format:16.5, time unit:Tref; ); // write_reg2 tdc_spi_wr_d32(0x82, DFLT_WREG2 | ( 0u<<31) // EN_FIRST_WAVE, 0:disable; 1:enable; ); // write_reg3 tdc_spi_wr_d32(0x83, DFLT_WREG3 | ( 1u<<13) // measure FW pulse width, 0:on; 1:off ); // write_reg4 tdc_spi_wr_d32(0x84, DFLT_WREG4); // write_reg5 tdc_spi_wr_d32(0x85, DFLT_WREG5); // write_reg6 tdc_spi_wr_d32(0x86, DFLT_WREG6); tdc_spi_wr_code(0x70); tdc_spi_wr_code(0x06); // not 'INTN' but 'us_delay' to avoid dead loop when INTN fault us_delay(2000); // > 480 + 600 + 16 + 70 tdc_read_cal(0x0u); // tof_flag ^= TOF_FLAG_UNALGND_CHOP; // // start a single ToF to align MS1030 internal calibration order(1 CAL every 24 ToF) // // must be sure that CAL not in the middle of UP Tof and DOWN Tof // tdc_start_tof(TDC_CFG_DO_WREG | TDC_CFG_FIRE_UP); // // // not 'INTN' but 'us_delay' to avoid dead loop when INTN fault // us_delay(2000); // > 480 + 600 + 16 + 70 // // tdc_read_tof(TDC_CFG_FIRE_UP); // read data and clear interrupt } #endif DMA_ReqClkOff(); PeriClk_MutDisable(PeriClk_Spi1); // MS1030 INTN : PD03 falling edge trigger GpioInit(PD03_AF0, GPIO_DIG_IN_PULLUP_NONEDOWN); GpioEventInit(PD03_AF0, GPIO_EVENT_FALL); // enable interrupt at NVIC side IRQMutEnable(PORTD_F_IRQn, NVIC_PRIO_1); } void tdc_start_tof(uint32_t cfg) { //rt_enter_critical(); uint32_t u32tmp; if (cfg & TDC_CFG_DO_CKENA){ PeriClk_MutEnable(PeriClk_Spi1); DMA_ReqClkOn(); } if (cfg & TDC_CFG_DO_WREG){ // write_reg0 tdc_spi_wr_d32(0x80, DFLT_WREG0 | ( 3u<< 4) // SEL_TIMO_MB, 0:64us; 1:128us; 2:256us; ~ 6:4096us; //modified by yuewei 20260526 1->3, 128us -> 512us, for DN300 almost 240us ); // write_reg1 // DN15: L/C > (42+21+10*2)*1000/1555=53us tdc_spi_wr_d32(0x81, DFLT_WREG1 | (5760u<< 12) // DELVAL1=45us, format:16.5, time unit:Tref;//1031 从BIT12开始 // (6400u<< 7) // DELVAL1=50us, format:16.5, time unit:Tref; // (7040u<< 7) // DELVAL1=55us, format:16.5, time unit:Tref; // (7680u<< 7) // DELVAL1=60us, format:16.5, time unit:Tref; // (8230u<< 7) // DELVAL1=65us, format:16.5, time unit:Tref; // (8960u<< 7) // DELVAL1=70us, format:16.5, time unit:Tref; ); // write_reg2 tdc_spi_wr_d32(0x82, DFLT_WREG2 | ( 1u<<31) // EN_FIRST_WAVE, 0:disable; 1:enable; ); // write_reg3 u32tmp = DFLT_WREG3 | ( 0u<<13) | // measure FW pulse width, 0:on; 1:off (TdcFwOfst<<6); // FW wave_offs, 0:0mv; 1~63:2~126mV; 64~127:-128~-2mV; if (tof_state&TOF_ST_VOID){ u32tmp |= (1u<<13); // disable FW pulse width to save power } tdc_spi_wr_d32(0x83, u32tmp); } // write_reg4 u32tmp = DFLT_WREG4; u32tmp |= (1u<<30); // FIRE_UP, 0:off; 1:on; tdc_spi_wr_d32(0x84, u32tmp); // Initial TDC status regs tdc_spi_wr_code(0x70); // start single-direction measure tdc_spi_wr_code(0x03); if (cfg & TDC_CFG_DO_CKDIS){ DMA_ReqClkOff(); PeriClk_MutDisable(PeriClk_Spi1); } //rt_exit_critical(); } uint16_t TdcStatus; uint16_t TdcUPwFw; uint16_t TdcUPwHw; uint32_t TdcUToF[9]; uint16_t TdcDPwFw; uint16_t TdcDPwHw; uint32_t TdcDToF[9]; uint32_t TdcCal; //float TdcScale = 250.0f/65536; float TdcScale = 0.00381147861f; uint32_t TdcPT[4]; float TdcResist = 5000; // OhmX10 //modified by yuewei 20260520 //float WaterPrmt[2] = {25.0f, 1496.6f}; float WaterPrmt[2] = {25.0f, 1497.1484f}; void tdc_read_tof(uint32_t cfg) { // uint16_t i; if (cfg & TDC_CFG_DO_CKENA){ PeriClk_MutEnable(PeriClk_Spi1); DMA_ReqClkOn(); } TdcStatus = tdc_spi_rd_d16(0xd0); // 读取状态寄存器 if ((TdcStatus&0x0300)==0x0000u){ tof_flag ^= TOF_FLAG_UNALGND_CHOP; // if (CntSync>=240){ // CntSync = 10; // } else { // CntSync += 10; // } } if ((TdcStatus&0x03F0)!=0x0090u){ tof_flag |= TOF_FLAG_BLANK_PIPE; tdc_spi_wr_code(0x70); } else { TdcUPwFw = tdc_spi_rd_d16(0xd3); // First wave pulse width TdcUPwHw = tdc_spi_rd_d16(0xd4); // 1st Hit wave pulse width tdc_spi_wr_code(0x70); // TOF_UP_HITs TdcUToF[0] = tdc_spi_rd_d32(0xb8)*(1.0f/8); // average of all // TOF_DOWN_HITs TdcDToF[0] = tdc_spi_rd_d32(0xc8)*(1.0f/8); // average of all // for(i=0; i<8; i++){ // TdcDToF[i+1] = tdc_spi_rd_d32(0xb0 + i); // }; } if (cfg & TDC_CFG_DO_CKDIS){ DMA_ReqClkOff(); PeriClk_MutDisable(PeriClk_Spi1); } } void tdc_start_cal(uint32_t cfg) { //rt_enter_critical(); if (cfg & TDC_CFG_DO_CKENA){ PeriClk_MutEnable(PeriClk_Spi1); DMA_ReqClkOn(); } // 8*T_32k = 244.140625us if (cfg & TDC_CFG_DO_WREG){ // write_reg0 tdc_spi_wr_d32(0x80, DFLT_WREG0 | ( 2u<< 4) // SEL_TIMO_MB, 0:64us; 1:128us; 2:256us; ~ 6:4096us; ); // write_reg3 tdc_spi_wr_d32(0x83, DFLT_WREG3 | ( 1u<<13) // measure FW pulse width, 0:on; 1:off ); } tdc_spi_wr_code(0x70); tdc_spi_wr_code(0x06); if (cfg & TDC_CFG_DO_CKDIS){ DMA_ReqClkOff(); PeriClk_MutDisable(PeriClk_Spi1); } //rt_exit_critical(); } void tdc_read_cal(uint32_t cfg) { //rt_enter_critical(); if (cfg & TDC_CFG_DO_CKENA){ PeriClk_MutEnable(PeriClk_Spi1); DMA_ReqClkOn(); } TdcStatus = tdc_spi_rd_d16(0xd0); // status if ((TdcStatus&0x0300)==0x0000u){ tof_flag ^= TOF_FLAG_UNALGND_CHOP; // if (CntSync>=240){ // CntSync = 10; // } else { // CntSync += 10; // } tdc_spi_wr_code(0x70); TdcCal = tdc_spi_rd_d32(0xd5); // TdcScale = 244140.625f/TdcCal; TdcScale += (244140.625f/TdcCal - TdcScale)*(1.0f/2); //LOG("----------TdcScale=%.6f\n",TdcScale); } else { tdc_spi_wr_code(0x70); } if (cfg & TDC_CFG_DO_CKDIS){ DMA_ReqClkOff(); PeriClk_MutDisable(PeriClk_Spi1); } //rt_exit_critical(); } void tdc_start_temp(uint32_t cfg) { //rt_enter_critical(); if (cfg & TDC_CFG_DO_CKENA){ PeriClk_MutEnable(PeriClk_Spi1); DMA_ReqClkOn(); } if (cfg & TDC_CFG_DO_WREG){ // write_reg0 tdc_spi_wr_d32(0x80, DFLT_WREG0 | ( 3u<< 6) // SEL_TIMO_MB, 0:64us; 1:128us; 2:256us; 3:512us; ~ 6:4096us; ); // write_reg1 tdc_spi_wr_d32(0x81, DFLT_WREG1 | ( 0u<< 12) // DELVAL1=0us, format:16.5, time unit:Tref; ); // write_reg3 tdc_spi_wr_d32(0x83, DFLT_WREG3 | ( 1u<<13) // measure FW pulse width, 0:on; 1:off ); } tdc_spi_wr_code(0x70); tdc_spi_wr_code(0x04); if (cfg & TDC_CFG_DO_CKDIS){ DMA_ReqClkOff(); PeriClk_MutDisable(PeriClk_Spi1); } //rt_exit_critical(); } void tdc_read_temp(uint32_t cfg) { //rt_enter_critical(); if (cfg & TDC_CFG_DO_CKENA){ PeriClk_MutEnable(PeriClk_Spi1); DMA_ReqClkOn(); } TdcStatus = tdc_spi_rd_d16(0xd0); // status tdc_spi_wr_code(0x70); TdcPT[0] = tdc_spi_rd_d32(0xd1); // REF 5K +/- 5 TdcPT[1] = tdc_spi_rd_d32(0xd2); // U7 //TdcPT[2] = tdc_spi_rd_d32(0xc4); // U8 //TdcPT[3] = tdc_spi_rd_d32(0xc5); // REF 1.5K // if (TdcPT[0]>0x1000000u && TdcPT[0]<0x2000000u){ // TdcResist = 5000.0f*TdcPT[1]/TdcPT[0]; // Ohm2Dgr2USSpd(TdcResist, WaterPrmt); // [0]:Temperature; [1]:ultrasonic speed @Temp // } if (TdcPT[0]>0xB00000u && TdcPT[0]<0xE00000u){ TdcResist = 2500.0f*TdcPT[1]/TdcPT[0]; // ohmX10 Ohm2Dgr2USSpd(TdcResist, WaterPrmt); // [0]:Temperature; [1]:ultrasonic speed @Temp } //LOG("----------T=%.6f,C=%.6f\n",WaterPrmt[0],WaterPrmt[1]); if (cfg & TDC_CFG_DO_CKDIS){ DMA_ReqClkOff(); PeriClk_MutDisable(PeriClk_Spi1); } //rt_exit_critical(); }