Diff: LidarSpi.cpp

Revision:

9:067f75510d67

Parent:

8:d49102c10940

Child:

11:bd2dee9957a9

--- a/LidarSpi.cpp	Tue Apr 05 08:16:10 2016 +0000
+++ b/LidarSpi.cpp	Fri Apr 15 13:53:31 2016 +0000
@@ -1473,14 +1473,22 @@
         //pc->printf("REG_PORT_CHSEL Write error\n\r");
         return res;
     }
-    //pc->printf("Write REG_PORT_OVR_ACCUM: 0x4B3\n\r");
-    res=WriteReg(0x14c, 1203); //0x4B3
-    //res=WriteReg(0x14c, 0x0fff); //0x4B3
+    //pc->printf("Write REG_PORT_OVR_ACCUM: 0x4B3,\n\r");
+    //res=WriteReg(0x14c, 1203); //0x4B3
+    res=WriteReg(0x14c, 0x0fff); //0x4B3
     //res=WriteReg(0x14c, 0b0011); //0x4B3
     if (res<0) {
         //pc->printf("REG_PORT_OVR_ACCUM Write error\n\r");
         return res;
     }
+    //pc->printf("Write REG_PORT_FIXED_DIVIDER: 0x802\n\r");
+    res=WriteReg(0x14E, 0x0); //0x802
+    if (res<0) {
+        //pc->printf("REG_PORT_FIXED_DIVIDER Write error\n\r");
+        return res;
+    }
+    
+    
     //pc->printf("Write REG_PORT_TRIGGER_PERIOD: 0x3E8\n\r");
     res=WriteReg(0x148, 1000); //0x3e8
     if (res<0) {
@@ -1538,3 +1546,80 @@
     return 0;
                
 }
+
+int LidarSpi::setEcho(){
+    uint32_t val=128;
+    int res;
+    //pc.printf("Write PORT_LONG_PACKET_SIZE: %d\n\r",val);
+    //lidar.Trigger(1);
+    res=WriteReg(0x1d8, val);
+    //lidar.Trigger(0);
+    if (res<0)return res;
+    res=ReadReg(0x1d8,&val);
+    if (res<0)return res;
+    val=val>>16;
+    //pc.printf("Packet size: %d\n\r", val);
+    
+    //pc.printf("Write REG_PORT_CHSEL: 0xFFFF\n\r");
+    res=WriteReg(0x150, 0xffff);
+    if (res<0) return res;
+    //pc.printf("Write REG_PORT_OVR_ACCUM: 0x4B3\n\r");
+    //res=WriteReg(0x14c, 1203); //0x4B3
+    res=WriteReg(0x14c, 0x0fff); //0x4B3
+    if (res<0) return res;
+    
+    
+    //pc.printf("Write REG_PORT_SH1_CNTL: 0xAE40\n\r");
+    res=WriteReg(0x00124, 0xAE40); 
+    if (res<0) return res;
+    //pc.printf("Write REG_PORT_SH2_CNTL: 0xAE40\n\r");
+    res=WriteReg(0x00126, 0xAE40); 
+    if (res<0) return res;
+    
+    //pc.printf("Write REG_PORT_FIXED_DIVIDER: 0x802\n\r");
+    res=WriteReg(0x14E, 0xc00); //0x802
+    if (res<0) return res;
+    
+    //pc.printf("Write REG_PORT_TRIGGER_PERIOD: 0x3E8\n\r");
+    res=WriteReg(0x148, 1000); //0x3e8
+    if (res<0) return res;
+    
+    //pc.printf("Write REG_PORT_MEAS_DELAY: 0x0000\n\r");
+    res=WriteReg(0x32, 0xff);
+    if (res<0) return res;
+    //pc.printf("Write REG_PORT_MODE_EN: 0x0000\n\r");
+    res=WriteReg(0x30, 0xc);
+    if (res<0) return res;
+    
+    //pc.printf("Write REG_MLX16_ITC_MASK0: 0x7C34\n\r");
+    res=WriteReg(0x6c, 31796); //0x7C19
+    if (res<0) return res;
+    //pc.printf("Write REG_STIMER2_VALUE: 0xBFFF\n\r");
+    res=WriteReg(0x22, 49151); //0xBFFF
+    if (res<0) return res;
+    
+    
+    //Channel thresholds
+    uint32_t strtAddr=0x1bd2;
+    
+    for(uint32_t i =strtAddr;i< strtAddr+16*4; i=i+4){
+        res=WriteReg(i, 0x0);
+        if (res<0) return res;
+        wait_us(4);
+        res=WriteReg(i+2, 0x5);   
+        if (res<0) return res;
+        wait_us(4);
+    }
+    //Disable Multi-object
+    res=WriteReg(0x1c52,0x9c);
+    if (res<0) return res;
+    
+    //Peak Min Pulse edge len
+    res=WriteReg(0x1c80,0xe);
+    if (res<0) return res;
+    
+    
+    
+    
+}
+