Revision 7:c47612b25c77, committed 2016-03-29

Comitter:

TNU

Date:

Tue Mar 29 13:34:25 2016 +0000

Parent:

6:748062f3de21

Child:

8:d49102c10940

Commit message:

LoadPatch works;

Changed in this revision

--- a/LidarSpi.cpp	Wed Mar 23 15:24:50 2016 +0000
+++ b/LidarSpi.cpp	Tue Mar 29 13:34:25 2016 +0000
@@ -724,7 +724,7 @@
     pc->printf("chsel = %d\n", val);
     cnt = 0;
     // Count how many channels are selected 
-    for (i = 0; i < 16; i++) {
+    for (int i = 0; i < 16; i++) {
         if (val & 0x1)
             cnt++;
         val >>= 1;
@@ -770,23 +770,14 @@
         pc->printf("WriteReg Error3\n\r");
         goto END;}
     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
-        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);
@@ -1112,171 +1103,176 @@
     uint16_t  nPack;
     uint16_t nWords;
     
-    uint8_t memory[512];
+    uint8_t memory[15000];
     uint16_t addrStart, startLine, nBytes ;
     startLine=0;
-    memset(memory, 0, 512);
+    memset(memory, 0, 15000);
     
-
+    int count=0;
     res=WriteReg(0x1d8, 128);
     if (res<0){
         pc->printf("PORT_LONG_PACKET_SIZE Write error\n\r");
         goto END;
     }
     
+    res=WriteReg(0x6c, 31796); //0x7C19
+    if (res<0) {
+        pc->printf("REG_MLX16_ITC_MASK0 Write error\n\r");
+    }
+    
     wait_us(5);
     pc->printf("Start loading fragments \n\r");
-    int count=0;
-    while(LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc)==-5){
-        Trigger(0);
-        pc->printf("Load fragment %d\n\r", count);
-        for(int i =0; i<nBytes;i++){
-            //pc->printf("Addr[%d] : %d\n\r", addrStart+i, memory[i] );    
-        }
-        pc->printf("addrStart: %d, startLine %d, nBytes: %d\n\r",addrStart, startLine, nBytes);
-        nWords= nBytes / 2;
-        
-        // Ensure all packets are all zeros to not send trash
-        memset(tx, 0, sizeof(tx));
-        memset(&rxL, 0, sizeof(rxL));
+    
+    LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
+    Trigger(0);
+    pc->printf("Load fragment %d\n\r", count);
+    for(int i =0; i<nBytes;i++){
+        //pc->printf("Addr[%d] : %d\n\r", addrStart+i, memory[i] );    
+    }
+    pc->printf("addrStart: %d, startLine %d, nBytes: %d\n\r",addrStart, startLine, nBytes);
+    nWords= nBytes / 2;
     
-        //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
-        //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
-        nPack = nWords / (MLX_LONG2_DATA_SZ / 2)  + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0  :1);
-        pc->printf("npack: %d\n\r", nPack);
-        // Encode the request and send it
-        res = MLX_ReqWriteFW(tx, nPack, addrStart);
+    // Ensure all packets are all zeros to not send trash
+    memset(tx, 0, sizeof(tx));
+    memset(&rxL, 0, sizeof(rxL));
+
+    //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
+    //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
+    nPack = nWords / (MLX_LONG2_DATA_SZ / 2)  + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0  :1);
+    pc->printf("npack: %d\n\r", nPack);
+    // Encode the request and send it
+    res = MLX_ReqWriteFW(tx, nPack, addrStart);
+    if (res < 0){
+        pc->printf("Err @ 1");
+        goto END;   }
+        
+    wait_us(16);
+    res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx));
+    if (res < 0){
+        pc->printf("Err @ 2");
+        goto END;   }
+    //Trigger(1);
+          
+    pc->printf("ReqWriteFirmware MOSI: \n\r0x");
+    for(int k=0;k<(sizeof(tx));k++){
+        uint8_t* pnt=(uint8_t*)(&tx);
+        pc->printf("%02X", *(pnt+k));   
+        if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+    }
+    pc->printf("\n\r");
+    pc->printf("ReqWriteFirmware MISO: \n\r0x");
+    for(int k=0;k<(sizeof(rx));k++){
+        uint8_t* pnt=(uint8_t*)(&rx);
+        pc->printf("%02X", *(pnt+k));   
+        if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+    }
+    pc->printf("\n\r-------------------\n\r");
+    
+    
+    
+    wait_us(5);
+
+    // Optional status decoding
+    res = MLX_DecodeStatusS(rx, &iserr, &err);
+    if (res < 0 || iserr){
+        pc->printf("Err @ 3");
+        
+        pc->printf("LOAD PATCH REQ response MISO: \n\r0x");
+            for(int k=0;k<(sizeof(PACK_LONG2));k++){
+                uint8_t* pnt=(uint8_t*)(&rxL);
+                pc->printf("%02X", *(pnt+k));   
+                if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
+            }
+        pc->printf("\n\r");
+        
+        
+        goto END;   }
+
+    nWordsRemaining = nWords;
+    for (uint a = 0; a < nPack; ++a)
+    {
+        uint16_t size; 
+        if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
+            size = MLX_LONG2_DATA_SZ / 2;
+        else
+            size = nWordsRemaining;
+
+        res = MLX_WriteDataL2(&txL, size, a, &memory[a*MLX_LONG2_DATA_SZ]);
         if (res < 0){
-            pc->printf("Err @ 1");
-            goto END;   }
+            res = -7;
+            goto END;
+        }
+
+        res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
+
+        //Trigger(0);
+        wait_us(12);
+        /*
             
-        wait_us(16);
-        res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx));
-        if (res < 0){
-            pc->printf("Err @ 2");
-            goto END;   }
-        //Trigger(1);
-             /*   
-        pc->printf("ReqWriteFirmware MOSI: \n\r0x");
+        pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d    MOSI: \n\r0x", a,count);
         for(int k=0;k<(sizeof(PACK_LONG2));k++){
             uint8_t* pnt=(uint8_t*)(&txL);
             pc->printf("%02X", *(pnt+k));   
             if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
         }
         pc->printf("\n\r");
-        pc->printf("ReqWriteFirmware MISO: \n\r0x");
+        pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d   MISO: \n\r0x", a,count);
         for(int k=0;k<(sizeof(PACK_LONG2));k++){
             uint8_t* pnt=(uint8_t*)(&rxL);
             pc->printf("%02X", *(pnt+k));   
             if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
         }
-        pc->printf("\n\r-------------------\n\r");
+        pc->printf("\n\r-----------\n\r");
+        
         */
         
