Diff: LidarSpi.cpp

Revision:

2:7edbd37d81fd

Parent:

0:dfe498e03679

diff -r dfe498e03679 -r 7edbd37d81fd LidarSpi.cpp
--- a/LidarSpi.cpp	Thu Feb 25 08:02:11 2016 +0000
+++ b/LidarSpi.cpp	Thu Feb 25 08:08:37 2016 +0000
@@ -568,11 +568,7 @@
 
     // Temporary pointer to uint16 data, for simplicity purpose
     u16ptr = (uint16*)rxL.data;
-    PACK_LONG2 prevRX;
-    PACK_LONG2 prevTX;
     res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
-    memset(&prevRX,0,sizeof(prevRX));
-    memset(&prevTX,0,sizeof(prevTX));
     pc->printf("Value of npack:%d \n\r", nPack);
     for (a = 0; a < nPack; ++a)
     {
@@ -595,7 +591,7 @@
         
         // Decode the long responses, then extract data values
         res = MLX_DecodeResL2(&rxL);
-        if ((res < 0)|| a==5){
+        if ((res < 0)){
 
             pc->printf("\n\r");
             pc->printf("-- -- -- -- -- -- --\n\r");
@@ -617,14 +613,19 @@
             pc->printf("\n\r");
             pc->printf("-- -- -- -- -- -- --\n\r");
             pc->printf("Packet #%d => Decode long response error \n\r", a);
-            goto END;}
+            goto END;
+        }
+        if(a==nPack-1){
+            pc->printf("Values in payload: \n\r");
+            for(int i=0; i<128;i++) pc->printf("value in payload[%d]:  %u  \n\r",i,u16ptr[i]);
+        }
 
         // Gather all of the echo data in a buffer.
         //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b)
-        for (b = 0; b < (128/2); ++b)
+        for (b = 0; b < (128); ++b)
         {
             //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8));  //Swap LSByte with MSByte
-            buf[a*(128/2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8));  //Swap LSByte with MSByte
+            buf[a*(128) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8));  //Swap LSByte with MSByte
         }
         //prevTX=txL;
         //prevRX=rxL;
@@ -651,403 +652,7 @@
 END:
     trigger.write(0);
     return res;
-}
 
