Diff: LidarSpi.cpp

Revision:

5:87e211a23654

Parent:

3:9ed1d493c235

Child:

6:748062f3de21

--- a/LidarSpi.cpp	Fri Mar 04 12:09:32 2016 +0000
+++ b/LidarSpi.cpp	Fri Mar 04 14:17:32 2016 +0000
@@ -141,7 +141,7 @@
     if (res < 0)
         return res;
         //return sizeof(tx);
-
+    wait_us(5);
     return 0;
 }
 
@@ -214,12 +214,16 @@
         }
         else return res;
         //return sizeof(tx);
+    wait_us(5);
     return 0;
 }
 
 int LidarSpi::WriteReg(uint32_t reg, uint32_t val)
 {
-    return WriteRegSpeed(reg, val, START_DELAY);
+    int res=WriteRegSpeed(reg, val, START_DELAY);
+    wait_us(5);
+    return res;
+    
 }
 
 // Add a speed input so that we can control delay between packets
@@ -257,7 +261,9 @@
 
 int LidarSpi::WriteReg(uint32_t reg, uint32_t val, Serial* pc)
 {
-    return WriteRegSpeed(reg, val, START_DELAY, pc);
+    int res=WriteRegSpeed(reg, val, START_DELAY, pc);
+    wait_us(5);
+    return res;
 }
 
 // Add a speed input so that we can control delay between packets
@@ -373,16 +379,27 @@
         goto END;
 
     // Wait till PORT_READY bit is set.
-    res = ReadReg(470, &val);     // PORT_READY 
-    
+    trigger.write(1);
+    // Wait till PORT_READY bit is set.
+
 
-    
+    //res = ReadReg(470, &val);     // PORT_READY 
     cnt = 0;
-    while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
+    
+    /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
         wait_us(50);
         res = ReadReg(470, &val);     // PORT_READY
         cnt++;
+    } */   
+    while((!dataReady.read())&& (cnt<2000)){
+        wait_us(50);
+        cnt++;
     }
+    
+    
+    res = ReadReg(470, &val);     // PORT_READY 
+    val=val>>16;
+    pc->printf("PORT READY: %x\n\r",val);
     trigger.write(0);
     // Encode the request and send it
     res = MLX_ReqReadEch(tx);
@@ -518,15 +535,27 @@
             res=ReadReg(470, &val);
         }
     cnt = 0;
-    while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
+    trigger.write(1);
+    // Wait till PORT_READY bit is set.
+
+
+    //res = ReadReg(470, &val);     // PORT_READY 
+    cnt = 0;
+    
+    /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
         wait_us(50);
         res = ReadReg(470, &val);     // PORT_READY
-        while(res<0){
-            wait_us(4);
-            res = ReadReg(470, &val);
-        }
+        cnt++;
+    } */   
+    while((!dataReady.read())&& (cnt<2000)){
+        wait_us(50);
         cnt++;
     }
+    
+    
+    res = ReadReg(470, &val);     // PORT_READY 
+    val=val>>16;
+    pc->printf("PORT READY: %x\n\r",val);
     pc->printf("Counter: %d\n\r", cnt);
 
     // Encode the request and send it
@@ -733,30 +762,33 @@
         pc->printf("ReadReg Error2\n\r");
         goto END;}
     val = (val>>16) | 1;
-    trigger.write(1);
+    
     res = WriteReg(0x146, val);     // PORT_ACQU
     if (res < 0){
         pc->printf("WriteReg Error3\n\r");
         goto END;}
-
+    trigger.write(1);
     // Wait till PORT_READY bit is set.
 
 
-    res = ReadReg(470, &val);     // PORT_READY 
+    //res = ReadReg(470, &val);     // PORT_READY 
     cnt = 0;
-    /*
-    while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
+    
+    /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
         wait_us(50);
         res = ReadReg(470, &val);     // PORT_READY
         cnt++;
-    }*/
-    
-    while((!dataReady.read())&& (cnt<500)){
+    } */   
+    while((!dataReady.read())&& (cnt<2000)){
         wait_us(50);
         cnt++;
     }
     
     
+    res = ReadReg(470, &val);     // PORT_READY 
+    val=val>>16;
+    pc->printf("PORT READY: %x\n\r",val);
+    
     trigger.write(0);
     pc-printf("Count: %d\n\r", cnt);