-        
-        wait_us(5);
-    
-        // Optional status decoding
-        res = MLX_DecodeStatusS(rx, &iserr, &err);
-        if (res < 0 || iserr){
-            pc->printf("Err @ 3");
-            
-            pc->printf("LOAD PATCH REQ response MISO: \n\r0x");
-                for(int k=0;k<(sizeof(PACK_LONG2));k++){
-                    uint8_t* pnt=(uint8_t*)(&rxL);
-                    pc->printf("%02X", *(pnt+k));   
-                    if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
-                }
-            pc->printf("\n\r");
-            
+        res = MLX_DecodeResL2(&rxL);
+        if (res < 0){
+            pc->printf("LONG WRITE ERROR: Stopped at the %d long message, res=%d\n", a, res);
             
-            goto END;   }
-    
-        nWordsRemaining = nWords;
-        for (uint a = 0; a < nPack; ++a)
-        {
-            uint16_t size; 
-            if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
-                size = MLX_LONG2_DATA_SZ / 2;
-            else
-                size = nWordsRemaining;
-
-            res = MLX_WriteDataL2(&txL, size, a, &memory[a*MLX_LONG2_DATA_SZ]);
-            if (res < 0){
-                res = -7;
-                goto END;
-            }
-
-            res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
-
-            //Trigger(0);
-            wait_us(8);
-
-            /*    
-            pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d    MOSI: \n\r0x", a,count);
+            pc->printf("LOAD PATCH LONG RESPONSE %d MOSI: \n\r0x", a);
             for(int k=0;k<(sizeof(PACK_LONG2));k++){
                 uint8_t* pnt=(uint8_t*)(&txL);
                 pc->printf("%02X", *(pnt+k));   
                 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
             }
             pc->printf("\n\r");
-            pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d   MISO: \n\r0x", a,count);
+            pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", a);
             for(int k=0;k<(sizeof(PACK_LONG2));k++){
                 uint8_t* pnt=(uint8_t*)(&rxL);
                 pc->printf("%02X", *(pnt+k));   
                 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
             }
-            pc->printf("\n\r-----------\n\r");
-            */
-            
-            
-            res = MLX_DecodeResL2(&rxL);
-            if (res < 0){
-                pc->printf("LONG WRITE ERROR: Stopped at the %d long message, res=%d\n", a, res);
-                
-                pc->printf("LOAD PATCH LONG RESPONSE %d MOSI: \n\r0x", a);
-                for(int k=0;k<(sizeof(PACK_LONG2));k++){
-                    uint8_t* pnt=(uint8_t*)(&txL);
-                    pc->printf("%02X", *(pnt+k));   
-                    if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
-                }
-                pc->printf("\n\r");
-                pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", a);
-                for(int k=0;k<(sizeof(PACK_LONG2));k++){
-                    uint8_t* pnt=(uint8_t*)(&rxL);
-                    pc->printf("%02X", *(pnt+k));   
-                    if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
-                }
-                pc->printf("\n\r");
-                res = -9;
-                goto END;
-            }
-    
-            nWordsRemaining = nWords - size;
-            
-        }
-        count++;
-        //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
-        res = MLX_EncodeStatusL2(&txL, 0, 0);
-        if (res < 0) {
-            res = -7;
+            pc->printf("\n\r");
+            res = -9;
             goto END;
         }
-    
-        // Clock the remaining long packets
-        res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
-        wait_us(11);
-        if (res < 0){
-            pc->printf("Err @ 4");
-            goto END;   }
+
+        nWordsRemaining = nWords - size;
         
-        
-        
-        
-        memset(memory, 0, 512);
-        wait_us(12);
+    }
+    count++;
+    //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
+    res = MLX_EncodeStatusL2(&txL, 0, 0);
+    if (res < 0) {
+        res = -7;
+        goto END;
     }