-
-
-
-int LidarSpi::GetTrace  ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx, Serial* pc){
-    int res, a, b, i;
-    uint32_t cnt;
-    uint16_t rSz;
-    PACK_SHORT rx[1], tx[1];
-    PACK_LONG1 rxL, txL;
-    uint8_t iserr;
-    uint16_t err;
-    uint32_t val = 0;
-    
-    uint16_t * u16ptr;
-    pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
-    // Required buffer space
-    if (maxN < MAXTRCLEN){
-        pc-printf("NOT ENOUGH BUFFER SPACEn\r");
-        return -1;}
-        
-    // Divide by payload size to get number of packets
-    //    const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
-    // WTA: change nPack to a variable, initially 64  
-    int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
-    pc->printf("npack: %d", nPack);
-    // Ensure transmitted packet is all zeros to not send trash
-    memset(&txL, 0, sizeof(PACK_LONG1));
-
-    memset(tx, 0, sizeof(tx));
-    //  memset(rx, 0, sizeof(rx));
-
-
-    res = ReadReg(336, &val);    // PORT_CHSEL
-    if (res < 0)
-        goto END;
-    val >>= 16;
-    pc->printf("chsel = %d\n", val);
-    cnt = 0;
-    // Count how many channels are selected 
-    for (i = 0; i < 16; i++) {
-        if (val & 0x1)
-            cnt++;
-        val >>= 1;
-    }
-    nPack *= cnt;
-    nPack /= 16;
-
-    res = ReadReg(332, &val);    // PORT_OVR_ACCUM_ACQ_OVR
-//  pc->printf("PORT_OVR  = %d\n", (val >> 16));
-    val = (val >> 16) & 3;               // Get bits 0 and 1
-
-    if (res < 0){
-        pc->printf("ReadReg Error1\n\r");
-        goto END;}
-
-    if (val == 0){       //00 = 1
-        nPack /= 4;
-    }
-    else if (val == 1){  //01 = 2
-        nPack /= 4;
-    }
-    else if (val == 2){  //10 = 4 
-        nPack /= 2;
-    }
-    else if (val == 3){  //11 = 8
-        nPack /= 1;
-    }
-    else {
-        pc->printf("GetTrace: bad value\n");
-    }
-
-
-    // Write 1 to PORT_ACQU register and then wait
-    res = ReadReg(0x146, &val);    // PORT_ACQU
-    if (res < 0){
-        pc->printf("ReadReg Error2\n\r");
-        goto END;}
-    val = (val>>16) | 1;
-    res = WriteReg(0x146, val);     // PORT_ACQU
-    if (res < 0){
-        pc->printf("WriteReg Error3\n\r");
-        goto END;}
-
-    // Wait till PORT_READY bit is set.
-    res = ReadReg(470, &val);     // PORT_READY 
-    cnt = 0;
-    while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
-        wait_us(50);
-        res = ReadReg(470, &val);     // PORT_READY
-        cnt++;
-    }
-
-    // Encode the request and send it
-    res = MLX_ReqReadTrc(tx);
-    if (res < 0)
-        goto END;
-
-    
-    //--------------------------------------------------------------------------------------
-    //----SHORT PACKET EXCHANGE--------
-    res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
-    if (res < 0)
-      goto END;
-    //----PACKET EXCHANGE
-    //--------------------------------------------------------------------------------------
-
-
-    // Optional status decoding
-    res = MLX_DecodeStatusS(rx, &iserr, &err);
-    if (res < 0 || iserr)
-        goto END;
-
-
-    // Temporary pointer to uint16 data, for simplicity purpose
-    u16ptr = (uint16_t*)rxL.data;
-    //device.format(16,1);
-    trigger.write(1);
-    for (a = 0; a < nPack; ++a)
-    {
-        res = MLX_EncodeStatusL1(&txL, 0, 0);
-        if (res < 0){
-            res = -7;
-            goto END;
-        }
-
-        //Tools::Wait(10);
-        wait_us(10);
-
-        // Clock the remaining long packets 
-        res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
-        //      pc->printf("\nRXL%d = 0x%02x%02x%02x...[x294 bytes]...%02x%02x%02x\n", a + 1, rxL.buf[0], rxL.buf[1], rxL.buf[2], rxL.buf[297], rxL.buf[298], rxL.buf[299]);
-        //      pc->printf("\nRXL%d = 0x%02x%02x%02x%02x%02x%02x\n", a + 1, rxL.buf[2], rxL.buf[3], rxL.buf[4], rxL.buf[5], rxL.buf[6], rxL.buf[7]);
-        if (res < 0){
-            res = -8;
-            goto END;
-        }
-
-        // Decode the long responses, then extract data values
-        res = MLX_DecodeResL1(&rxL);
-        if ((res < 0)){
-            pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
-            pc->printf("TXLONG MOSI Response:        ");
-            for(int i=0;i<(sizeof(PACK_LONG1));i++){
-                uint8_t* pnt=(uint8_t*)(&txL);
-                pc->printf("%02X ", *(pnt+i));   
-            }
-            pc->printf("\n\r");
-            pc->printf("-- -- -- -- -- -- --\n\r");
-            pc->printf("TXLONG MISO Response:        ");
-            for(int i=0;i<(sizeof(PACK_LONG1));i++){
-                uint8_t* pnt=(uint8_t*)(&rxL);
-                pc->printf("%02X ", *(pnt+i));   
-            
-            }
-            pc->printf("\n\r");
-            pc->printf("-- -- -- -- -- -- --\n\r");
-            //printf("last RXL = \n");
-            //for (i = 0; i < 300; i++) {
-            //  pc->printf("%02x", rxL.buf[i]);
-            //  if (((i + 1) % 8) == 0)
-            //      pc->printf("\n");
-            //}
-            //printf("\n");
-            res = -9;
-            goto END;
-        }
-        
-        // Copy the returned data into the user buffer
-        // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time 
-        for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
-        {
-            //WTA: removed MLX_HDR_SZ 
-            buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
-        }
-        
-    }
-    trigger.write(0);
-    device.format(8,1);
-
-END:
-    device.format(8,1);
-    return res;    
-}
-
-int LidarSpi::GetTraceOne  ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx,int index , Serial* pc){
-    int res, a, b, i;
-    uint32_t cnt;
-    uint16_t rSz;
-    PACK_SHORT rx[1], tx[1];
-    PACK_LONG1 rxL, txL;
-    uint8_t iserr;
-    uint16_t err;
-    uint32_t val = 0;
-    
-    uint16_t * u16ptr;
-    //pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
-    // Required buffer space
-    if (maxN < MAXTRCLEN){
-        pc-printf("NOT ENOUGH BUFFER SPACEn\r");
-        return -1;}
-        
-    // Divide by payload size to get number of packets
-    //    const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
-    // WTA: change nPack to a variable, initially 64  
-    int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
-    //pc->printf("npack: %d", nPack);
-    // Ensure transmitted packet is all zeros to not send trash
-    memset(&txL, 0, sizeof(PACK_LONG1));
-
-    memset(tx, 0, sizeof(tx));
-    //  memset(rx, 0, sizeof(rx));
-
-
-    res = ReadReg(336, &val);    // PORT_CHSEL
-    if (res < 0)
-        goto END;
-    val >>= 16;
-    //pc->printf("chsel = %d\n", val);
-    cnt = 0;
-    // Count how many channels are selected 
-    for (i = 0; i < 16; i++) {
-        if (val & 0x1)
-            cnt++;
-        val >>= 1;
-    }
-    nPack *= cnt;
-    nPack /= 16;
-
-    res = ReadReg(332, &val);    // PORT_OVR_ACCUM_ACQ_OVR
-//  pc->printf("PORT_OVR  = %d\n", (val >> 16));
-    val = (val >> 16) & 3;               // Get bits 0 and 1
-
-    if (res < 0){
-        pc->printf("ReadReg Error1\n\r");
-        goto END;}
-
-    if (val == 0){       //00 = 1
-        nPack /= 4;
-    }
-    else if (val == 1){  //01 = 2
-        nPack /= 4;
-    }
-    else if (val == 2){  //10 = 4 
-        nPack /= 2;
-    }
-    else if (val == 3){  //11 = 8
-        nPack /= 1;
-    }
-    else {
-        pc->printf("GetTrace: bad value\n");
-    }
-
-
-    // Write 1 to PORT_ACQU register and then wait
-    res = ReadReg(0x146, &val);    // PORT_ACQU
-    if (res < 0){
-        pc->printf("ReadReg Error2\n\r");
-        goto END;}
-    val = (val>>16) | 1;
-    res = WriteReg(0x146, val);     // PORT_ACQU
-    if (res < 0){
-        pc->printf("WriteReg Error3\n\r");
-        goto END;}
-
-    // Wait till PORT_READY bit is set.
-    res = ReadReg(470, &val);     // PORT_READY 
-    cnt = 0;
-    while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
-        wait_us(50);
-        res = ReadReg(470, &val);     // PORT_READY
-        cnt++;
-    }
-
-    // Encode the request and send it
-    res = MLX_ReqReadTrc(tx);
-    if (res < 0)
-        goto END;
-
-    
-    //--------------------------------------------------------------------------------------
-    //----SHORT PACKET EXCHANGE--------
-    res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
-    if (res < 0)
-      goto END;
-      
-    pc->printf("MOSI ReqReadTrace:\t 0x");
-    for(int i=0;i<(sizeof(tx));i++){
-        uint8_t* pnt=(uint8_t*)tx;
-        pc->printf("%02X", *(pnt+i));   
-    }
-    pc->printf("\n");
-    pc->printf("MISO ReqReadTrace:\t 0x");
-    for(int i=0;i<(sizeof(tx));i++){
-        uint8_t* pnt=(uint8_t*)rx;
-        pc->printf("%02X", *(pnt+i));   
-    }
-    pc->printf("\n");
-    pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n");
-    //----PACKET EXCHANGE
-    //--------------------------------------------------------------------------------------
-
-
-    // Optional status decoding
-    res = MLX_DecodeStatusS(rx, &iserr, &err);
-    if (res < 0 || iserr)
-        goto END;
-
-
-    // Temporary pointer to uint16 data, for simplicity purpose
-    u16ptr = (uint16_t*)rxL.data;
-    //device.format(16,1);
-    trigger.write(1);
-    for (a = 0; a < nPack; ++a)
-    {
-        res = MLX_EncodeStatusL1(&txL, 0, 0);
-        if (res < 0){
-            res = -7;
-            goto END;
-        }
-
-        //Tools::Wait(10);
-        wait_us(10);
-
-        // Clock the remaining long packets 
-        res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
-        //      pc->printf("\nRXL%d = 0x%02x%02x%02x...[x294 bytes]...%02x%02x%02x\n", a + 1, rxL.buf[0], rxL.buf[1], rxL.buf[2], rxL.buf[297], rxL.buf[298], rxL.buf[299]);
-        //      pc->printf("\nRXL%d = 0x%02x%02x%02x%02x%02x%02x\n", a + 1, rxL.buf[2], rxL.buf[3], rxL.buf[4], rxL.buf[5], rxL.buf[6], rxL.buf[7]);
-        if (res < 0){
-            res = -8;
-            goto END;
-        }
-        
-
-        // Decode the long responses, then extract data values
-        res = MLX_DecodeResL1(&rxL);
-        if ((res < 0)){
-            pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
-            pc->printf("TXLONG MOSI Response:        ");
-            for(int i=0;i<(sizeof(PACK_LONG1));i++){
-                uint8_t* pnt=(uint8_t*)(&txL);
-                pc->printf("%02X ", *(pnt+i));   
-            }
-            pc->printf("\n\r");
-            pc->printf("-- -- -- -- -- -- --\n\r");
-            pc->printf("TXLONG MISO Response:        ");
-            for(int i=0;i<(sizeof(PACK_LONG1));i++){
-                uint8_t* pnt=(uint8_t*)(&rxL);
-                pc->printf("%02X ", *(pnt+i));   
-            
-            }
-            pc->printf("\n\r");
-            pc->printf("-- -- -- -- -- -- --\n\r");
-            //printf("last RXL = \n");
-            //for (i = 0; i < 300; i++) {
-            //  pc->printf("%02x", rxL.buf[i]);
-            //  if (((i + 1) % 8) == 0)
-            //      pc->printf("\n");
-            //}
-            //printf("\n");
-            res = -9;
-            goto END;
-        }
-        
-        // Copy the returned data into the user buffer
-        // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time 
-        for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
-        {
-            //WTA: removed MLX_HDR_SZ 
-            buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
-        }
-        if(a<64){
-            pc->printf("Trace packet %d MOSI: \n\t\t\t\t\t0x", a);
-            for(int i=0;i<(sizeof(PACK_LONG1));i++){
-                uint8_t* pnt=(uint8_t*)(&txL);
-                pc->printf("%02X", *(pnt+i));   
-                if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
-            }
-            pc->printf("\n\r");
-            pc->printf("Trace packet %d MISO: \n\t\t\t\t\t0x", a);
-            for(int i=0;i<(sizeof(PACK_LONG1));i++){
-                uint8_t* pnt=(uint8_t*)(&rxL);
-                pc->printf("%02X", *(pnt+i));   
-                if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
-            }
-            pc->printf("\n\r");
-            pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");    
-        }
-        
-    }
-    trigger.write(0);
-    device.format(8,1);
-
-END:
-    device.format(8,1);
-    return res;    
 }