Revision 4:217334c3a5b2, committed 2020-07-28

Comitter:

china_sn0w

Date:

Tue Jul 28 01:40:05 2020 +0000

Parent:

3:35b05d91568d

Commit message:

A

Changed in this revision

--- a/CmdHandler.cpp	Mon Jun 22 05:27:48 2020 +0000
+++ b/CmdHandler.cpp	Tue Jul 28 01:40:05 2020 +0000
@@ -8,6 +8,8 @@
 //#include "Config.h"
 #include "ServoRun.h"
 
+#include "Van_API.h"
+
 GLOBAL_CMD_VAR g_cmd;
 
 //extern CONFIG g_config;
@@ -29,6 +31,7 @@
 extern DigitalOut rLED;
 extern DigitalOut xSHUT;
 extern InterruptIn  chip_int;
+extern DigitalOut TRIM_EN;
 
 extern uint16_t histogram_pos_num;
 extern uint16_t histogram_per_pos;
@@ -36,6 +39,8 @@
 extern uint16_t range_step_num;
 extern uint16_t range_per_step;
 
+extern uint8_t work_mode;
+
 void CmdHandleTask(void)
 {
     uint8_t ret;
@@ -164,7 +169,7 @@
 
         case VAN_SINGLE_MEAS:// 单次测量
             if(HandleOneTimeMeasure() == 0)
-                UART_CmdAckSend(READ_CMD | 0x80, VAN_SINGLE_MEAS, _uart_send_pbuff, 10);
+                UART_CmdAckSend(READ_CMD | 0x80, VAN_SINGLE_MEAS, _uart_send_pbuff, 14);
             else
                 UART_CmdAckSend(READ_CMD | 0x10, VAN_SINGLE_MEAS, _uart_send_pbuff, 2);
             break;
@@ -244,7 +249,13 @@
 
         case INT_ENABLE_CMD:
             if(pd[5] == 1)
+            {
+                work_mode = 1;
                 chip_int.disable_irq();
+                WriteOneReg(0x3D, 0xC8);
+                //WriteOneReg(0x3E, 0x04);
+                TRIM_EN = 1;
+            }
             break;
 
         case VAN_DCR_TEST_CMD:
@@ -278,6 +289,9 @@
         case VAN_RCO_TRIM_CMD:
             if(RCO_Trim(_uart_send_pbuff) == 0)
             {
+                WriteOneReg(0x37, _uart_send_pbuff[0]);
+                WriteOneReg(0x3E, 0x40);
+                WriteOneReg(0x3F, 0x8C);
                 _uart_send_pbuff[1] = _uart_send_pbuff[0];
                 _uart_send_pbuff[0] = 0;
                 UART_CmdAckSend(READ_CMD | 0x80, VAN_RCO_TRIM_CMD, _uart_send_pbuff, 2);
@@ -311,6 +325,29 @@
             else
                 UART_CmdAckSend(READ_CMD | 0x10, VAN_PIXEL_EN_CMD, _uart_send_pbuff, 2);        
         break;
+        
+        case VAN_MP_SLECTE_CMD:
+            if(MP_Slect(_uart_send_pbuff) == 0)
+                UART_CmdAckSend(READ_CMD | 0x80, VAN_MP_SLECTE_CMD, _uart_send_pbuff, 129);
+            else
+                UART_CmdAckSend(READ_CMD | 0x10, VAN_MP_SLECTE_CMD, _uart_send_pbuff, 2); 
+        
+        break;
+        
+        case VAN_OTP_WRITE_CMD:
+            if(OTP_Write(&pd[5]) == 0)
+                UART_CmdAckSend(READ_CMD | 0x80, VAN_OTP_WRITE_CMD, _uart_send_pbuff, 2);
+            else
+                UART_CmdAckSend(READ_CMD | 0x10, VAN_OTP_WRITE_CMD, _uart_send_pbuff, 2); 
+        break;
+        
+        case VAN_OTP_READ_CMD:
+            if(OTP_Read(&pd[5], _uart_send_pbuff) == 0)
+                UART_CmdAckSend(READ_CMD | 0x80, VAN_OTP_READ_CMD, _uart_send_pbuff, pd[6]+2);
+            else
+                UART_CmdAckSend(READ_CMD | 0x10, VAN_OTP_READ_CMD, _uart_send_pbuff, 2); 
+            break;
+        
 
         default:
 
@@ -351,22 +388,31 @@
 
 uint8_t HandleOneTimeMeasure(void)
 {
-    return OneTimeMeasure((uint16_t*)_uart_send_pbuff, (uint16_t*)(_uart_send_pbuff + 2), (uint32_t*)(_uart_send_pbuff + 4), (uint16_t*)(_uart_send_pbuff + 8));
+    return OneTimeMeasure((uint16_t*)_uart_send_pbuff, (uint16_t*)(_uart_send_pbuff + 2), (uint32_t*)(_uart_send_pbuff + 4), (uint16_t*)(_uart_send_pbuff + 8), (uint16_t*)(_uart_send_pbuff + 10), (uint16_t*)(_uart_send_pbuff + 12));
 }
 
 uint8_t HandleContinuousMeasure(uint8_t *pd)
-{
+{    
     if(pd[5] == 0x00)
+    {
         return StopContinuousMeasure();
+    }
     else
+    {
         return ContinuousMeasure();
+    }
 }
 
 uint8_t HandleReadHistogram(uint8_t tdc_idx)
 {
     _uart_send_pbuff[0] = tdc_idx;
+    
+    if(tdc_idx == 0xF1)
+        memcpy(&_uart_send_pbuff[1], histogram[0], 1024);
+    else
+        memcpy(&_uart_send_pbuff[1], histogram[tdc_idx], 1024);
 
-    memcpy(&_uart_send_pbuff[1], histogram[tdc_idx], 1024);
+    
 
     UART_CmdAckSend(READ_CMD | 0x80, VAN_READ_HIST_CMD, _uart_send_pbuff, 1025);
     return 0;
@@ -413,7 +459,7 @@
     return ret;
 }
 
-uint8_t HandleContinuousMeasureReport(uint16_t lsb, uint16_t milimeter, uint32_t peak, uint16_t noise_level)
+uint8_t HandleContinuousMeasureReport(uint16_t lsb, uint16_t milimeter, uint32_t peak, uint16_t noise_level, uint16_t ref_lsb, uint16_t ref_milimeter)
 {
     memcpy(_uart_send_pbuff, &lsb, 2);
     memcpy(_uart_send_pbuff + 2, &milimeter, 2);
@@ -421,17 +467,18 @@
     memcpy(_uart_send_pbuff + 4, &peak, 4);
     memcpy(_uart_send_pbuff + 8, &noise_level, 2);
     
+    memcpy(_uart_send_pbuff + 10, &ref_lsb, 2);
+    memcpy(_uart_send_pbuff + 12, &ref_milimeter, 2);
+    
 
-    UART_CmdAckSend(READ_CMD | 0x80, VAN_CONTIU_MEAS, _uart_send_pbuff, 10);
+    UART_CmdAckSend(READ_CMD | 0x80, VAN_CONTIU_MEAS, _uart_send_pbuff, 14);
 
     return 0;
 }
 
 void HandleChipReset(void)
 {
-    xSHUT = 0;
-    wait_ms(30);
-    xSHUT = 1;
+    ChipInitReset();
 }
 
 void HandleFreqReport(float* V_I_Value)

--- a/CmdHandler.h	Mon Jun 22 05:27:48 2020 +0000
+++ b/CmdHandler.h	Tue Jul 28 01:40:05 2020 +0000
@@ -80,6 +80,10 @@
 #define VAN_BVD_TRIM_CMD                   0x61
 #define VAN_PIXEL_EN_CMD                   0x62
 
+#define VAN_MP_SLECTE_CMD                  0x63
+#define VAN_OTP_WRITE_CMD                  0x65
+#define VAN_OTP_READ_CMD                   0x66
+
 
 
 
@@ -94,7 +98,7 @@
 uint8_t HandleContinuousMeasure(uint8_t *pd);
 uint8_t HandleReadHistogram(uint8_t tdc_idx);
 uint8_t HandleDownloadFW(uint8_t *pd, uint16_t cmdLen);
-uint8_t HandleContinuousMeasureReport(uint16_t lsb, uint16_t milimeter, uint32_t peak, uint16_t noise_level);
+uint8_t HandleContinuousMeasureReport(uint16_t lsb, uint16_t milimeter, uint32_t peak, uint16_t noise_level, uint16_t ref_lsb, uint16_t ref_milimeter);
 
 uint8_t HandleSwitch(uint8_t *pd, uint16_t cmdLen);
 void HandleChipReset(void);

--- a/DUT_RegConfig.cpp	Mon Jun 22 05:27:48 2020 +0000
+++ b/DUT_RegConfig.cpp	Tue Jul 28 01:40:05 2020 +0000
@@ -9,21 +9,48 @@
 #include "Firmware.h"
 #include "RegTable.h"
 #include "CmdHandler.h"
+#include "Van_API.h"
 
 DUTREG dut_reg[DUT_REG_NUM];
 uint8_t Firmware[8192];
 uint8_t histogram[10][1024];
+//uint8_t hist_ma[2048];
 uint8_t dcr_matrix[17][9*2];
 
+uint8_t pixel_map[16][18] = {
+    0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00,
+    0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00,
+    0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00,
+    0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00,
+    
+    0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00,
+    0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00,
+    0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00,
+    0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00,
+    
+    0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01,
+    0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02, 0x00, 0x02,
+    0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04, 0x00, 0x04,
+    0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x08,
+    
+    0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10, 0x00, 0x10,
+    0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20, 0x00, 0x20,
+    0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, 0x00, 0x40,
+    0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80
+    };
+
+uint8_t work_mode = 0;
 uint8_t int_mark = 0;
 uint8_t int_enable = 1;
 uint8_t histogram_mode = 0;
 uint8_t histogram_tdc = 0;
+uint8_t continus_ranging = 0;
 
 Semaphore chip_int_semph(1);
 extern DigitalOut xSHUT;
 extern I2C i2c_v;
 extern DigitalOut TRIM_EN;
+extern DigitalOut HIGH_EN;
 
 uint16_t histogram_pos_num = 0;
 uint16_t histogram_per_pos = 0;
@@ -31,16 +58,53 @@
 uint16_t range_step_num = 0;
 uint16_t range_per_step = 0;
 
+uint16_t single_measure_times = 0;
+
 extern const uint8_t reg_table[];
 extern const uint8_t Firmware_Ranging[];
 extern uint8_t  _uart_send_pbuff[CMD_BUF_LEN] ;
 
+
+void ECO_ByPassROM(void)
+{
+    uint8_t tmp = 0;   
+    
+    WriteOneReg(0x00, 0x00);
+    WriteOneReg(0x01, 0x00);
+    ReadOneReg(0x07, &tmp);
+    WriteOneReg(0x07, tmp & 0xFE);    
+}
+
+void ECO_DefaultRegInit(void)
+{
+    WriteOneReg(0x00, 0x03);
+    WriteOneReg(0x01, 0x00);
+    WriteOneReg(0x02, 0x00);
+    WriteOneReg(0x37, 0x61);
+    WriteOneReg(0x38, 0x00);
+    WriteOneReg(0x39, 0x00);
+    WriteOneReg(0x3A, 0x1F);
+    WriteOneReg(0x41, 0x04);
+    WriteOneReg(0x48, 0x7F);
+    WriteOneReg(0x49, 0x01);
+    WriteOneReg(0x4C, 0xFF);
+    WriteOneReg(0x4D, 0x00);
+    WriteOneReg(0x51, 0x0E);
+    WriteOneReg(0xE4, 0xD1);
+    WriteOneReg(0xE8, 0x00);
+    WriteOneReg(0xF7, 0xFF);
+    WriteOneReg(0xF8, 0x8B);
+}
+
 void ChipInitReset(void)
 {
     xSHUT = 0;
     wait_ms(30);
     xSHUT = 1;
     wait_ms(30);
+    //ECO_ByPassROM();
+    //wait_ms(30);
+    //ECO_DefaultRegInit();
 }
 
 void SetBitThree(uint8_t rco, uint8_t tdc, uint8_t dcr)
@@ -53,14 +117,21 @@
 void DUT_RegInit(uint8_t rco, uint8_t tdc, uint8_t dcr)
 {
     LoadRegTable();
-    for(uint16_t i = 0; i < 256; i++) 
-    {
-        WriteOneReg(dut_reg[i].addr, dut_reg[i].value);        
+    for(uint16_t i = 0; i < 256; i++) {
+        WriteOneReg(dut_reg[i].addr, dut_reg[i].value);
     }
 
     SetBitThree(rco, tdc, dcr);
 }
 
+void DUT_RegTableInit(void)
+{
+    for(uint16_t i = 0; i < 256; i++) {
+        dut_reg[i].addr = i;
+        dut_reg[i].value = i;
+    }
+}
+
 void DUT_FirmwareInit(void)
 {
     WriteFW(LoadFirmware());
@@ -69,10 +140,13 @@
 void DeviceAllInit(uint8_t rco, uint8_t tdc, uint8_t dcr)
 {
     ChipInitReset();
-    wait_ms(100);
-    DUT_RegInit(rco, tdc, dcr);
-    wait_ms(100);
-    DUT_FirmwareInit();
+    //wait_ms(100);
+    //DUT_RegInit(rco, tdc, dcr);
+    //wait_ms(100);
+    //DUT_FirmwareInit();
+      
+    
+    DUT_RegTableInit();
     wait_ms(100);
 }
 
@@ -104,7 +178,7 @@
 void HistogramReport()
 {
     uint8_t ret = 0;
-    uint16_t lsb, mili, noise_level;
+    uint16_t lsb, mili, ref_lsb, ref_mili, noise_level;
     uint32_t peak;
 
 
@@ -115,7 +189,7 @@
         if(histogram_mode == 1) {
             //ReadOneReg(REG_SYS_CFG, &sys_cfg_save);
             //WriteOneReg(REG_SYS_CFG, 0x00);
-            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level);
+            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level, &ref_lsb, &ref_mili);//////
             if(ret != 0) {
                 histogram_mode = 0;
             } else {
@@ -130,19 +204,15 @@
         } else if(histogram_mode == 2) {
             //ReadOneReg(REG_SYS_CFG, &sys_cfg_save);
             //WriteOneReg(REG_SYS_CFG, 0x00);
-            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level);
+            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level, &ref_lsb, &ref_mili);//////
             if(ret != 0) {
                 histogram_mode = 0;
             } else {
-                for(uint8_t i = 0; i < 10; i++) {
-                    ret = vangogh_ram_rd(i);
-                    if(ret != 0) {
-                        histogram_mode = 0;
-                        break;
-                    } else {
-                        HandleReadHistogram(i);
-                        wait_ms(1);
-                    }
+                ret = vangogh_ram_rd_ma();
+                if(ret != 0) {
+                    histogram_mode = 0;
+                } else {
+                    HandleReadHistogram(0xF1);
                 }
             }
             //WriteOneReg(REG_SYS_CFG, sys_cfg_save);
@@ -155,12 +225,12 @@
                 if(histogram_pos >= histogram_pos_num) {
                     histogram_mode = 0;
                 }
-                ServoRun(1, 10);
+                ServoRun(1, 20);
                 while(CheckUntil()) wait_ms(100);
             }
             //ReadOneReg(REG_SYS_CFG, &sys_cfg_save);
             //WriteOneReg(REG_SYS_CFG, 0x00);
-            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level);
+            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level, &ref_lsb, &ref_mili);
             if(ret != 0) {
                 histogram_mode = 0;
             } else {
@@ -180,7 +250,7 @@
         } else if(histogram_mode == 4) {
             //ReadOneReg(REG_SYS_CFG, &sys_cfg_save);
             //WriteOneReg(REG_SYS_CFG, 0x00);
-            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level);
+            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level, &ref_lsb, &ref_mili);
             if(ret != 0) {
                 histogram_mode = 0;
             } else {
@@ -203,11 +273,12 @@
                 if(range_step >= range_step_num) {
                     histogram_mode = 0;
                 }
-                ServoRun(1, 10);
-                while(CheckUntil()) wait_ms(100);
+                //ServoRun(1, 10);
+                //while(CheckUntil()) wait_ms(100);
 
                 range_per_step = 0;
-                ret = ContinuousMeasure();
+                single_measure_times = 256;                
+                //ret = ContinuousMeasure();
                 if(ret != 0) {
                     histogram_mode = 0;
                 } else {
@@ -222,9 +293,16 @@
                     wait_ms(5);
                     StoreHistogram(range_step, 0, 3);
                     wait_ms(5);
+                    StoreHistogram(range_step, 0, 4);
+                    wait_ms(5);
+                    StoreHistogram(range_step, 0, 5);
+                    wait_ms(5);
                     range_per_step = 0;
 
                 }
+                
+                ServoRun(1, 10);
+                while(CheckUntil()) wait_ms(100);
             }
 
 
@@ -236,60 +314,104 @@
             histogram_pos_num = 0;
             histogram_per_pos = 0;
         }