-    /*
+
+    // Clock the remaining long packets
+    res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
+    wait_us(11);
+    if (res < 0){
+        pc->printf("Err @ 4");
+        goto END;   }
+    
+    
+    
+    
+    memset(memory, 0, 512);
+    wait_us(12);
+    
+    
     pc->printf("Status long packet to check status after last fragment : %d, res=%d\n", count, res);
                 
     pc->printf("LOAD PATCH LONG STATUS %d MOSI: \n\r0x", count);
@@ -1293,13 +1289,13 @@
         if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
     }
     pc->printf("\n\r-----------\n\r");
-*/
+
     // Change jump table pointer.
     //res=LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
     //pc->printf("Return from a loadPatchFragment: res = %d \n\r", res);
     wait_ms(100);
     
-    if (count>3){
+    if (true){
         pc->printf("Change jumptable... \n\r");
         res = WriteReg(0x1000, 0x7000, pc);     // write addr: 0x1000 value:0x7000
         if (res < 0){
@@ -1342,63 +1338,78 @@
         pc->printf("   Can't open file '%s' for reading.\n\r", patch);
         return -10;
     }
+    pc->printf("Opened file\n\r");
     //  printf("Patch file %s opened\n", filename);
     
     while (true) {
         line[0] = '\0';
-        do {
-            fgets(line, 1000, fin);
-            lines++;
-        } while (lines < (*startLine));
+        fgets(line, 1000, fin);
+        lines++;
         //pc->printf("Startline: %d, lines: %d\n\r", *startLine,lines);
 
         if (line[strlen(line) - 1] == '\n') line[strlen(line) - 1] = '\0';
         if (line[strlen(line) - 1] == '\r') line[strlen(line) - 1] = '\0';
         *addrStart= 0;
-        if (parse_hex_line(line, bytes, &addr, &n, &status)) {
+        //int res=parse_hex_line(line, bytes, &addr, &n, &status);
+        //pc->printf("Result of parse: %d\n\r", res);
+        if (parse_hex_line(line, bytes, &addr, &n, &status)>0) {
             //cout << "Parse OK\n\r";
             //pc->printf("Line: %s\n\r", line);
-            if (!(isFirst | ((addr == prevAddr + 1) && (((*nBytes)+n) < 512)))) {
+
+            if (*nBytes>15000) {
                 *addrStart = minaddr;
                 *startLine = lines;
                 fclose(fin);
+                pc->printf("Close file\n\r");
                 return -5;
             }
             
             if (status == 0) {  /* data */
                                 //cout << "Status=0";
+                //if (addr < minaddr) minaddr = addr;
+                //pc->printf("addr: %d, minaddr: %d\n\r", addr, minaddr);
                 isFirst = false;
+                if (addr < minaddr) minaddr = addr;
                 for (i = 0; i <= (n - 1); i++) {
+                    
+                    
+                    prevAddr = addr;
+                    
                     if (i % 2 == 0)
-                        memory[*nBytes+i] = bytes[i + 1] & 255;  //Assumption even no of bytes per line
+                        memory[addr-minaddr] = bytes[i + 1] & 255;  //Assumption even no of bytes per line
                     else
-                        memory[*nBytes+i] = bytes[i - 1] & 255;
-                    total++;
+                        memory[addr-minaddr] = bytes[i - 1] & 255;
+                    
                     if (addr < minaddr) minaddr = addr;
-                    if (addr > maxaddr) maxaddr = addr;
-                    prevAddr = addr;
+                    if ((addr) > maxaddr) maxaddr = (addr);
                     addr++;
+                    total++;
                 }
             }
             if (status == 1) {  /* end of file */
                 fclose(fin);
+                pc->printf("Closed file\n\r");
                 //printf("load_file parsed %d bytes between:", total);
                 //printf(" %04X to %04X\n", minaddr, maxaddr);
+                
                 *addrStart= minaddr;
+                pc->printf("minAddr: %d, maxAddr: %d\n\r", minaddr, maxaddr);
+                *nBytes=maxaddr-minaddr;
                 pc->printf("End of file\n\r");
                 return total;
             }
             if (status == 2)  /* begin of file */
                 goto NEXT;
-            *nBytes += n;
+            *nBytes =0;
         }
         else {
-            printf("   Error: '%s', line: %d\n", patch, lineno);
+            printf("   Error: '%s', line: %d\n\r", patch, lineno);
         }
         lineCount++;
     NEXT:   lineno++;
     }
     fclose(fin);
+    pc->printf("Close file\n\r");
     return 0;
     
 }
@@ -1406,7 +1417,7 @@
     unsigned int sum, len, cksum;
     unsigned int newAddr, newCode, newByte;
     
-    
+
     char *ptr;
     //cout << "Start parsing\n\r";
     *num = 0;
@@ -1421,6 +1432,7 @@
     ptr += 4;
     //printf("Line: length=%d Addr=%d\n", len, *addr);
     if (!sscanf(ptr, "%02x", &newCode)) return 0;
+
     *code=(uint16_t)newCode;
     ptr += 2;
     sum = (len & 255) + ((*addr >> 8) & 255) + (*addr & 255) + (*code & 255);
@@ -1436,3 +1448,88 @@
     if (((sum & 255) + (cksum & 255)) & 255) return 0; /* checksum error */
     return 1;
 }
+
+int LidarSpi::setTrace()
+{
+    uint16_t val=148;
+    int res=0;
+    //pc->printf("Write PORT_LONG_PACKET_SIZE: %d\n\r",val);
+    //Trigger(1);
+    res=WriteReg(0x1d8, val);
+    //Trigger(0);
+    if (res<0){
+        //pc->printf("PORT_LONG_PACKET_SIZE Write error\n\r");
+        return res;
+    }
+
+    //pc->printf("Packet size: %d\n\r", val);
+    
+    //pc->printf("Write REG_PORT_CHSEL: 0xFFFF\n\r");
+    res=WriteReg(0x150, 0xffff);
+    if (res<0) {
+        //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
+    //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_TRIGGER_PERIOD: 0x3E8\n\r");
+    res=WriteReg(0x148, 1000); //0x3e8
+    if (res<0) {
+        //pc->printf("REG_PORT_TRIGGER_PERIOD Write error\n\r");
+        return res;
+    }
+    
+    //pc->printf("Write REG_PORT_MEAS_DELAY: 0x0000\n\r");
+    res=WriteReg(0x32, 0);
+    if (res<0) {
+        //pc->printf("REG_PORT_MEAS_DELAY Write error\n\r");
+        return res;
+    }
+    //pc->printf("Write REG_PORT_MODE_EN: 0x0000\n\r");
+    res=WriteReg(0x30, 0x00);
+    if (res<0) {
+        //pc->printf("REG_PORT_MODE_EN Write error\n\r");
+        return res;
+    }
+    
+    //pc->printf("Write REG_MLX16_ITC_MASK0: 0x7C34\n\r");
+    res=WriteReg(0x6c, 0x7C34); //0x7C19
+    if (res<0) {
+        //pc->printf("REG_MLX16_ITC_MASK0 Write error\n\r");
+        return res;
+    }
+    //pc->printf("Write REG_STIMER2_VALUE: 0xBFFF\n\r");
+    res=WriteReg(0x22, 49151); //0xBFFF
+    if (res<0) {
+        //pc->printf("REG_STIMER2_VALUE 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;
+    }
+    
+    //0x124=0xae40, and 0x126=0xae40
+    //pc->printf("Improvement registers for trace without patch\n\r");
+    res=WriteReg(0x124, 0xae40); //0x802
+    if (res<0) {
+        //pc->printf("PORT SH1 Write error\n\r");
+        return res;
+    }
+    res=WriteReg(0x126, 0xae40); //0x802
+    if (res<0) {
+        //pc->printf("PORT SH2 Write error\n\r");
+        return res;
+    }*/
+    return 0;
+               
+}

--- a/LidarSpi.h	Wed Mar 23 15:24:50 2016 +0000
+++ b/LidarSpi.h	Tue Mar 29 13:34:25 2016 +0000
@@ -175,7 +175,7 @@
         int GetTrace  ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx, Serial* pc);
         int GetTraceOne  ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx,int index , Serial* pc);
         int LoadPatch (const char *patch, Serial *pc);
-        
+        int setTrace(void);
         int PrintAllReg (uint16_t * regs, uint32_t * val, uint16_t size);
         
         void Trigger(int level);
@@ -202,6 +202,4 @@
 };
 
 
-
-
 #endif
\ No newline at end of file