Tom N
This is a class which contains function to interface with the MLX75320
Diff: LidarSpi.cpp
- Revision:
- 7:c47612b25c77
- Parent:
- 6:748062f3de21
- Child:
- 8:d49102c10940
diff -r 748062f3de21 -r c47612b25c77 LidarSpi.cpp
--- 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;
+
+}