-        wait(1);
+        wait_ms(100);
     }
 }
 
-void StoreMeasureData(uint16_t lsb, uint16_t milimeter, uint32_t peak, uint16_t noise_level, uint16_t i)
+void StoreMeasureData(uint16_t lsb, uint16_t milimeter, uint32_t peak, uint16_t noise_level, uint16_t ref_lsb, uint16_t ref_milimeter, uint16_t i)
 {
     histogram[0][4*i] = 0;
     histogram[0][4*i+1] = 0;
     histogram[0][4*i+2] = lsb >> 8;
     histogram[0][4*i+3] = lsb ;
-    
+
     histogram[1][4*i] = 0;
     histogram[1][4*i+1] = 0;
     histogram[1][4*i+2] = milimeter >> 8;
     histogram[1][4*i+3] = milimeter ;
-    
+
     histogram[2][4*i]   = peak >> 24;
     histogram[2][4*i+1] = peak >> 16;
     histogram[2][4*i+2] = peak >> 8;
     histogram[2][4*i+3] = peak ;
-    
+
     histogram[3][4*i]   = 0;
     histogram[3][4*i+1] = 0;
     histogram[3][4*i+2] = noise_level >> 8;
     histogram[3][4*i+3] = noise_level ;
     
+    histogram[4][4*i]   = 0;
+    histogram[4][4*i+1] = 0;
+    histogram[4][4*i+2] = ref_lsb >> 8;
+    histogram[4][4*i+3] = ref_lsb ;
+    
+    histogram[5][4*i]   = 0;
+    histogram[5][4*i+1] = 0;
+    histogram[5][4*i+2] = ref_milimeter >> 8;
+    histogram[5][4*i+3] = ref_milimeter ;
 }
 
 void ContinuousMeasureReport()
 {
-    uint16_t lsb, milimeter, noise_level;
+    uint16_t lsb, milimeter, ref_lsb, ref_milimeter, noise_level;
     uint32_t peak;
-    uint8_t int_flag = 0;
-    uint16_t time_out = 0;
+
+    uint8_t ret = 0;
+    
+    Van_Dist_TypeDef result;
+    Van_Status status;
+
 
     while(1) {
-
-        chip_int_semph.wait();
-        if(histogram_mode == 5) {
-            if(RaadContinuousMeasure(&lsb, &milimeter, &peak, &noise_level) == 0) {
-                StoreMeasureData(lsb, milimeter, peak, noise_level, range_per_step++);
-                if(range_per_step == 256) {
-                    StopContinuousMeasure();
-                }
-            }
-        } else {
-            if(RaadContinuousMeasure(&lsb, &milimeter, &peak, &noise_level) == 0) {
-                HandleContinuousMeasureReport(lsb, milimeter, peak, noise_level);
+        
+        if(continus_ranging == 1)
+        {
+            
+    
+            status = PollingGetRangingResult(&result, VAN_GO_ON);
+            if(status == VAN_OK)
+            {
+                lsb = 0;
+                milimeter = result.millimeter;
+                peak = result.peak;
+                noise_level = result.noise;
+                //ref_lsb = result.confidence;
+                ref_lsb = result.histcnt;
+                ref_milimeter = result.dmax;    
+                
+                ret = ReadOneReg(0x0d, (uint8_t*)&lsb);
+                ret = ReadOneReg(0x0e, (uint8_t*)&lsb + 1);   
+            
+                HandleContinuousMeasureReport(lsb, milimeter, peak, noise_level, ref_lsb, ref_milimeter);
             }
         }
+        wait_ms(25);
+       }
+}
 
+void AutoSingleMeasurment()
+{
+    uint16_t lsb, milimeter, ref_lsb, ref_milimeter, noise_level;
+    uint32_t peak;
+    
+    
+    while(1)
+    {
+        if(single_measure_times > 0)
+        {
+            if(OneTimeMeasure(&lsb, &milimeter, &peak, &noise_level, &ref_lsb, &ref_milimeter) == 0)
+            {
+                StoreMeasureData(lsb, milimeter, peak, noise_level, ref_lsb, ref_milimeter, range_per_step++);
+            }
+            single_measure_times--;
+        }
+        
+        wait_ms(30);
     }
 }
 
+/*
+
 uint8_t WriteOneReg(uint8_t addr, uint8_t data)
 {
     uint8_t buf[2];
@@ -312,6 +434,7 @@
     return 0;
 }
 
+*/
 
 uint8_t ReadAllRegToTable(void)
 {
@@ -324,48 +447,15 @@
 
 uint8_t WriteFW(uint16_t size)
 {
-    uint8_t  ret = 0;
-    uint8_t  i;
-    uint8_t  reg_sys_cfg;
-    uint8_t  uart_rx_data;
-    uint16_t fw_size = size;
-    uint16_t fw_send = 0;
-
-    ret = WriteOneReg(REG_PW_CTRL, 0x08);
-    ret = WriteOneReg(REG_PW_CTRL, 0x0a);
-    ret = WriteOneReg(REG_MCU_CFG, 0x02);
-    ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg);
-    ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
-    ret = WriteOneReg(REG_CMD, 0x01);
-    ret = WriteOneReg(REG_SIZE, 0x02);
-    ret = WriteOneReg(REG_SCRATCH_PAD_BASE+0x00, 0x00);
-    ret = WriteOneReg(REG_SCRATCH_PAD_BASE+0x01, 0x00);
+    DownloadFirmware(Firmware, size);
+    
+    wait_ms(30);
+    Van_Config_TypeDef cfg;
+    uint8_t cg_para[20];
+    cfg.preSetMode = VAN_HIGH_PRECISION;    
+    InitRanging(&cfg, cg_para);
 
-    while(fw_size >= 32) {
-        ret = WriteOneReg(REG_CMD,0x03);
-        ret = WriteOneReg(REG_SIZE,0x20);
-        for(i=0; i<32; i++) {
-            uart_rx_data = Firmware[fw_send++];
-            ret = WriteOneReg(REG_SCRATCH_PAD_BASE+i, uart_rx_data);
-            wait_us(5);
-        }
-        fw_size -= 32;
-    }
-    if(fw_size > 0) {
-        ret = WriteOneReg(REG_CMD,0x03);
-        ret = WriteOneReg(REG_SIZE,(uint8_t)fw_size);
-        for(i=0; i<fw_size; i++) {
-            uart_rx_data = Firmware[fw_send++];
-            ret = WriteOneReg(REG_SCRATCH_PAD_BASE+i, uart_rx_data);
-            wait_us(5);
-        }
-    }
-    ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0));
-    ret = WriteOneReg(REG_MCU_CFG, 0x03);
-    ret = WriteOneReg(REG_PW_CTRL, 0x02); //reset r_otp_ld_done, is a must
-    ret = WriteOneReg(REG_PW_CTRL, 0x00); //reset r_otp_ld_done, exit low power mode
-
-    return ret;
+    return 0;
 }
 
 uint8_t vangogh_ram_rd(uint8_t tdc)       //UART CMD foramt: CMD-1-byte|TDC-index-1-byte
@@ -401,38 +491,62 @@
     ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
     ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<1)); //restore power         control register
     ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl); //clear pw_ctrl_lp
+      
 
     return ret;
 }
 
-uint8_t OneTimeMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level)
+uint8_t vangogh_ram_rd_ma(void)       //UART CMD foramt: CMD-1-byte|TDC-index-1-byte
 {
     uint8_t ret = 0;
+    uint8_t i;
+    uint8_t j;
     uint8_t reg_pw_ctrl;
-    uint8_t reg_sys_cfg;
+    uint8_t reg_sys_cfg;    
+    uint16_t ram_addr_base = 0x0800;
+
+    ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
+    ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
+    ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
+    ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
+    ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3) | (0x01<<1)); //set otp_ld_done, pw_ctrl_lp
+    ret = WriteOneReg(REG_MCU_CFG, 0x01);
+    ret = WriteOneReg(REG_CMD, 0x01);
+    ret = WriteOneReg(REG_SIZE, 0x02);
+    ret = WriteOneReg(REG_SCRATCH_PAD_BASE+0x00, *((uint8_t*)(&ram_addr_base)  ));
+    ret = WriteOneReg(REG_SCRATCH_PAD_BASE+0x01, *((uint8_t*)(&ram_addr_base)+1));
+    wait_us(100);
+    for(j=0; j<32; j++) {
+        ret = WriteOneReg(REG_CMD,0x05);  //Issue RAM read command
+        ret = WriteOneReg(REG_SIZE,0x20); //Issue RAM read command
+        wait_us(100);
+        for(i=0; i<32; i++) {
+            ret = ReadOneReg(0x0c+i, &histogram[0][32*j + i]);
+        }
+    }
+    ret = WriteOneReg(REG_MCU_CFG, 0x03);
+    ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
+    ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<1)); //restore power         control register
+    ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl); //clear pw_ctrl_lp
+
+    return ret;
+}
+
+uint8_t OneTimeMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level, uint16_t *ref_lsb, uint16_t *ref_milimeter)
+{
+    uint8_t ret = 0;   
     uint32_t timeout = 0;
-    uint8_t flag = 0;
+   
+    Van_Dist_TypeDef result;
 
     int_mark = 1;//One Time Measure
 
-    //ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
-    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
-    //ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
-    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
-    //ret = WriteOneReg(REG_PW_CTRL, (reg_pw_ctrl | (0x01<<3)) & ~(0x01<<1)); //set otp_ld_done, clear pw_ctrl_lp
-    //ret = WriteOneReg(0x08, 0x00);
-    ret = WriteOneReg(REG_CMD, 0x0E);
-    //ret = WriteOneReg(REG_SIZE, 0x00);
+    StartRanging();   
 
     timeout = 1000;
     while(int_mark == 1 && timeout != 0) {
         timeout--;
-        wait_ms(5);
-        //ReadOneReg(0x08, &flag);
-        // if(flag == 0xFF)
-        //{
-        //     int_mark = 0;
-        //  }
+        wait_ms(5);       
     }
 
     if(timeout == 0) {
@@ -440,25 +554,20 @@
 
     } else {
 
+        ReadRangingResult(&result);
+        
         ret = ReadOneReg(0x0d, (uint8_t*)lsb);
         ret = ReadOneReg(0x0e, (uint8_t*)lsb + 1);
-
-        ret = ReadOneReg(0x18, (uint8_t*)milimeter);
-        ret = ReadOneReg(0x19, (uint8_t*)milimeter + 1);
-
-        ret = ReadOneReg(0x26, (uint8_t*)noise_level);
-        ret = ReadOneReg(0x27, (uint8_t*)noise_level + 1);
+        
+        *milimeter = result.millimeter;
+        *peak = result.peak;
+        *noise_level = result.noise;
+        
+        //*ref_lsb = result.confidence;
+        *ref_lsb = result.histcnt;
+        *ref_milimeter = result.dmax;       
 
-        ret = ReadOneReg(0x28, (uint8_t*)peak );
-        ret = ReadOneReg(0x29, (uint8_t*)peak + 1);
-        ret = ReadOneReg(0x2a, (uint8_t*)peak + 2);
-        ret = ReadOneReg(0x2b, (uint8_t*)peak + 3);
-
-    }
-
-    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
-    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<1)); //restore power         control register
-    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl & ~(0x01<<1)); //clear pw_ctrl_lp
+    }   
 
     return ret;
 }
@@ -466,37 +575,16 @@
 uint8_t ContinuousMeasure(void)
 {
     uint8_t ret = 0;
-    uint8_t reg_pw_ctrl;
-    uint8_t reg_sys_cfg;
-
-    int_mark = 2;//Continuous Time Measure
-
-    //ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
-    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
-    //ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
-    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
-    //ret = WriteOneReg(REG_PW_CTRL, (reg_pw_ctrl | (0x01<<3)) & ~(0x01<<1)); //set otp_ld_done, clear pw_ctrl_lp
-    ret = WriteOneReg(REG_CMD, 0x0F);
-    //ret = WriteOneReg(REG_SIZE, 0x00);
-
-    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
+    
+    StartRanging();   
+    continus_ranging = 1; 
 
     return ret;
 }
 
-uint8_t RaadContinuousMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level)
+uint8_t RaadContinuousMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level, uint16_t *ref_lsb, uint16_t *ref_milimeter)
 {
-    uint8_t ret = 0;
-    uint8_t reg_pw_ctrl;
-    uint8_t reg_sys_cfg;
-
-    int_mark = 2;//Continuous Time Measure
-
-    //ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
-    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
-    //ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
-    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
-    //ret = WriteOneReg(REG_PW_CTRL, (reg_pw_ctrl | (0x01<<3)) & ~(0x01<<1)); //set otp_ld_done, clear pw_ctrl_lp
+    uint8_t ret = 0;   
 
     ret = ReadOneReg(0x0d, (uint8_t*)lsb);
     ret = ReadOneReg(0x0e, (uint8_t*)lsb + 1);
@@ -504,15 +592,19 @@
     ret = ReadOneReg(0x18, (uint8_t*)milimeter);
     ret = ReadOneReg(0x19, (uint8_t*)milimeter + 1);
 
+    ret = ReadOneReg(0x20, (uint8_t*)ref_lsb);
+    ret = ReadOneReg(0x21, (uint8_t*)ref_lsb + 1);
+
+    ret = ReadOneReg(0x22, (uint8_t*)ref_milimeter);
+    ret = ReadOneReg(0x23, (uint8_t*)ref_milimeter+1);
+
     ret = ReadOneReg(0x26, (uint8_t*)noise_level);
     ret = ReadOneReg(0x27, (uint8_t*)noise_level + 1);
 
     ret = ReadOneReg(0x28, (uint8_t*)peak );
     ret = ReadOneReg(0x29, (uint8_t*)peak + 1);
     ret = ReadOneReg(0x2a, (uint8_t*)peak + 2);
-    ret = ReadOneReg(0x2b, (uint8_t*)peak + 3);
-
-    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
+    ret = ReadOneReg(0x2b, (uint8_t*)peak + 3);  
 
     return ret;
 }
@@ -520,45 +612,14 @@
 uint8_t StopContinuousMeasure()
 {
     uint8_t ret = 0;
-    uint8_t reg_pw_ctrl;
-    uint8_t reg_sys_cfg;
-
-    int_mark = 0;//Continuous Time Measure
-
-    //ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
-    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
-    //ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
-    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
-    //ret = WriteOneReg(REG_PW_CTRL, (reg_pw_ctrl | (0x01<<3)) & ~(0x01<<1)); //set otp_ld_done, clear pw_ctrl_lp
-    ret = WriteOneReg(REG_CMD, 0x00);
-    ret = WriteOneReg(REG_SIZE, 0x00);
-
-    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
-    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<1)); //restore power         control register
-    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl & ~(0x01<<1)); //clear pw_ctrl_lp
+   
+    continus_ranging = 0;
 
     return ret;
 }
 
 void StoreHistogram(uint16_t histogram_pos, uint16_t histogram_num, uint8_t tdc)
 {
-    /*
-    char file_name[20];
-
-    //sprintf(file_name, "/sd/hist_%d_%d_tdc%d", histogram_pos, histogram_num, tdc);
-    sprintf(file_name, "/sd/hello.txt");
-
-    FILE *fp = fopen(file_name, "w");
-    //for(uint32_t i = 0; i < 1024; i++) {
-    //    fseek(fp,0,SEEK_END);
-    //    fprintf(fp, "%02X ", histogram[tdc][i]);
-    //}
-    fprintf(fp, "hello ");
-
-    fflush(fp);
-    fclose(fp);
-    */
-
     _uart_send_pbuff[0] = histogram_pos;
     _uart_send_pbuff[1] = histogram_pos >> 8;
 
@@ -613,9 +674,7 @@
 
 uint8_t DelayLineTest(uint8_t phase, uint8_t* buf)
 {
-    uint8_t ret = 0;
-    uint32_t timeout = 0;
-
+    
     WriteOneReg(0xE4, phase);
     wait_ms(10);
 
@@ -623,21 +682,22 @@
         WriteOneReg(0xE1, 0x61 | (step << 1));
         wait_ms(10);
 
-        timeout = 50;
-        int_mark = 1;
+        //timeout = 50;
+        //int_mark = 1;
         WriteOneReg(0x0A, 0x0B);
-        wait_ms(10);
-        while(int_mark == 1 && timeout != 0) {
-            timeout--;
-            wait_ms(100);
+        wait_ms(100);
+        //while(int_mark == 1 && timeout != 0) {
+        //    timeout--;
+        //   wait_ms(100);
+        //}
+        //wait_ms(100);
+        for(uint8_t tdc = 0; tdc < 9; tdc++) {
+            ReadOneReg(0x0c + tdc*2, buf + step*18 + tdc*2 + 1);
+            //wait_ms(1);
+            ReadOneReg(0x0c + tdc*2 + 1, buf + step*18 + tdc*2);
+            //wait_ms(1);
         }
-        for(uint8_t tdc = 0; tdc < 9; tdc++) {
-            ret = ReadOneReg(0x0c + tdc*2, buf + step*18 + tdc*2 + 1);
-            wait_ms(1);
-            ret = ReadOneReg(0x0c + tdc*2 + 1, buf + step*18 + tdc*2);
-            wait_ms(1);
-        }
-
+        //wait_ms(100);
     }
 
     return 0;
@@ -649,20 +709,20 @@
     uint32_t timeout = 0;
     memset(buf, 0x00, 10);
 
-    WriteOneReg(0x0C, 0x0C);
-    WriteOneReg(0x0A, 0x0A);
+    //WriteOneReg(0x0C, 0x0C);
+    //WriteOneReg(0x0A, 0x0A);
     wait_ms(1);
 
-    timeout = 10;
+    timeout = 20;
     int_mark = 1;
-    WriteOneReg(0x0A, 0x0B);
+    WriteOneReg(0x0A, 0x0A);
     while(int_mark == 1 && timeout != 0) {
         timeout--;
         wait_ms(100);
     }
     //osDelay(80);
 
-    ret = ReadOneReg(0x0D, buf);
+    ret = ReadOneReg(0x0C, buf);
     ret = ReadOneReg(0xE4, buf + 1);
 
     if(ret != 0)
@@ -675,51 +735,50 @@
 {
     uint8_t ret = 0;
     uint8_t buf[14];
-    
+
     memset(buf, 0x00, 14);
-    
+
     buf[0] |= (pd[5]&0xF);
     buf[0] |= ((pd[6] << 4)&0xF0);
-    
+
     buf[1] |= (pd[7]&0xF);
     buf[1] |= ((pd[8] << 4)&0xF0);
-    
+
     buf[2] |= (pd[9]&0xF);
     buf[2] |= ((pd[10] << 4)&0xF0);
-    
+
     buf[3] |= (pd[11]&0xF);
     buf[3] |= ((pd[12] << 4)&0xF0);
-    
+
     buf[4] |= (pd[13]&0xF);
     buf[4] |= ((pd[14] << 4)&0xF0);
-    
+
     buf[5] |= (pd[15]&0xF);
     buf[5] |= ((pd[16] << 4)&0xF0);
-    
+
     buf[6] |= (pd[17]&0xF);
     buf[6] |= ((pd[18] << 4)&0xF0);
-    
+
     buf[7] |= (pd[19]&0xF);
     buf[7] |= ((pd[20] << 4)&0xF0);
-    
+
     buf[8] = 0x00;// Fisrt 16LSB cut off
     buf[9] = 0xFF;
-    
+
     buf[10] = 0x01;// Ref PAD switch
-    
+
     buf[11] = 0x1F;// TDC Resolution 1F = 31
-    
+
     buf[12] = 0x0C;// Length
     buf[13] = 0x09;//CMD
-    
-    for(uint8_t i = 0; i < 12; i++)
-    {
+
+    for(uint8_t i = 0; i < 12; i++) {
         WriteOneReg(0x0C + i, buf[i]);
     }
-    
+
     WriteOneReg(0x0B, 0x0C);
     WriteOneReg(0x0A, 0x09);
-    
+
     return 0;
 }
 
@@ -727,103 +786,99 @@
 {
     uint8_t ret = 0;
     uint8_t value = 0, result = 0;
-    
+
     TRIM_EN = 1;
 
     //==================First BAND=====================//
     ChipInitReset();
-    ret = WriteOneReg(0xED    , 0x03);
-    ret = WriteOneReg(0xEC    , 0x36);
-    ret = WriteOneReg(0x3D    , 0xC0);
-    ret = WriteOneReg(0xE8    , 0x00);
-    ret = WriteOneReg(0xEA    , 0x3F);
-    ret = WriteOneReg(0x3D    , 0xC8);
-    ret = WriteOneReg(0xED    , 0x83);
+    ret = WriteOneReg(0xED, 0x03);
+    ret = WriteOneReg(0xEC, 0x36);
+    ret = WriteOneReg(0x3D, 0xC0);
+    ret = WriteOneReg(0xE8, 0x00);
+    ret = WriteOneReg(0xEA, 0x3F);
+    ret = WriteOneReg(0x3D, 0xC8);
+    ret = WriteOneReg(0xED, 0x83);
 
     wait_ms(1000);
 
     ret = ReadOneReg(0xFA, &value);
     ret = ReadOneReg(0xFB, &result);
 
-    if((result&0xC0) == 0xC0)
-    {
+    if((result&0xC0) == 0xC0) {
         *rco = value;
-         ChipInitReset();
-         TRIM_EN = 0;
+        ChipInitReset();
+        TRIM_EN = 0;
         return 0;
     }
 
     //==================Second BAND=====================//
     ChipInitReset();
-    ret = WriteOneReg(0xED    , 0x03);
-    ret = WriteOneReg(0xEC    , 0x36);
-    ret = WriteOneReg(0x3D    , 0xC0);
-    ret = WriteOneReg(0xE8    , 0x00);
-    ret = WriteOneReg(0xEA    , 0x7F);
-    ret = WriteOneReg(0xEB    , 0x40);
-    ret = WriteOneReg(0x3D    , 0xC8);
-    ret = WriteOneReg(0xED    , 0x83);
+    ret = WriteOneReg(0xED, 0x03);
+    ret = WriteOneReg(0xEC, 0x36);
+    ret = WriteOneReg(0x3D, 0xC0);
+    ret = WriteOneReg(0xE8, 0x00);
+    ret = WriteOneReg(0xEA, 0x7F);
+    ret = WriteOneReg(0xEB, 0x40);
+    ret = WriteOneReg(0x3D, 0xC8);
+    ret = WriteOneReg(0xED, 0x83);
 
     wait_ms(1000);
 
     ret = ReadOneReg(0xFA, &value);
     ret = ReadOneReg(0xFB, &result);
 
-    if((result&0xC0) == 0xC0)
-    {
+    if((result&0xC0) == 0xC0) {
         *rco = value;
-         ChipInitReset();
-         TRIM_EN = 0;
+        ChipInitReset();
+        TRIM_EN = 0;
         return 0;
-    }    
+    }
 
     //==================Third BAND=====================//
     ChipInitReset();
-    ret = WriteOneReg(0xED    , 0x03);
-    ret = WriteOneReg(0xEC    , 0x36);
-    ret = WriteOneReg(0x3D    , 0xC0);
-    ret = WriteOneReg(0xE8    , 0x00);
-    ret = WriteOneReg(0xEA    , 0xBF);
-    ret = WriteOneReg(0xEB    , 0x80);
-    ret = WriteOneReg(0x3D    , 0xC8);
-    ret = WriteOneReg(0xED    , 0x83);
+    ret = WriteOneReg(0xED, 0x03);
+    ret = WriteOneReg(0xEC, 0x36);
+    ret = WriteOneReg(0x3D, 0xC0);
+    ret = WriteOneReg(0xE8, 0x00);
+    ret = WriteOneReg(0xEA, 0xBF);
+    ret = WriteOneReg(0xEB, 0x80);
+    ret = WriteOneReg(0x3D, 0xC8);
+    ret = WriteOneReg(0xED, 0x83);
 
     wait_ms(1000);
 
     ret = ReadOneReg(0xFA, &value);
     ret = ReadOneReg(0xFB, &result);
 
-    if((result&0xC0) == 0xC0)
-    {
+    if((result&0xC0) == 0xC0) {
         *rco = value;
-         ChipInitReset();
-         TRIM_EN = 0;
+        ChipInitReset();
+        TRIM_EN = 0;
         return 0;
-    }    
+    }
 
     //==================Fourth BAND=====================//
     ChipInitReset();
-    ret = WriteOneReg(0xED    , 0x03);
-    ret = WriteOneReg(0xEC    , 0x36);
-    ret = WriteOneReg(0x3D    , 0xC0);
-    ret = WriteOneReg(0xE8    , 0x00);
-    ret = WriteOneReg(0xEA    , 0xFF);
-    ret = WriteOneReg(0xEB    , 0xC0);
-    ret = WriteOneReg(0x3D    , 0xC8);
-    ret = WriteOneReg(0xED    , 0x83);
+    ret = WriteOneReg(0xED, 0x03);
+    ret = WriteOneReg(0xEC, 0x36);
+    ret = WriteOneReg(0x3D, 0xC0);
+    ret = WriteOneReg(0xE8, 0x00);
+    ret = WriteOneReg(0xEA, 0xFF);
+    ret = WriteOneReg(0xEB, 0xC0);
+    ret = WriteOneReg(0x3D, 0xC8);
+    ret = WriteOneReg(0xED, 0x83);
 
     wait_ms(1000);
 
     ret = ReadOneReg(0xFA, &value);
     ret = ReadOneReg(0xFB, &result);
 
-    if((result&0xC0) == 0xC0)
-    {
+    if((result&0xC0) == 0xC0) {
         *rco = value;
-         ChipInitReset();
-         TRIM_EN = 0;
+        ChipInitReset();
+        TRIM_EN = 0;
         return 0;
-    }    
+    }
 
     ChipInitReset();
     TRIM_EN = 0;
@@ -834,8 +889,8 @@
 {
     uint8_t ret = 0;
     uint32_t timeout = 0;
-        
-    WriteOneReg(0xC0, 0x00);    
+
+    WriteOneReg(0xC0, 0x00);
     wait_ms(1);
 
     timeout = 600;
@@ -845,8 +900,8 @@
         timeout--;
         wait_ms(100);
     }
-    //osDelay(80);   
-    
+    //osDelay(80);
+
     ret = ReadOneReg(0xC0, bvd);
 
     if(timeout == 0)
@@ -857,12 +912,301 @@
 
 uint8_t Pixel_Enable(uint8_t *buf)
 {
-    for(uint8_t i = 0; i < 18; i++)
+    for(uint8_t i = 0; i < 18; i++) {
+        WriteOneReg(0xC2 + i, buf[i]);
+    }
+
+    return 0;
+}
+
+uint8_t MP_Measure(uint8_t mp, uint16_t *mp_tof, uint32_t* mp_peak)
+{
+    uint8_t ret = 0;
+    uint16_t lsb, mili, ref_lsb, ref_mili, noise_level;
+    uint32_t peak;
+    
+    uint32_t total_peak = 0, total_tof = 0;    
+    
+    switch(mp)   
     {
-        WriteOneReg(0xC2 + i, buf[i]);  
+        case 0:
+        case 1:
+        case 4:
+        case 5:
+            WriteOneReg(0xD7, 0x00); 
+            Pixel_Enable(pixel_map[mp]);
+            for(uint8_t j = 0; j < 16; j++)   
+            {
+                ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level, &ref_lsb, &ref_mili);//////
+                if(ret != 0)
+                {
+                    return 1;
+                }
+                
+                total_peak += peak;
+                total_tof += mili;
+            }
+            
+            *mp_tof = total_tof / 16;
+            *mp_peak = total_peak / 16;            
+        break;
+        
+        case 2:
+        case 3:
+        case 6:
+        case 7:
+            WriteOneReg(0xD7, 0x01); 
+            Pixel_Enable(pixel_map[mp]);
+            for(uint8_t j = 0; j < 16; j++)   
+            {
+                ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level, &ref_lsb, &ref_mili);//////
+                if(ret != 0)
+                {
+                    return 1;
+                }
+                
+                total_peak += peak;
+                total_tof += mili;
+            }
+            
+            *mp_tof = total_tof / 16;
+            *mp_peak = total_peak / 16;            
+        break;
+        
+        case 8:
+        case 9:
+        case 12:
+        case 13:
+            WriteOneReg(0xD7, 0x02); 
+            Pixel_Enable(pixel_map[mp]);
+            for(uint8_t j = 0; j < 16; j++)   
+            {
+                ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level, &ref_lsb, &ref_mili);//////
+                if(ret != 0)
+                {
+                    return 1;
+                }
+                
+                total_peak += peak;
+                total_tof += mili;
+            }
+            
+            *mp_tof = total_tof / 16;
+            *mp_peak = total_peak / 16;            
+        break;
+        
+        case 10:
+        case 11:
+        case 14:
+        case 15:
+            WriteOneReg(0xD7, 0x03); 
+            Pixel_Enable(pixel_map[mp]);
+            for(uint8_t j = 0; j < 16; j++)   
+            {
+                ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level, &ref_lsb, &ref_mili);//////
+                if(ret != 0)
+                {
+                    return 1;
+                }
+                
+                total_peak += peak;
+                total_tof += mili;
+            }
+            
+            *mp_tof = total_tof / 16;
+            *mp_peak = total_peak / 16;            
+        break;
     }
     
     return 0;
 }
 
+uint8_t MP_Slect(uint8_t *buf)
+{
+    uint32_t sum3x3[4];
+    uint32_t tmp = 0;
+    
+    uint16_t tof;
+    uint32_t peak;
+    
+    uint32_t tof_buf[16], peak_buf[16];
+        
+    for(uint8_t i = 0; i < 16; i++)
+    {
+        MP_Measure(i, &tof, &peak);
+        tof_buf[i] = tof;
+        peak_buf[i] = peak;
+    }    
+    
+    sum3x3[0] = peak_buf[0] + peak_buf[1] + peak_buf[2]
+              + peak_buf[4] + peak_buf[5] + peak_buf[6]
+              + peak_buf[8] + peak_buf[9] + peak_buf[10];
+              
+    sum3x3[1] = peak_buf[1] + peak_buf[2] + peak_buf[3]
+              + peak_buf[5] + peak_buf[6] + peak_buf[7]
+              + peak_buf[9] + peak_buf[10] + peak_buf[11];
+              
+    sum3x3[2] = peak_buf[4] + peak_buf[5] + peak_buf[6]
+              + peak_buf[8] + peak_buf[9] + peak_buf[10]
+              + peak_buf[12] + peak_buf[13] + peak_buf[14];
+              
+    sum3x3[3] = peak_buf[5] + peak_buf[6] + peak_buf[7]
+              + peak_buf[9] + peak_buf[10] + peak_buf[11]
+              + peak_buf[13] + peak_buf[14] + peak_buf[15];          
+              
+    for(uint8_t j = 0; j < 4; j++)
+    {
+        if(sum3x3[j] >= tmp)
+        {
+            tmp = sum3x3[j];
+            buf[0] = j;
+        }
+    }
+    
+    for(uint8_t k = 0; k < 16; k++)
+    {
+        memcpy(&buf[1+k*8], &tof_buf[k], 4);
+        memcpy(&buf[1+k*8 + 4], &peak_buf[k], 4);
+    }
+    
+    return 0;
+}
 
+uint8_t OTP_WritePart(uint8_t base, uint8_t len, uint8_t *buf)
+{
+    uint32_t timeout = 0;
+    uint8_t flag = 0;    
+    
+    if(len > 29)
+        return 2;
+    
+    WriteOneReg(0x0C, 0x02); 
+    WriteOneReg(0x0D, len); 
+    WriteOneReg(0x0E, base); 
+    
+    for(uint8_t i = 0; i < len; i++)
+    {
+        WriteOneReg(0x0F + i, buf[i]); 
+    }
+    
+    timeout = 2000;
+    WriteOneReg(0x08, 0x00); 
+    WriteOneReg(0x0A, 0x09);     
+    while(timeout != 0) {
+        timeout--;
+        wait_ms(1);
+        ReadOneReg(0x08, &flag);
+        if(flag == 0xFF)
+        {
+            break;
+        }
+    }
+    if(timeout == 0)
+    {
+        return 1;
+    }
+    
+    HIGH_EN = 1;
+    timeout = 1000;
+    while(timeout != 0) {
+        timeout--;
+        wait_ms(1);
+        ReadOneReg(0x08, &flag);
+        if(flag == 0x00)
+        {
+            break;
+        }
+    }
+    HIGH_EN = 0;
+    if(timeout == 0)
+    {
+        return 1;
+    }
+    
+    return 0;
+}
+
+uint8_t OTP_Write(uint8_t *buf)
+{    
+    uint8_t i = 0;
+    uint8_t ret = 0;
+    uint8_t base = buf[0];
+    uint8_t len = buf[1];   
+    
+    while(len > 29) 
+    {
+        ret = OTP_WritePart(base + i*29, 29, &buf[2 + i*29]);
+        if(ret != 0)
+        {
+            return ret;
+        }
+        len -= 29;
+        i++;
+        
+        wait_ms(1000);
+    }
+    if(len > 0)
+    {
+        ret = OTP_WritePart(base + i*29, len, &buf[2 + i*29]);
+        if(ret != 0)
+        {
+            return ret;
+        }
+    }
+    
+    return 0;
+}
+
+uint8_t OTP_ReadPart(uint8_t base, uint8_t len, uint8_t* out)
+{
+    if(len > 29)
+        return 2;    
+
+    WriteOneReg(0x0C, 0x03); 
+    WriteOneReg(0x0D, len); 
+    WriteOneReg(0x0E, base); 
+    WriteOneReg(0x0A, 0x09);     
+    
+    wait_ms(100);
+    
+    for(uint8_t i = 0; i < len; i++)
+    {
+        ReadOneReg(0x0F + i, &out[i]); 
+    }  
+    
+    return 0;
+}
+
+uint8_t OTP_Read(uint8_t* in, uint8_t* out)
+{
+    uint8_t ret = 0;
+    uint8_t i = 0;
+    
+    uint8_t base = in[0];
+    uint8_t len = in[1];   
+    
+    while(len > 29) 
+    {
+        ret = OTP_ReadPart(base + i*29, 29, &out[2 + i*29]);
+        if(ret != 0)
+        {
+            return ret;
+        }
+        len -= 29;
+        i++;
+    }
+    if(len > 0)
+    {
+        ret = OTP_ReadPart(base + i*29, len, &out[2 + i*29]);
+        if(ret != 0)
+        {
+            return ret;
+        }
+    }
+    
+    out[0] = base;
+    out[1] = len;
+    
+    return 0;
+}
+

--- a/DUT_RegConfig.h	Mon Jun 22 05:27:48 2020 +0000
+++ b/DUT_RegConfig.h	Tue Jul 28 01:40:05 2020 +0000
@@ -39,15 +39,17 @@
 
 void ContinuousMeasureReport();
 void HistogramReport();
+void AutoSingleMeasurment();
 
 uint8_t WriteOneReg(uint8_t addr, uint8_t data);
 uint8_t ReadOneReg(uint8_t addr, uint8_t *data);
 uint8_t ReadAllRegToTable(void);
 uint8_t WriteFW(uint16_t size);
 uint8_t vangogh_ram_rd(uint8_t tdc);
-uint8_t OneTimeMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level);
+uint8_t vangogh_ram_rd_ma(void);
+uint8_t OneTimeMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level, uint16_t *ref_lsb, uint16_t *ref_milimeter);
 uint8_t ContinuousMeasure(void);
-uint8_t RaadContinuousMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level);
+uint8_t RaadContinuousMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level, uint16_t *ref_lsb, uint16_t *ref_milimeter);
 uint8_t StopContinuousMeasure(void);
 uint8_t DCRTest(uint8_t vspad, uint8_t test_time);
 uint8_t DelayLineTest(uint8_t phase, uint8_t* buf);
@@ -56,7 +58,9 @@
 uint8_t RCO_Trim(uint8_t *rco);
 uint8_t BVD_Trim(uint8_t *bvd);
 uint8_t Pixel_Enable(uint8_t *buf);
-
+uint8_t MP_Slect(uint8_t *buf);
+uint8_t OTP_Write(uint8_t *buf);
+uint8_t OTP_Read(uint8_t* in, uint8_t* out);
 
 void StoreHistogram(uint16_t histogram_pos, uint16_t histogram_num, uint8_t tdc);
 

--- a/ServoRun.cpp	Mon Jun 22 05:27:48 2020 +0000
+++ b/ServoRun.cpp	Tue Jul 28 01:40:05 2020 +0000
@@ -10,7 +10,7 @@
 extern DigitalOut PULSE;
 extern DigitalOut DI;
 
-uint32_t servo_run = 0;
+float servo_run = 0;
 
 void ServoRunThread(void)
 {    
@@ -20,7 +20,8 @@
     {    
         if(servo_run)
         {
-            step = servo_run * 40;
+            step = servo_run * 40;//1M Stage
+            //step = servo_run * 30.30303030;//2M Stage
             while(step--)
             {
                 PULSE = 1;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_API.cpp	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,198 @@
+#include "Van_API.h"
+#include "Van_Algorithm.h"
+
+uint16_t GetAPIVersion(void)
+{
+    return APIversion;
+}
+
+Van_Status InitRanging(Van_Config_TypeDef *cfg, uint8_t *cg_para)
+{
+    Van_Status ret = VAN_OK;
+
+    ret = SetRangingMode(cfg);//设置测距模式
+    VAN_CHECK_RET(ret);
+
+#if(config_USE_CG_Correction) 
+    ret = WriteCGPara(cg_para);//下传 CoverGlass校正参数
+    VAN_CHECK_RET(ret);
+#endif
+    
+#if(config_CAL_DMAX || config_CAL_COFF)
+    ret = InitAlgorithmParameter(cfg);
+    VAN_CHECK_RET(ret);
+#endif
+    
+    return VAN_OK;
+}
+
+Van_Status StartRanging(void)
+{    
+    Van_Status ret = VAN_OK;
+    ret = ClearInterrupt();
+    VAN_CHECK_RET(ret);
+    ret = WriteCommand(VAN_START_RANG_CMD);
+    VAN_CHECK_RET(ret);
+
+    return VAN_OK; 
+}
+
+Van_Status ReadRangingResult(Van_Dist_TypeDef *result)
+{
+    Van_Status ret = VAN_OK;
+    ret = GetRawRangingData(result);
+    VAN_CHECK_RET(ret);
+
+    //这里要进行算法计算
+#if(config_CAL_COFF || config_CAL_DMAX)    
+    ret = RangingResultAnalysis(result);
+    VAN_CHECK_RET(ret);
+    //result->confidence = 1234;
+#endif
+    
+    return VAN_OK;
+}
+
+Van_Status GetIntStatus(uint8_t *int_status)
+{
+    return GetAndClearInterrupt(int_status);
+}
+
+Van_Status PollingGetRangingResult(Van_Dist_TypeDef *result, uint8_t next_actiong)
+{
+    Van_Status ret = VAN_OK;
+    uint8_t int_status = 0;
+
+    ret = GetAndClearInterrupt(&int_status);
+    VAN_CHECK_RET(ret);
+
+    if((int_status & 0x01) == 0x00)
+    {
+        return VAN_RANGING;
+    }
+
+    ret = ReadRangingResult(result);
+    VAN_CHECK_RET(ret);
+
+    if(next_actiong == VAN_GO_ON)
+    {
+        ret = WriteCommand(VAN_START_RANG_CMD);
+        VAN_CHECK_RET(ret);
+    }
+    
+    return VAN_OK;
+}
+
+Van_Status GetPileUpPara(Van_PileUp_TypeDef *pile_up_para)
+{
+    return VAN_OK;
+}
+
+Van_Status SetRangingMode(Van_Config_TypeDef *cfg)
+{
+    Van_Status ret = VAN_OK;
+
+    if(cfg->preSetMode == VAN_ADVANCED)
+    {
+        ret = WriteCommand_SW(VAN_USER_CFG_CMD, VAN_CFG_SUBCMD, 0x00, 0x20, (uint8_t*)cfg);
+        VAN_CHECK_RET(ret);
+    }
+    else
+    {
+        ret = WriteCommand_SW(VAN_USER_CFG_CMD, VAN_CFG_SUBCMD, 0x00, 0x01, (uint8_t*)cfg);
+        VAN_CHECK_RET(ret);
+    }
+
+    return VAN_OK;
+}
+
+Van_Status WriteCGPara(uint8_t *cg_para)
+{
+    //这里以后会增加传递CoverGlass的接口
+
+    //
+    return VAN_OK;
+}
+
+Van_Status SetPileUpPara(Van_PileUp_TypeDef *pile_up_para)
+{
+    //预留的设置 PileUp计算参数的接口, 当前PileUp校正在芯片内部完成
+
+    //
+    return VAN_OK;
+}
+
+//这个函数对固件下载做了封装，如果用户的平台不支持8KB的缓冲区，则可以自行分开
+//实现下面函数的内容，分段写入固件，每段32 Byte。
+//调用次序：
+//  1，WriteFirmwarePreConfig()
+//  2, 多次调用WriteFirmware32Byte，直到写完全部固件
+//  3, WriteFirmwarePostConfig()
+Van_Status DownloadFirmware(uint8_t *buf, uint32_t size)
+{
+    Van_Status ret = VAN_OK;
+    uint8_t page = 0;
+
+    ret = WriteFirmwarePreConfig();
+    VAN_CHECK_RET(ret);
+
+    while(size >= 32)
+    {
+        ret = WriteFirmware32Byte(32, buf + page * 32);
+        VAN_CHECK_RET(ret);
+
+        size -= 32;
+        page++;
+    }
+
+    if(size > 0)
+    {
+        ret = WriteFirmware32Byte(size, buf + page * 32);
+        VAN_CHECK_RET(ret);
+    }
+
+    ret = WriteFirmwarePostConfig();
+    VAN_CHECK_RET(ret);
+
+    return VAN_OK;
+}
+
+//下面这个函数实现了从OTP中读取数据，这个函数是Block的，用户需要自行实现在Van_Platform.c中
+//的毫秒延时函数Van_Delay_ms()，如果用户自行控制延时，则可以分开执行PreReadOTPData()和ReadOTPDataPage()
+//两个函数调用之间间隔5ms以上
+Van_Status ReadOTPData(uint8_t addr, uint8_t *buf, uint8_t len)
+{
+    Van_Status ret = VAN_OK;
+
+    ret = PreReadOTPData(addr, len);
+    VAN_CHECK_RET(ret);
+
+    Van_Delay_ms(5);
+
+    ret = ReadOTPDataPage(len, buf);
+    VAN_CHECK_RET(ret);
+
+    return VAN_OK;
+}
+
+
+#if(config_CAL_DMAX || config_CAL_COFF)
+
+Van_Status InitAlgorithmParameter(Van_Config_TypeDef *cfg)
+{
+    Van_Status ret = VAN_OK;
+    uint8_t buf[32];
+
+    ret = ReadOTPData(0x61, buf, 10);
+    VAN_CHECK_RET(ret);    
+
+    //这里调用算法接口，将算法参数传递至Van_Algorithm.c中，如果用户在其他层次实现算法
+    //则用户自行在这里添加保存或传递算法参数的代码
+    ret = TransferAlgorithmParameter(cfg, buf);
+    VAN_CHECK_RET(ret);
+
+    //
+    return VAN_OK;
+}
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_API.h	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,36 @@
+#ifndef __VAN_API_H__
+#define __VAN_API_H__
+
+#include "Van_Defines.h"
+#include "Van_ChipInterface.h"
+
+uint16_t GetAPIVersion(void);
+
+Van_Status InitRanging(Van_Config_TypeDef *cfg, uint8_t *cg_para);
+
+Van_Status StartRanging(void);
+
+Van_Status ReadRangingResult(Van_Dist_TypeDef *result);
+
+Van_Status GetIntStatus(uint8_t *int_status);
+
+Van_Status PollingGetRangingResult(Van_Dist_TypeDef *result, uint8_t next_actiong);
+
+Van_Status GetPileUpPara(Van_PileUp_TypeDef *pile_up_para);
+
+Van_Status SetRangingMode(Van_Config_TypeDef *cfg);
+
+Van_Status WriteCGPara(uint8_t *cg_para);
+
+Van_Status SetPileUpPara(Van_PileUp_TypeDef *pile_up_para);
+
+Van_Status DownloadFirmware(uint8_t *buf, uint32_t size);
+
+Van_Status ReadOTPData(uint8_t addr, uint8_t *buf, uint8_t len);
+
+Van_Status InitAlgorithmParameter(Van_Config_TypeDef *cfg);
+
+
+#endif
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_Algorithm.cpp	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,98 @@
+#include "Van_Algorithm.h"
+#include <math.h>
+
+#if(config_CAL_COFF || config_CAL_DMAX)
+
+#define IT0 65535.0
+#define rou 0.5
+#define rou0 0.88
+#define Nma0 126
+#define Sigmama0 1360
+#define Tdead 20
+#define R0 400
+
+float IT = 65535;
+float P0 = 0;
+
+#if(config_CAL_COFF)
+Van_Status Cal_Confidence(Van_Dist_TypeDef *result)
+{
+    float Nma = Nma0*result->noise;
+    float SigmaNma = sqrt((float)Sigmama0*result->noise);
+    float tmp;
+
+    if(result->peak < (Nma + 3*SigmaNma))
+    {
+        result->confidence = 0;
+    }
+    else if(result->peak > (Nma + 6*SigmaNma))
+    {
+        result->confidence = 63;
+    }
+    else
+    {
+        tmp = (6*SigmaNma - result->peak + Nma)/(3*SigmaNma);
+        tmp = 64 - 64*tmp*tmp;
+        result->confidence = tmp;
+    }    
+
+    return VAN_OK;
+}
+#endif
+
+#if(config_CAL_COFF)
+Van_Status Cal_Dmax(Van_Dist_TypeDef *result)
+{
+    float Atte = 1 - result->noise * Tdead / IT;
+    float SigmaNma = sqrt((float)Sigmama0*result->noise);
+    float den, num;
+
+    num = Atte * P0 * rou * R0 * R0 * IT * 9;
+    den = 6 * rou0 * SigmaNma * IT0 * result->histcnt;
+
+    result->dmax = sqrt(num/den);
+
+    return VAN_OK;
+}
+#endif
+
+Van_Status RangingResultAnalysis(Van_Dist_TypeDef *result) 
+{
+    Van_Status ret = VAN_OK;
+
+#if(config_CAL_COFF)
+    ret = Cal_Confidence(result);
+    VAN_CHECK_RET(ret);
+#endif
+
+#if(config_CAL_DMAX)
+    ret = Cal_Dmax(result);
+    VAN_CHECK_RET(ret);
+#endif
+
+    return VAN_OK;
+}
+
+Van_Status TransferAlgorithmParameter(Van_Config_TypeDef *cfg, uint8_t *buf)
+{
+    //传递积分次数
+    if(cfg->preSetMode == VAN_HIGH_PRECISION || cfg->preSetMode == VAN_LONG_DISTANCE)
+        IT = 65535.0;
+    else if(cfg->preSetMode == VAN_HIGH_SPEED)
+        IT = 20479.0;
+    else if(cfg->preSetMode == VAN_ANTI_SUNLIGHT)
+        IT = 131071.0;
+    else
+    {
+        IT = cfg->integalCnt[0] + cfg->integalCnt[1]*0x100 + cfg->integalCnt[2]*0x10000;
+    }
+    
+    //传递标定参数
+    P0 = buf[0] + buf[1]*0x100 + buf[2]*0x10000;
+
+    return VAN_OK;
+}
+
+#endif
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_Algorithm.h	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,12 @@
+#ifndef __VAN_ALGORITHM_H__
+#define __VAN_ALGORITHM_H__
+
+#include "Van_Defines.h"
+
+Van_Status RangingResultAnalysis(Van_Dist_TypeDef *result);
+Van_Status TransferAlgorithmParameter(Van_Config_TypeDef *cfg, uint8_t *buf);
+
+
+#endif
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_ChipInterface.cpp	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,183 @@
+#include "Van_ChipInterface.h"
+
+Van_Status WriteOneReg(uint8_t addr, uint8_t value)
+{
+    return I2C_WriteXBytes(addr, &value, 1);
+}
+
+Van_Status ReadOneReg(uint8_t addr, uint8_t *value)
+{
+    return I2C_ReadXBytes(addr, value, 1);
+}
+
+Van_Status WriteCommand(uint8_t cmd)
+{
+    return WriteOneReg(VAN_REG_CMD, cmd);
+}
+
+Van_Status WriteCommand_SW(uint8_t cmd, uint8_t subcmd, uint8_t start, uint8_t len, uint8_t *data)
+{
+    Van_Status ret = VAN_OK;
+    uint8_t headbuf[3];
+    
+    headbuf[0] = subcmd;
+    headbuf[1] = len;
+    headbuf[2] = start;
+    
+    if(NULL != data)
+    {
+        ret = I2C_WriteXBytes(VAN_REG_SCRATCH_PAD_BASE + 3, data, len);
+        VAN_CHECK_RET(ret);
+    }   
+    
+    ret = I2C_WriteXBytes(VAN_REG_SCRATCH_PAD_BASE, headbuf, 3);
+    VAN_CHECK_RET(ret);
+
+    ret =  WriteCommand(cmd);
+    VAN_CHECK_RET(ret);
+
+    return VAN_OK; 
+}
+
+Van_Status WriteCommand_HW(uint8_t cmd, uint8_t len, uint8_t *data)
+{
+    Van_Status ret = VAN_OK;
+
+    uint8_t cmd_size[2];
+
+    cmd_size[0] = cmd;
+    cmd_size[1] = len;
+    
+    I2C_WriteXBytes(VAN_REG_CMD, cmd_size, 2);
+    VAN_CHECK_RET(ret);
+
+    if(NULL != data)
+    {
+        ret = I2C_WriteXBytes(VAN_REG_SCRATCH_PAD_BASE, data, len);
+        VAN_CHECK_RET(ret);
+    }
+    
+    return VAN_OK; 
+}
+
+Van_Status WriteFirmwarePreConfig(void)
+{
+    Van_Status ret = VAN_OK;
+    uint8_t reg_sys_cfg = 0;
+
+    ret = WriteOneReg(VAN_REG_PW_CTRL, 0x08);
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_PW_CTRL, 0x0a);
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_MCU_CFG, 0x02);
+    VAN_CHECK_RET(ret);  
+    ret = ReadOneReg (VAN_REG_SYS_CFG, &reg_sys_cfg);
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_CMD, 0x01);
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_SIZE, 0x02);
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_SCRATCH_PAD_BASE+0x00, 0x00);
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_SCRATCH_PAD_BASE+0x01, 0x00);
+    VAN_CHECK_RET(ret);  
+
+    return VAN_OK;
+}
+
+Van_Status WriteFirmwarePostConfig(void)
+{
+    Van_Status ret = VAN_OK;
+
+    ret = WriteOneReg(VAN_REG_SYS_CFG, 0x04);
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_MCU_CFG, 0x03);
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_PW_CTRL, 0x02); 
+    VAN_CHECK_RET(ret);  
+    ret = WriteOneReg(VAN_REG_PW_CTRL, 0x00); 
+    VAN_CHECK_RET(ret);  
+
+    return VAN_OK;
+}
+
+Van_Status WriteFirmware32Byte(uint8_t len, uint8_t *data)
+{
+    Van_Status ret = VAN_OK;
+
+    if(len > 32)
+        return VAN_ERROR;
+
+    uint8_t cmd_size[2];
+    uint8_t i = 0;
+
+    cmd_size[0] = VAN_WRITEFW_CMD;
+    cmd_size[1] = len;
+    
+    I2C_WriteXBytes(VAN_REG_CMD, cmd_size, 2);
+    VAN_CHECK_RET(ret);   
+    
+    ret = I2C_WriteXBytes(VAN_REG_SCRATCH_PAD_BASE, data, len);
+    VAN_CHECK_RET(ret);
+
+    return VAN_OK; 
+}
+
+Van_Status GetRawRangingData(Van_Dist_TypeDef *result)
+{
+    Van_Status ret = VAN_OK;
+
+    uint8_t buf[32];
+
+    ret = I2C_ReadXBytes(VAN_REG_SCRATCH_PAD_BASE, buf, 32);
+    VAN_CHECK_RET(ret);
+
+    result->millimeter = *(uint16_t*)(buf + 12);
+    result->peak       = *(uint32_t*)(buf + 28);
+    result->noise      = *(uint16_t*)(buf + 26);
+    result->histcnt    = *(uint8_t*)(buf + 25);
+    result->retCode    = VAN_RAW_DATA;
+
+    return VAN_OK;
+}
+
+Van_Status GetAndClearInterrupt(uint8_t *status)
+{
+    return ReadOneReg(VAN_RET_INT_STATUS, status);
+}
+
+Van_Status ClearInterrupt(void)
+{
+    uint8_t tmp;
+    return ReadOneReg(VAN_RET_INT_STATUS, &tmp);
+}
+
+Van_Status PreReadOTPData(uint8_t start_addr, uint8_t len)
+{
+    Van_Status ret = VAN_OK;
+
+    if(len > 29)
+        return VAN_ERROR;
+
+    ret = WriteCommand_SW(VAN_USER_CFG_CMD, VAN_OTPR_SUBCMD, start_addr, len, NULL);
+    VAN_CHECK_RET(ret);
+
+    return VAN_OK;
+}
+
+Van_Status ReadOTPDataPage(uint8_t len, uint8_t *buf)
+{
+    Van_Status ret = VAN_OK;
+
+    if(len > 29)
+        return VAN_ERROR;
+
+    ret = I2C_ReadXBytes(VAN_REG_SCRATCH_PAD_BASE + 3, buf, len);
+    VAN_CHECK_RET(ret);
+
+    return VAN_OK;
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_ChipInterface.h	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,25 @@
+#ifndef __VAN_CHIPINTERFACE_H__
+#define __VAN_CHIPINTERFACE_H__
+
+#include "Van_Defines.h"
+#include "Van_UserPlatform.h"
+
+Van_Status WriteCommand(uint8_t cmd);
+Van_Status WriteCommand_SW(uint8_t cmd, uint8_t subcmd, uint8_t start, uint8_t len, uint8_t *data);
+Van_Status WriteCommand_HW(uint8_t cmd, uint8_t len, uint8_t *data);
+
+
+Van_Status WriteFirmware32Byte(uint8_t len, uint8_t *data);
+Van_Status WriteFirmwarePostConfig(void);
+Van_Status WriteFirmwarePreConfig(void);
+
+Van_Status GetRawRangingData(Van_Dist_TypeDef *result);
+Van_Status GetAndClearInterrupt(uint8_t *status);
+Van_Status ClearInterrupt(void);
+
+Van_Status PreReadOTPData(uint8_t start_addr, uint8_t len);
+Van_Status ReadOTPDataPage(uint8_t len, uint8_t *buf);
+
+#endif
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_Config.h	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,13 @@
+#ifndef __VAN_CONFIG_H__
+#define __VAN_CONFIG_H__
+
+#define APIversion                   0x0200
+
+#define config_USE_CG_Correction     1
+#define config_USE_PileUp_Correction 1
+#define config_CAL_COFF              1
+#define config_CAL_DMAX              1
+
+#endif
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_Defines.h	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,108 @@
+#ifndef __VAN_DEFINES_H__
+#define __VAN_DEFINES_H__
+
+#include "mbed.h"
+#include "max32630fthr.h"
+//#include "Van_Types.h"
+#include "Van_Config.h"
+
+#define VAN_DEVICE_ADDR          0xD8
+
+#define VAN_REG_MCU_CFG          0x00
+#define VAN_RET_INT_STATUS       0x03
+#define VAN_REG_SYS_CFG          0x01
+#define VAN_REG_PW_CTRL          0x07
+#define VAN_REG_CMD              0x0a
+#define VAN_REG_SIZE             0x0b
+#define VAN_REG_SCRATCH_PAD_BASE 0x0c 
+
+#define VAN_WRITEFW_CMD          0x03
+#define VAN_USER_CFG_CMD         0x09
+#define VAN_START_RANG_CMD       0x0E
+
+#define VAN_CFG_SUBCMD           0x01
+#define VAN_OTPW_SUBCMD          0x02
+#define VAN_OTPR_SUBCMD          0x03
+
+#define VAN_CHECK_RET(a)   if(a != VAN_OK) return a
+
+
+typedef struct DISTANCE
+{
+    uint32_t millimeter;
+    uint32_t peak;
+    uint32_t noise;
+    uint32_t histcnt;
+    uint32_t confidence;
+    uint32_t dmax;
+    uint32_t retCode;
+} Van_Dist_TypeDef;
+
+typedef struct CONFIG
+{
+    //预设模式：VAN_HIGH_PRECISION, VAN_LONG_DISTANCE, VAN_HIGH_SPEED, VAN_ANTI_SUNLIGHT
+    //模组上电后默认是VAN_HIGH_PRECISION模式
+    uint8_t preSetMode;
+
+    //以下用于高级调试功能，普通用户仅使用预设模式即可
+    uint8_t pulseWidth;
+    uint8_t refSPAD;
+    uint8_t cgCorrection;
+    uint8_t integalCnt[3];
+    uint8_t MA[8];
+    uint8_t reserved[17];
+    
+    //以下默认存储在OTP中，用户通常不用修改
+    uint8_t oscTrim;
+    uint8_t bvdTrim;
+    uint8_t tdcTrim;
+    uint8_t mpSelect;
+    uint8_t offset;
+    uint8_t peak0[3];
+    uint8_t pileup[20];
+} Van_Config_TypeDef;
+
+typedef struct PILEUP
+{
+    float a4;
+    float a3;
+    float a2;
+    float a1;
+    float a0;
+} Van_PileUp_TypeDef;
+
+typedef enum Van_Status
+{
+    VAN_OK       = 0x00,
+    VAN_RANGING  = 0x01,
+    VAN_BUSY     = 0x02,
+    VAN_BUS_BUSY = 0x03,
+    VAN_SLEEP    = 0x04,
+    VAN_BOOTING  = 0x05,
+    VAN_ERROR    = 0x06
+} Van_Status;
+
+typedef enum PRE_MODE
+{
+    VAN_ADVANCED       = 0x00,
+    VAN_HIGH_PRECISION = 0x01,
+    VAN_LONG_DISTANCE  = 0x02,
+    VAN_HIGH_SPEED     = 0x03,
+    VAN_ANTI_SUNLIGHT  = 0x04
+} Van_PreMode;
+
+typedef enum RANGINGDATA
+{
+    VAN_DATA_VALID = 0x00,
+    VAN_RAW_DATA   = 0x01,
+    VAN_DATA_ERROR = 0x02
+} Van_RangingDataStatus;
+
+typedef enum NEXTACTION
+{
+    VAN_GO_ON = 0x00,
+    VAN_STOP  = 0x01
+} Van_NextAction;
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_Types.h	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,23 @@
+#ifndef __VAN_TYPES_H__
+#define __VAN_TYPES_H__
+
+typedef unsigned long long uint64_t;
+
+typedef unsigned int uint32_t;
+
+typedef int int32_t;
+
+typedef unsigned short uint16_t;
+
+typedef short int16_t;
+
+typedef unsigned char uint8_t;
+
+typedef signed char int8_t;
+
+typedef uint32_t FixPoint1616_t;
+
+#define NULL (void*)0
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_UserPlatform.cpp	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,80 @@
+#include "Van_UserPlatform.h"
+#include "DUT_RegConfig.h"
+
+extern I2C i2c_v;
+
+#define VAN_ECO_2V1 1
+
+#ifdef VAN_ECO_2V1
+
+Van_Status I2C_2V1_WriteOneReg(uint8_t addr, uint8_t value)
+{
+    uint8_t buf[2];
+    buf[0] = addr;
+    buf[1] = value;
+
+    i2c_v.write(DUT_DEV_ADDR, (char*)buf, 2);
+    
+    return VAN_OK;
+}
+
+Van_Status I2C_2V1_ReadOneReg(uint8_t addr, uint8_t *value)
+{
+    uint8_t buf[2];
+    buf[0] = addr;
+
+    i2c_v.write(DUT_DEV_ADDR, (char*)buf,  1);
+    i2c_v.read (DUT_DEV_ADDR, (char*)value, 1);
+    
+    return VAN_OK;
+}
+
+Van_Status I2C_WriteXByteWraper(uint8_t startaddr, uint8_t *buf, uint8_t len)
+{
+    Van_Status ret = VAN_OK;
+    for(uint8_t i = 0; i < len; i++)
+    {
+        ret = I2C_2V1_WriteOneReg(startaddr + i, *(buf + i));
+        VAN_CHECK_RET(ret);
+    }
+
+    return VAN_OK;
+}
+
+Van_Status I2C_ReadXByteWraper(uint8_t startaddr, uint8_t *buf, uint8_t len)
+{
+    Van_Status ret = VAN_OK;
+    for(uint8_t i = 0; i < len; i++)
+    {
+        ret = I2C_2V1_ReadOneReg(startaddr + i, buf + i);
+        VAN_CHECK_RET(ret);
+    }
+
+    return VAN_OK;
+}
+
+#endif
+
+Van_Status I2C_WriteXBytes(uint8_t startaddr, uint8_t *buf, uint8_t len)
+{
+    return I2C_WriteXByteWraper(startaddr, buf, len);
+}
+
+Van_Status I2C_ReadXBytes(uint8_t startaddr, uint8_t *buf, uint8_t len)
+{
+    return I2C_ReadXByteWraper(startaddr, buf, len);
+}
+
+void Van_Delay_us(uint32_t us)
+{
+
+}
+
+void Van_Delay_ms(uint32_t ms)
+{
+    
+}
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Van_Algorithm/Van_UserPlatform.h	Tue Jul 28 01:40:05 2020 +0000
@@ -0,0 +1,19 @@
+#ifndef __VAN_USERPLATFORM_H__
+#define __VAN_USERPLATFORM_H__
+
+#include "Van_Defines.h"
+
+Van_Status I2C_WriteXBytes(uint8_t startaddr, uint8_t *buf, uint8_t len);
+
+
+Van_Status I2C_ReadXBytes(uint8_t startaddr, uint8_t *buf, uint8_t len);
+
+void Van_Delay_us(uint32_t us);
+void Van_Delay_ms(uint32_t ms);
+
+#endif
+
+
+
+
+

--- a/main.cpp	Mon Jun 22 05:27:48 2020 +0000
+++ b/main.cpp	Tue Jul 28 01:40:05 2020 +0000
@@ -95,6 +95,7 @@
 Thread coutinu_measure_thread;
 Thread Histogram_thread;
 Thread Sevo_Thread;
+Thread AutoSingle;
 //Thread usb_receive_thread;
 
 Semaphore usb_semph(32);
@@ -148,6 +149,7 @@
     Histogram_thread.start(HistogramReport);
     //usb_receive_thread.start(usb_receive);
     Sevo_Thread.start(ServoRunThread);
+    AutoSingle.start(AutoSingleMeasurment);
 
     while(1) {
         //microUSB.printf("micro USB serial port\r\n");