Tom N
/
LidarSpi
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
LidarSpi.cpp
00001 #include "mbed.h" 00002 #include "LidarSpi.h" 00003 00004 //#include "ToolsClass.h" 00005 #include <string.h> 00006 #include "LidarSpi.h" 00007 #include "MLX_BaseSPI.h" 00008 #include <inttypes.h> 00009 #define SPIFREQ 8000 00010 00011 //Set DEBUG to 1 to enable debug printf over Serial interface 00012 #ifndef DEBUG 00013 #define DEBUG 0 00014 #define debug_print(pntPc, ...) \ 00015 do { if (DEBUG) (Serial*)pntPc->printf(__VA_ARGS__); } while (0) //Arguments: Serial pointer and printf arguments 00016 #endif 00017 00018 LidarSpi::LidarSpi(PinName mosi, PinName miso, PinName clk, PinName chipSelect, PinName dr, PinName rs, PinName tr, PinName smpl):device(mosi, miso, clk), chipS(chipSelect), dataReady(dr),resetPin(rs), trigger(tr), sampling(smpl) 00019 { 00020 //resetPin.write(1); 00021 chipS.write(1); 00022 00023 //8 bit 00024 //Mode 1: CPPOL=0, CLPHA=1 => Default of lidar, mbed side receiving is bad near 25Mhz 00025 // -> mbed does not read first bit, this shifts the entire message on MISO 1 bit 00026 //Mode 3: CLPOL=1, CLPHA=1 => Transmission on mbed side good, Lidar chip returns CRC errors on the succesful tranmissions 00027 // -> Cant't send anything to lidar without changing lidar SPI mode 00028 device.format(8,1); //8 bit, CLPOL=0, CLPHA=1 00029 device.frequency(16000000); 00030 resetPin.write(0); 00031 wait_ms(300); 00032 resetPin.write(1); 00033 00034 } 00035 00036 00037 int LidarSpi::BasicRead(){ 00038 return device.write(0x6B); 00039 00040 } 00041 00042 00043 int LidarSpi::SpiSetting(long freq, int mode, Serial* pc){ 00044 int cPha= mode & 0x01; 00045 int cPol=(mode>>1) & 0x01; 00046 uint32_t val=0; 00047 int res=0; 00048 res=ReadReg(0x10E, &val,pc); 00049 if(res<0) return -1; 00050 val=val>>16; 00051 val=val | (cPol<<5) | (cPha<<6); 00052 wait_us(4); 00053 res=WriteReg(0x10E, val, pc); 00054 wait_us(4); 00055 device.format(8, mode); 00056 res=ReadReg(0x10E, &val,pc); 00057 00058 device.frequency(freq); 00059 return res; 00060 } 00061 00062 00063 //int LidarSpi::BasicTransfer(uint8_t* rData, uint16_t rSz, const uint8_t* tData, uint16_t tSz, const event_callback_t callback){ 00064 // device.transfer(tData,tSz,rData,rSz,callback,event=SPI_EVENT_COMPLETE); 00065 // //device.transfer((uint8_t*)_cmd, (2 + (DISPLAY_WIDTH / DISPLAY_BUFFER_TYPE_SIZE * sizeof(DISPLAY_BUFFER_TYPE))) , (uint8_t*)NULL, 0, _internalEventCallback, SPI_EVENT_COMPLETE) != 0) 00066 // device.transfer(); 00067 // return 0; 00068 //} 00069 00070 00071 int LidarSpi::TxPacket(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz){ 00072 chipS=0; 00073 int i =0; 00074 for(i=0; i< tSz;i++){ 00075 *rData=device.write(*tData); 00076 rData++; 00077 tData++; 00078 } 00079 chipS=1; 00080 *rSz=i; 00081 return 0; 00082 } 00083 00084 int LidarSpi::TxPacketWord(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz){ 00085 chipS=0; 00086 int i =0; 00087 uint16_t* recPoint=(uint16_t*)rData; 00088 uint16_t* transPoint=(uint16_t*)tData; 00089 00090 for(i=0; i< tSz/2;i++){ 00091 *recPoint=device.write(*transPoint); 00092 recPoint++; 00093 transPoint++; 00094 } 00095 chipS=1; 00096 *rSz=i*2; 00097 return 0; 00098 } 00099 00100 00101 00102 int LidarSpi::TxPacket(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz, Serial* pc){ 00103 chipS=0; 00104 int i =0; 00105 //debug_print(pc,"Transmitting %d bytes...\n\r",tSz); 00106 //debug_print(pc,"Received: "); 00107 for(i=0; i< tSz;i++){ 00108 //*(tData+i)=*(tData+i)+1; //<================Uncomment to write gibberish with wrong CRC 00109 *(rData+i)=device.write(*(tData+i)); 00110 //debug_print(pc,"%02X", *(rData+i)); 00111 //rData++; 00112 //tData++; 00113 } 00114 //debug_print(pc,"\n\r"); 00115 chipS=1; 00116 *rSz=i; 00117 return 0; 00118 } 00119 00120 int LidarSpi::TxPacketSlow(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz, uint16_t usDelay){ 00121 int res=TxPacket(rData, rSz, tData, tSz); 00122 wait_us(usDelay); 00123 return res; 00124 } 00125 00126 int LidarSpi::ReadReg(uint32_t reg, uint32_t *val) 00127 { 00128 int res; 00129 uint16_t rSz; 00130 PACK_SHORT rx[2], tx[2]; 00131 00132 // Ensure all packets are all zeros to not send trash 00133 memset(tx, 0, sizeof(tx)); 00134 00135 // Encode the request and send it 00136 res = MLX_ReqReadReg(tx, (uint32_t)reg); 00137 if (res < 0) 00138 return -1; 00139 00140 // tx[1].hdr = 0x0040; 00141 // tx[1].crc = 0x106a; 00142 res = MLX_EncodeStatusS(tx + 1, 0, 0); 00143 if (res < 0) 00144 return -2; 00145 00146 res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx)/2); 00147 wait_us(5); 00148 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2); 00149 if (res < 0) 00150 return -3; 00151 00152 // Decode the response packet with register value 00153 res = MLX_DecodeResS(rx + 1, val); 00154 if (res < 0) 00155 return res; 00156 //return sizeof(tx); 00157 wait_us(5); 00158 return 0; 00159 } 00160 00161 int LidarSpi::ReadReg(uint32_t reg, uint32_t *val, Serial* pc) 00162 { 00163 int res; 00164 uint16_t rSz; 00165 PACK_SHORT rx[2], tx[2]; 00166 00167 // Ensure all packets are all zeros to not send trash 00168 memset(tx, 0, sizeof(tx)); 00169 00170 // Encode the request and send it 00171 res = MLX_ReqReadReg(tx, (uint32_t)reg); 00172 if (res < 0) 00173 return -1; 00174 00175 // tx[1].hdr = 0x0040; 00176 // tx[1].crc = 0x106a; 00177 res = MLX_EncodeStatusS(tx + 1, 0, 0); 00178 if (res < 0) 00179 return -2; 00180 00181 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc); 00182 wait_us(5); 00183 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc); 00184 if (res < 0) 00185 return -3; 00186 00187 debug_print(pc,"Read register request and response\n\r"); 00188 00189 debug_print(pc,"MOSI ReadRegRequest:\t"); 00190 for(int i=0;i<(sizeof(tx)/2);i++){ 00191 uint8_t* pnt=(uint8_t*)tx; 00192 debug_print(pc,"%02X ", *(pnt+i)); 00193 } 00194 debug_print(pc,"\n\r"); 00195 debug_print(pc,"MISO ReadRegRequest:\t"); 00196 for(int i=0;i<(sizeof(tx)/2);i++){ 00197 uint8_t* pnt=(uint8_t*)rx; 00198 debug_print(pc,"%02X ", *(pnt+i)); 00199 } 00200 debug_print(pc,"\n\r"); 00201 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 00202 00203 00204 debug_print(pc,"MOSI ReadRegResponse:\t"); 00205 for(int i=0;i<(sizeof(tx)/2);i++){ 00206 uint8_t* pnt=(uint8_t*)(tx+1); 00207 debug_print(pc,"%02X ", *(pnt+i)); 00208 } 00209 debug_print(pc,"\n\r"); 00210 debug_print(pc,"MISO ReadRegResponse:\t"); 00211 for(int i=0;i<(sizeof(tx)/2);i++){ 00212 uint8_t* pnt=(uint8_t*)(rx+1); 00213 debug_print(pc,"%02X ", *(pnt+i)); 00214 } 00215 debug_print(pc,"\n\r"); 00216 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 00217 00218 // Decode the response packet with register value 00219 res = MLX_DecodeResS(rx + 1, val); 00220 if (res < 0) 00221 if(res==-6) { 00222 uint8_t isser=0; 00223 uint16_t err=0; 00224 MLX_DecodeStatusS(rx+1,&isser, &err); 00225 debug_print(pc,"Not a valid ReadRequestResponse-> Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,isser,err); 00226 return res; 00227 } 00228 else return res; 00229 //return sizeof(tx); 00230 wait_us(5); 00231 return 0; 00232 } 00233 00234 int LidarSpi::WriteReg(uint32_t reg, uint32_t val) 00235 { 00236 int res=WriteRegSpeed(reg, val, START_DELAY); 00237 wait_us(5); 00238 return res; 00239 00240 } 00241 00242 // Add a speed input so that we can control delay between packets 00243 int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed) 00244 { 00245 int res; 00246 uint16_t rSz; 00247 uint8_t iserr; 00248 uint16_t error; 00249 PACK_SHORT rx[2], tx[2]; 00250 00251 // Ensure all packets are all zeros to not send trash 00252 memset(tx, 0, sizeof(tx)); 00253 00254 // Encode the request and send it 00255 res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val); 00256 if (res < 0) 00257 return res; 00258 00259 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2); 00260 wait_us(5); 00261 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2); 00262 if (res < 0) 00263 return res; 00264 00265 00266 00267 // Decode response (a status packet) 00268 res = MLX_DecodeStatusS(rx + 1, &iserr, &error); 00269 if (res < 0 || iserr) 00270 return res; 00271 00272 return 0; 00273 } 00274 00275 int LidarSpi::WriteReg(uint32_t reg, uint32_t val, Serial* pc) 00276 { 00277 int res=WriteRegSpeed(reg, val, START_DELAY, pc); 00278 wait_us(5); 00279 return res; 00280 } 00281 00282 // Add a speed input so that we can control delay between packets 00283 int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed, Serial* pc) 00284 { 00285 int res; 00286 uint16_t rSz; 00287 uint8_t iserr; 00288 uint16_t error; 00289 PACK_SHORT rx[2], tx[2]; 00290 00291 // Ensure all packets are all zeros to not send trash 00292 memset(tx, 0, sizeof(tx)); 00293 00294 // Encode the request and send it 00295 res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val); 00296 if (res < 0) 00297 return res; 00298 00299 /* 00300 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), speed); 00301 if (res < 0) 00302 return res; 00303 */ 00304 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc); 00305 wait_us(5); 00306 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc); 00307 if (res < 0) 00308 return -3; 00309 00310 //debug_print(pc,"Write register request and response\n\r"); 00311 00312 debug_print(pc,"MOSI WriteRegRequest:\t 0x"); 00313 for(int i=0;i<(sizeof(tx)/2);i++){ 00314 uint8_t* pnt=(uint8_t*)tx; 00315 debug_print(pc,"%02X", *(pnt+i)); 00316 } 00317 debug_print(pc,"\n\r"); 00318 debug_print(pc,"MISO WriteRegRequest:\t 0x"); 00319 for(int i=0;i<(sizeof(tx)/2);i++){ 00320 uint8_t* pnt=(uint8_t*)rx; 00321 debug_print(pc,"%02X", *(pnt+i)); 00322 } 00323 debug_print(pc,"\n\r"); 00324 debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r"); 00325 00326 00327 debug_print(pc,"MOSI WriteRegResponse:\t 0x"); 00328 for(int i=0;i<(sizeof(tx)/2);i++){ 00329 uint8_t* pnt=(uint8_t*)(tx+1); 00330 debug_print(pc,"%02X", *(pnt+i)); 00331 } 00332 debug_print(pc,"\n\r"); 00333 debug_print(pc,"MISO WriteRegResponse:\t 0x"); 00334 for(int i=0;i<(sizeof(tx)/2);i++){ 00335 uint8_t* pnt=(uint8_t*)(rx+1); 00336 debug_print(pc,"%02X", *(pnt+i)); 00337 } 00338 debug_print(pc,"\n\r"); 00339 debug_print(pc,"==========================================================\n\r"); 00340 00341 00342 00343 // Decode response (a status packet) 00344 res = MLX_DecodeStatusS(rx + 1, &iserr, &error); 00345 if (res < 0 || iserr){ 00346 debug_print(pc,"Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,iserr,error); 00347 return res;} 00348 00349 return 0; 00350 } 00351 int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN) 00352 { 00353 trigger.write(0); 00354 int res, a, b; 00355 uint16_t rSz; 00356 uint32_t cnt; 00357 PACK_SHORT rx[1], tx[1]; 00358 PACK_LONG2 rxL, txL; 00359 uint8_t iserr; 00360 uint16_t err; 00361 uint16_t idx = 0; 00362 uint32_t val = 0; 00363 uint16_t buf[MAXCH*4*BYTES_PER_ECH/2]; 00364 00365 uint16_t * u16ptr ; 00366 00367 00368 // Required buffer space 00369 if (maxN < MAXECH) 00370 return -1; 00371 00372 // Echo data is transmitted in 128 word payload => PACK_LONG2 00373 // Each echo is X bytes, divide by payload size to get number of packets 00374 const int nEchoes = MAXCH * 4; 00375 const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ; 00376 const int WORDS_PER_ECH = BYTES_PER_ECH/2; 00377 00378 // Ensure transmitted packet is all zeros to not send trash 00379 memset(&txL, 0, sizeof(PACK_LONG2)); 00380 memset(tx, 0, sizeof(tx)); 00381 00382 //res = MLXSPI::SetConfig(0); //debug 00383 00384 // Write 1 to PORT_ACQU register and then wait 00385 res = ReadReg(0x146, &val); // PORT_ACQU 00386 if (res < 0) 00387 goto END; 00388 val = (val >> 16) | 1; 00389 trigger.write(1); 00390 res = WriteReg(0x146, val); // PORT_ACQU 00391 if (res < 0) 00392 goto END; 00393 00394 // Wait till PORT_READY bit is set. 00395 trigger.write(1); 00396 // Wait till PORT_READY bit is set. 00397 00398 00399 //res = ReadReg(470, &val); // PORT_READY 00400 cnt = 0; 00401 00402 /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) { 00403 wait_us(50); 00404 res = ReadReg(470, &val); // PORT_READY 00405 cnt++; 00406 } */ 00407 while((!dataReady.read())&& (cnt<2000)){ 00408 wait_us(50); 00409 cnt++; 00410 } 00411 00412 00413 res = ReadReg(470, &val); // PORT_READY 00414 val=val>>16; 00415 //debug_print(pc,"PORT READY: %x\n\r",val); 00416 trigger.write(0); 00417 // Encode the request and send it 00418 res = MLX_ReqReadEch(tx); 00419 if (res < 0) 00420 goto END; 00421 00422 //-------------------------------------------------------------------------------------- 00423 //----SHORT PACKET EXCHANGE-------- 00424 res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4); 00425 if (res < 0) 00426 goto END; 00427 //----PACKET EXCHANGE 00428 00429 // Optional status decoding 00430 res = MLX_DecodeStatusS(rx, &iserr, &err); 00431 if (res < 0 || iserr) 00432 goto END; 00433 00434 // Temporary pointer to uint16 data, for simplicity purpose 00435 u16ptr = (uint16*)rxL.data; 00436 00437 00438 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet 00439 if (res < 0){ 00440 res = -7; 00441 goto END; 00442 } 00443 for (a = 0; a < nPack; ++a) 00444 { 00445 00446 wait_us(10); 00447 //Tools::Wait(10); 00448 // Clock the remaining long packets 00449 //-------------------------------------------------------------------------------------- 00450 //----LONG PACKET EXCHANGE-------- 00451 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2)); 00452 if (res < 0) 00453 goto END; 00454 00455 // Decode the long responses, then extract data values 00456 res = MLX_DecodeResL2(&rxL); 00457 if (res < 0) 00458 goto END; 00459 00460 // Gather all of the echo data in a buffer. 00461 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b) 00462 for (b = 0; b < (128 / 2); ++b) 00463 { 00464 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte 00465 buf[a*(128 / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte 00466 } 00467 } 00468 00469 // Do something with the data... decode an echo, manage the empty echo case 00470 for (b = 0; b < nEchoes; ++b) { 00471 00472 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH]; 00473 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2]; 00474 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4]; 00475 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6]; 00476 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7]; 00477 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8; 00478 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8]; 00479 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10]; 00480 00481 ++idx; 00482 } 00483 00484 res = idx; 00485 00486 END: 00487 00488 return res; 00489 } 00490 00491 00492 int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, Serial* pc) 00493 { 00494 trigger.write(0); 00495 int res, a, b; 00496 uint16_t rSz; 00497 uint32_t cnt; 00498 PACK_SHORT rx[1], tx[1]; 00499 PACK_LONG2 rxL, txL; 00500 uint8_t iserr; 00501 uint16_t err; 00502 uint16_t idx = 0; 00503 uint32_t val = 0; 00504 uint16_t buf[MAXCH*4*BYTES_PER_ECH/2]; 00505 memset(buf, 0, MAXCH*4*BYTES_PER_ECH/2); 00506 uint16_t * u16ptr ; 00507 00508 00509 // Required buffer space 00510 if (maxN < MAXECH){ 00511 debug_print(pc,"maxN too small\n\r"); 00512 return -1; 00513 } 00514 00515 // Echo data is transmitted in 128 word payload => PACK_LONG2 00516 // Each echo is X bytes, divide by payload size to get number of packets 00517 const int nEchoes = MAXCH * 4; 00518 const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ; 00519 const int WORDS_PER_ECH = BYTES_PER_ECH/2; 00520 00521 // Ensure transmitted packet is all zeros to not send trash 00522 memset(&txL, 0, sizeof(PACK_LONG2)); 00523 memset(tx, 0, sizeof(tx)); 00524 00525 //res = MLXSPI::SetConfig(0); //debug 00526 00527 // Write 1 to PORT_ACQU register and then wait 00528 debug_print(pc,"\tRead PORT_ACQU\n\r"); 00529 res = ReadReg(0x146, &val); // PORT_ACQU 00530 wait_us(4); 00531 if (res < 0){ 00532 debug_print(pc,"ReadReg Error\n\r"); 00533 goto END;} 00534 val = (val >> 16) | 1; 00535 debug_print(pc,"\tWrite 1 to PORT_ACQU\n\r"); 00536 trigger.write(1); 00537 res = WriteReg(0x146, val); // PORT_ACQU 00538 wait_us(4); 00539 if (res < 0){ 00540 debug_print(pc,"WriteReg Error\n\r"); 00541 goto END;} 00542 00543 // Wait till PORT_READY bit is set. 00544 cnt = 0; 00545 debug_print(pc,"\tWait for PORT_READY bit\n\r"); 00546 res = ReadReg(470, &val); // PORT_READY 00547 wait_us(4); 00548 while((!dataReady.read())&& (cnt<2000)){ 00549 wait_us(50); 00550 cnt++; 00551 } 00552 if (cnt>=2000) 00553 debug_print(pc,"Port ready pin timeout\n\r"); 00554 trigger.write(1); 00555 // Wait till PORT_READY bit is set. 00556 00557 00558 //res = ReadReg(470, &val); // PORT_READY 00559 cnt = 0; 00560 00561 /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) { 00562 wait_us(50); 00563 res = ReadReg(470, &val); // PORT_READY 00564 cnt++; 00565 } */ 00566 00567 00568 00569 res = ReadReg(470, &val); // PORT_READY 00570 wait_us(4); 00571 val=val>>16; 00572 debug_print(pc,"PORT READY: %x\n\r",val); 00573 debug_print(pc,"Counter: %d\n\r", cnt); 00574 00575 // Encode the request and send it 00576 res = MLX_ReqReadEch(tx); 00577 if (res < 0){ 00578 debug_print(pc,"ReqreadEch error\n\r"); 00579 goto END;} 00580 00581 //-------------------------------------------------------------------------------------- 00582 //----SHORT PACKET EXCHANGE-------- 00583 debug_print(pc,"\tSend ReqReadEch\n\r"); 00584 res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4); 00585 if (res < 0){ 00586 debug_print(pc,"txPacketSlow Error\n\r"); 00587 goto END;} 00588 wait_us(4); 00589 /* 00590 debug_print(pc,"\tFirmware read request - processed\n\r"); 00591 debug_print(pc,"Firmware readRequest MOSI: \t"); 00592 for(int i =0; i<sizeof(tx);i++) { 00593 uint8_t* pnt=(uint8_t*)(&tx); 00594 debug_print(pc,"%02X ", *(pnt+i)); 00595 } 00596 debug_print(pc,"\n\r"); 00597 debug_print(pc,"Firmware readRequest MISO: \t"); 00598 for(int i =0; i<sizeof(rx);i++) { 00599 uint8_t* pnt=(uint8_t*)(&rx); 00600 debug_print(pc,"%02X ", *(pnt+i)); 00601 } 00602 debug_print(pc,"\n\r"); 00603 debug_print(pc,"-- -- -- -- -- -- --\n\r");*/ 00604 00605 //----PACKET EXCHANGE 00606 00607 // Optional status decoding 00608 res = MLX_DecodeStatusS(rx, &iserr, &err); 00609 if (res < 0 || iserr){ 00610 debug_print(pc,"Short status decode: Iss err?\n\r"); 00611 goto END;} 00612 00613 // Temporary pointer to uint16 data, for simplicity purpose 00614 u16ptr = (uint16*)rxL.data; 00615 PACK_LONG2 prevRX; 00616 PACK_LONG2 prevTX; 00617 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet 00618 memset(&prevRX,0,sizeof(prevRX)); 00619 memset(&prevTX,0,sizeof(prevTX)); 00620 debug_print(pc,"Value of npack:%d \n\r", nPack); 00621 for (a = 0; a < nPack; ++a) 00622 { 00623 00624 if (res < 0){ 00625 res = -7; 00626 debug_print(pc,"Problem creating echo status\n\r"); 00627 goto END; 00628 } 00629 wait_us(10); 00630 //Tools::Wait(10); 00631 // Clock the remaining long packets 00632 //-------------------------------------------------------------------------------------- 00633 //----LONG PACKET EXCHANGE-------- 00634 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2)); 00635 if (res < 0){ 00636 debug_print(pc,"Packet #%d => txPacket_long error\n\r",a); 00637 goto END;} 00638 wait_us(12); 00639 00640 // Decode the long responses, then extract data values 00641 res = MLX_DecodeResL2(&rxL); 00642 if ((res < 0)){ 00643 00644 debug_print(pc,"\n\r"); 00645 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 00646 debug_print(pc,"Packet #%d => Decode long response error \n\r", a); 00647 00648 debug_print(pc,"TXLONG MOSI Response: "); 00649 for(int i=0;i<(sizeof(PACK_LONG2));i++){ 00650 uint8_t* pnt=(uint8_t*)(&txL); 00651 debug_print(pc,"%02X ", *(pnt+i)); 00652 } 00653 debug_print(pc,"\n\r"); 00654 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 00655 debug_print(pc,"TXLONG MISO Response: "); 00656 for(int i=0;i<(sizeof(PACK_LONG2));i++){ 00657 uint8_t* pnt=(uint8_t*)(&rxL); 00658 debug_print(pc,"%02X ", *(pnt+i)); 00659 00660 } 00661 debug_print(pc,"\n\r"); 00662 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 00663 debug_print(pc,"Packet #%d => Decode long response error \n\r", a); 00664 goto END;} 00665 00666 // Gather all of the echo data in a buffer. 00667 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b) 00668 for (b = 0; b < (128); ++b) 00669 { 00670 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte 00671 buf[a*(128) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte 00672 } 00673 //prevTX=txL; 00674 //prevRX=rxL; 00675 } 00676 00677 // Do something with the data... decode an echo, manage the empty echo case 00678 for (b = 0; b < nEchoes; ++b) { 00679 00680 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH]; 00681 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2]; 00682 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4]; 00683 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6]; 00684 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7]; 00685 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8; 00686 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8]; 00687 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10]; 00688 00689 ++idx; 00690 } 00691 00692 res = idx; 00693 trigger.write(0); 00694 00695 END: 00696 trigger.write(0); 00697 return res; 00698 } 00699 00700 00701 00702 00703 int LidarSpi::GetTrace ( uint16_t *buf, uint16_t maxN,Serial* pc){ 00704 trigger.write(0); 00705 int res, a, b; 00706 uint32_t cnt; 00707 uint16_t rSz; 00708 PACK_SHORT rx[1], tx[1]; 00709 PACK_LONG1 rxL, txL; 00710 uint8_t iserr; 00711 uint16_t err; 00712 uint32_t val = 0; 00713 int nBytes=0; 00714 00715 uint16_t * u16ptr; 00716 debug_print(pc,"Buffer space required: %d\n\r", MAXTRCLEN); 00717 // Required buffer space 00718 if (maxN < MAXTRCLEN){ 00719 debug_print(pc,"NOT ENOUGH BUFFER SPACEn\r"); 00720 return -1;} 00721 00722 // Divide by payload size to get number of packets 00723 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ; 00724 // WTA: change nPack to a variable, initially 64 00725 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ; 00726 debug_print(pc,"npack: %d", nPack); 00727 // Ensure transmitted packet is all zeros to not send trash 00728 memset(&txL, 0, sizeof(PACK_LONG1)); 00729 00730 memset(tx, 0, sizeof(tx)); 00731 // memset(rx, 0, sizeof(rx)); 00732 00733 00734 res = ReadReg(336, &val); // PORT_CHSEL 00735 if (res < 0) 00736 goto END; 00737 val >>= 16; 00738 debug_print(pc,"chsel = %d\n", val); 00739 cnt = 0; 00740 // Count how many channels are selected 00741 for (int i = 0; i < 16; i++) { 00742 if (val & 0x1) 00743 cnt++; 00744 val >>= 1; 00745 } 00746 00747 nPack *= cnt; 00748 nPack /= 16; 00749 00750 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR 00751 // debug_print(pc,"PORT_OVR = %d\n", (val >> 16)); 00752 val = (val >> 16) & 3; // Get bits 0 and 1 00753 00754 if (res < 0){ 00755 debug_print(pc,"ReadReg Error1\n\r"); 00756 goto END;} 00757 00758 if (val == 0){ //00 = 1 00759 nPack /= 4; 00760 } 00761 else if (val == 1){ //01 = 2 00762 nPack /= 4; 00763 } 00764 else if (val == 2){ //10 = 4 00765 nPack /= 2; 00766 } 00767 else if (val == 3){ //11 = 8 00768 nPack /= 1; 00769 } 00770 else { 00771 debug_print(pc,"GetTrace: bad value\n"); 00772 } 00773 00774 00775 // Write 1 to PORT_ACQU register and then wait 00776 res = ReadReg(0x146, &val); // PORT_ACQU 00777 if (res < 0){ 00778 debug_print(pc,"ReadReg Error2\n\r"); 00779 goto END;} 00780 val = (val>>16) | 1; 00781 00782 res = WriteReg(0x146, val); // PORT_ACQU 00783 if (res < 0){ 00784 debug_print(pc,"WriteReg Error3\n\r"); 00785 goto END;} 00786 trigger.write(1); 00787 00788 // Wait till PORT_READY bit is set. 00789 cnt = 0; 00790 while((!dataReady.read())&& (cnt<2000)){ 00791 wait_us(50); 00792 cnt++; 00793 } 00794 00795 res = ReadReg(470, &val); // PORT_READY 00796 val=val>>16; 00797 debug_print(pc,"PORT READY: %x\n\r",val); 00798 00799 trigger.write(0); 00800 debug_print(pc,"Count: %d\n\r", cnt); 00801 00802 // Encode the request and send it 00803 res = MLX_ReqReadTrc(tx); 00804 if (res < 0) 00805 goto END; 00806 00807 00808 //-------------------------------------------------------------------------------------- 00809 //----SHORT PACKET EXCHANGE-------- 00810 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0); 00811 if (res < 0) 00812 goto END; 00813 //----PACKET EXCHANGE 00814 //-------------------------------------------------------------------------------------- 00815 00816 00817 // Optional status decoding 00818 res = MLX_DecodeStatusS(rx, &iserr, &err); 00819 if (res < 0 || iserr) 00820 goto END; 00821 00822 00823 // Temporary pointer to uint16 data, for simplicity purpose 00824 u16ptr = (uint16_t*)rxL.data; 00825 //device.format(16,1); 00826 //trigger.write(1); 00827 for (a = 0; a < nPack; ++a) 00828 { 00829 res = MLX_EncodeStatusL1(&txL, 0, 0); 00830 if (res < 0){ 00831 res = -7; 00832 goto END; 00833 } 00834 00835 //Tools::Wait(10); 00836 wait_us(10); 00837 00838 // Clock the remaining long packets 00839 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1)); 00840 // debug_print(pc,"\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]); 00841 // debug_print(pc,"\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]); 00842 if (res < 0){ 00843 res = -8; 00844 goto END; 00845 } 00846 00847 // Decode the long responses, then extract data values 00848 res = MLX_DecodeResL1(&rxL); 00849 if ((res < 0)){ 00850 debug_print(pc,"LONG READ ERROR: Stopped at the %d long message\n", a); 00851 debug_print(pc,"TXLONG MOSI Response: "); 00852 for(int i=0;i<(sizeof(PACK_LONG1));i++){ 00853 uint8_t* pnt=(uint8_t*)(&txL); 00854 debug_print(pc,"%02X ", *(pnt+i)); 00855 } 00856 debug_print(pc,"\n\r"); 00857 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 00858 debug_print(pc,"TXLONG MISO Response: "); 00859 for(int i=0;i<(sizeof(PACK_LONG1));i++){ 00860 uint8_t* pnt=(uint8_t*)(&rxL); 00861 debug_print(pc,"%02X ", *(pnt+i)); 00862 00863 } 00864 debug_print(pc,"\n\r"); 00865 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 00866 //printf("last RXL = \n"); 00867 //for (i = 0; i < 300; i++) { 00868 // debug_print(pc,"%02x", rxL.buf[i]); 00869 // if (((i + 1) % 8) == 0) 00870 // debug_print(pc,"\n"); 00871 //} 00872 //printf("\n"); 00873 res = -9; 00874 goto END; 00875 } 00876 00877 // Copy the returned data into the user buffer 00878 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time 00879 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b) 00880 { 00881 //WTA: removed MLX_HDR_SZ 00882 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b]; 00883 nBytes++; 00884 } 00885 00886 } 00887 trigger.write(0); 00888 device.format(8,1); 00889 res=nBytes; 00890 00891 END: 00892 device.format(8,1); 00893 return res; 00894 } 00895 00896 int LidarSpi::GetTraceOne ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx,int index , Serial* pc){ 00897 int res, a, b, i; 00898 uint32_t cnt; 00899 uint16_t rSz; 00900 PACK_SHORT rx[1], tx[1]; 00901 PACK_LONG1 rxL, txL; 00902 uint8_t iserr; 00903 uint16_t err; 00904 uint32_t val = 0; 00905 00906 uint16_t * u16ptr; 00907 //debug_print(pc,"Buffer space required: %d\n\r", MAXTRCLEN); 00908 // Required buffer space 00909 if (maxN < MAXTRCLEN){ 00910 pc-printf("NOT ENOUGH BUFFER SPACEn\r"); 00911 return -1;} 00912 00913 // Divide by payload size to get number of packets 00914 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ; 00915 // WTA: change nPack to a variable, initially 64 00916 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ; 00917 //debug_print(pc,"npack: %d", nPack); 00918 // Ensure transmitted packet is all zeros to not send trash 00919 memset(&txL, 0, sizeof(PACK_LONG1)); 00920 00921 memset(tx, 0, sizeof(tx)); 00922 // memset(rx, 0, sizeof(rx)); 00923 00924 00925 res = ReadReg(336, &val); // PORT_CHSEL 00926 if (res < 0) 00927 goto END; 00928 val >>= 16; 00929 //debug_print(pc,"chsel = %d\n", val); 00930 cnt = 0; 00931 // Count how many channels are selected 00932 for (i = 0; i < 16; i++) { 00933 if (val & 0x1) 00934 cnt++; 00935 val >>= 1; 00936 } 00937 nPack *= cnt; 00938 nPack /= 16; 00939 00940 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR 00941 // debug_print(pc,"PORT_OVR = %d\n", (val >> 16)); 00942 val = (val >> 16) & 3; // Get bits 0 and 1 00943 00944 if (res < 0){ 00945 debug_print(pc,"ReadReg Error1\n\r"); 00946 goto END;} 00947 00948 if (val == 0){ //00 = 1 00949 nPack /= 4; 00950 } 00951 else if (val == 1){ //01 = 2 00952 nPack /= 4; 00953 } 00954 else if (val == 2){ //10 = 4 00955 nPack /= 2; 00956 } 00957 else if (val == 3){ //11 = 8 00958 nPack /= 1; 00959 } 00960 else { 00961 debug_print(pc,"GetTrace: bad value\n"); 00962 } 00963 00964 00965 // Write 1 to PORT_ACQU register and then wait 00966 res = ReadReg(0x146, &val); // PORT_ACQU 00967 if (res < 0){ 00968 debug_print(pc,"ReadReg Error2\n\r"); 00969 goto END;} 00970 val = (val>>16) | 1; 00971 res = WriteReg(0x146, val); // PORT_ACQU 00972 if (res < 0){ 00973 debug_print(pc,"WriteReg Error3\n\r"); 00974 goto END;} 00975 00976 // Wait till PORT_READY bit is set. 00977 res = ReadReg(470, &val); // PORT_READY 00978 cnt = 0; 00979 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) { 00980 wait_us(50); 00981 res = ReadReg(470, &val); // PORT_READY 00982 cnt++; 00983 } 00984 00985 // Encode the request and send it 00986 res = MLX_ReqReadTrc(tx); 00987 if (res < 0) 00988 goto END; 00989 00990 00991 //-------------------------------------------------------------------------------------- 00992 //----SHORT PACKET EXCHANGE-------- 00993 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0); 00994 if (res < 0) 00995 goto END; 00996 00997 debug_print(pc,"MOSI ReqReadTrace:\t 0x"); 00998 for(int i=0;i<(sizeof(tx));i++){ 00999 uint8_t* pnt=(uint8_t*)tx; 01000 debug_print(pc,"%02X", *(pnt+i)); 01001 } 01002 debug_print(pc,"\n"); 01003 debug_print(pc,"MISO ReqReadTrace:\t 0x"); 01004 for(int i=0;i<(sizeof(tx));i++){ 01005 uint8_t* pnt=(uint8_t*)rx; 01006 debug_print(pc,"%02X", *(pnt+i)); 01007 } 01008 debug_print(pc,"\n"); 01009 debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n"); 01010 //----PACKET EXCHANGE 01011 //-------------------------------------------------------------------------------------- 01012 01013 01014 // Optional status decoding 01015 res = MLX_DecodeStatusS(rx, &iserr, &err); 01016 if (res < 0 || iserr) 01017 goto END; 01018 01019 01020 // Temporary pointer to uint16 data, for simplicity purpose 01021 u16ptr = (uint16_t*)rxL.data; 01022 //device.format(16,1); 01023 trigger.write(1); 01024 for (a = 0; a < nPack; ++a) 01025 { 01026 res = MLX_EncodeStatusL1(&txL, 0, 0); 01027 if (res < 0){ 01028 res = -7; 01029 goto END; 01030 } 01031 01032 //Tools::Wait(10); 01033 wait_us(10); 01034 01035 // Clock the remaining long packets 01036 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1)); 01037 // debug_print(pc,"\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]); 01038 // debug_print(pc,"\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]); 01039 if (res < 0){ 01040 res = -8; 01041 goto END; 01042 } 01043 01044 01045 // Decode the long responses, then extract data values 01046 res = MLX_DecodeResL1(&rxL); 01047 if ((res < 0)){ 01048 debug_print(pc,"LONG READ ERROR: Stopped at the %d long message\n", a); 01049 debug_print(pc,"TXLONG MOSI Response: "); 01050 for(int i=0;i<(sizeof(PACK_LONG1));i++){ 01051 uint8_t* pnt=(uint8_t*)(&txL); 01052 debug_print(pc,"%02X ", *(pnt+i)); 01053 } 01054 debug_print(pc,"\n\r"); 01055 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 01056 debug_print(pc,"TXLONG MISO Response: "); 01057 for(int i=0;i<(sizeof(PACK_LONG1));i++){ 01058 uint8_t* pnt=(uint8_t*)(&rxL); 01059 debug_print(pc,"%02X ", *(pnt+i)); 01060 01061 } 01062 debug_print(pc,"\n\r"); 01063 debug_print(pc,"-- -- -- -- -- -- --\n\r"); 01064 //printf("last RXL = \n"); 01065 //for (i = 0; i < 300; i++) { 01066 // debug_print(pc,"%02x", rxL.buf[i]); 01067 // if (((i + 1) % 8) == 0) 01068 // debug_print(pc,"\n"); 01069 //} 01070 //printf("\n"); 01071 res = -9; 01072 goto END; 01073 } 01074 01075 // Copy the returned data into the user buffer 01076 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time 01077 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b) 01078 { 01079 //WTA: removed MLX_HDR_SZ 01080 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b]; 01081 } 01082 if(a<64){ 01083 debug_print(pc,"Trace packet %d MOSI: \n\t\t\t\t\t0x", a); 01084 for(int i=0;i<(sizeof(PACK_LONG1));i++){ 01085 uint8_t* pnt=(uint8_t*)(&txL); 01086 debug_print(pc,"%02X", *(pnt+i)); 01087 if(((i %30) ==0)&&(i>0))debug_print(pc,"\n\t\t\t\t\t"); 01088 } 01089 debug_print(pc,"\n\r"); 01090 debug_print(pc,"Trace packet %d MISO: \n\t\t\t\t\t0x", a); 01091 for(int i=0;i<(sizeof(PACK_LONG1));i++){ 01092 uint8_t* pnt=(uint8_t*)(&rxL); 01093 debug_print(pc,"%02X", *(pnt+i)); 01094 if(((i %30) ==0)&&(i>0))debug_print(pc,"\n\t\t\t\t\t"); 01095 } 01096 debug_print(pc,"\n\r"); 01097 debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r"); 01098 } 01099 01100 } 01101 trigger.write(0); 01102 device.format(8,1); 01103 01104 END: 01105 device.format(8,1); 01106 return res; 01107 } 01108 01109 01110 int LidarSpi::LoadPatch (const char *patch, Serial * pc){ 01111 debug_print(pc,"Entered LiadPatch method....\n\r"); 01112 int res=0; 01113 uint16_t rSz; 01114 PACK_SHORT rx[1], tx[1]; 01115 PACK_LONG2 rxL, txL; 01116 uint8_t iserr; 01117 uint16_t err; 01118 //uint32_t val = 0; 01119 uint16_t nWordsRemaining; 01120 uint16_t nPack; 01121 uint16_t nWords; 01122 debug_print(pc,"Allocating memory....\n\r"); 01123 uint8_t memory[15000]; 01124 debug_print(pc,"Memory allocated\n\r"); 01125 uint16_t addrStart, startLine, nBytes ; 01126 startLine=0; 01127 memset(memory, 0, 15000); 01128 01129 int count=0; 01130 res=WriteReg(0x1d8, 128); 01131 if (res<0){ 01132 debug_print(pc,"PORT_LONG_PACKET_SIZE Write error\n\r"); 01133 goto END; 01134 } 01135 01136 res=WriteReg(0x6c, 31796); //0x7C19 01137 if (res<0) { 01138 debug_print(pc,"REG_MLX16_ITC_MASK0 Write error\n\r"); 01139 } 01140 01141 wait_us(5); 01142 debug_print(pc,"Start loading fragments \n\r"); 01143 01144 LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc); 01145 Trigger(0); 01146 debug_print(pc,"Load fragment %d\n\r", count); 01147 for(int i =0; i<nBytes;i++){ 01148 //debug_print(pc,"Addr[%d] : %d\n\r", addrStart+i, memory[i] ); 01149 } 01150 debug_print(pc,"addrStart: %d, startLine %d, nBytes: %d\n\r",addrStart, startLine, nBytes); 01151 nWords= nBytes / 2; 01152 01153 // Ensure all packets are all zeros to not send trash 01154 memset(tx, 0, sizeof(tx)); 01155 memset(&rxL, 0, sizeof(rxL)); 01156 01157 //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not 01158 //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0 01159 nPack = nWords / (MLX_LONG2_DATA_SZ / 2) + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0 :1); 01160 debug_print(pc,"npack: %d\n\r", nPack); 01161 // Encode the request and send it 01162 res = MLX_ReqWriteFW(tx, nPack, addrStart); 01163 if (res < 0){ 01164 debug_print(pc,"Err @ 1"); 01165 goto END; } 01166 01167 wait_us(16); 01168 res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx)); 01169 if (res < 0){ 01170 debug_print(pc,"Err @ 2"); 01171 goto END; } 01172 //Trigger(1); 01173 01174 debug_print(pc,"ReqWriteFirmware MOSI: \n\r0x"); 01175 for(int k=0;k<(sizeof(tx));k++){ 01176 uint8_t* pnt=(uint8_t*)(&tx); 01177 debug_print(pc,"%02X", *(pnt+k)); 01178 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01179 } 01180 debug_print(pc,"\n\r"); 01181 debug_print(pc,"ReqWriteFirmware MISO: \n\r0x"); 01182 for(int k=0;k<(sizeof(rx));k++){ 01183 uint8_t* pnt=(uint8_t*)(&rx); 01184 debug_print(pc,"%02X", *(pnt+k)); 01185 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01186 } 01187 debug_print(pc,"\n\r-------------------\n\r"); 01188 01189 01190 01191 wait_us(5); 01192 01193 // Optional status decoding 01194 res = MLX_DecodeStatusS(rx, &iserr, &err); 01195 if (res < 0 || iserr){ 01196 debug_print(pc,"Err @ 3"); 01197 01198 debug_print(pc,"LOAD PATCH REQ response MISO: \n\r0x"); 01199 for(int k=0;k<(sizeof(PACK_LONG2));k++){ 01200 uint8_t* pnt=(uint8_t*)(&rxL); 01201 debug_print(pc,"%02X", *(pnt+k)); 01202 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01203 } 01204 debug_print(pc,"\n\r"); 01205 01206 01207 goto END; } 01208 01209 nWordsRemaining = nWords; 01210 for (uint a = 0; a < nPack; ++a) 01211 { 01212 uint16_t size; 01213 if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2)) 01214 size = MLX_LONG2_DATA_SZ / 2; 01215 else 01216 size = nWordsRemaining; 01217 01218 res = MLX_WriteDataL2(&txL, size, a, &memory[a*MLX_LONG2_DATA_SZ]); 01219 if (res < 0){ 01220 res = -7; 01221 goto END; 01222 } 01223 01224 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2)); 01225 01226 //Trigger(0); 01227 wait_us(12); 01228 /* 01229 01230 debug_print(pc,"LOAD PATCH LONG RESPONSE %d of fragment %d MOSI: \n\r0x", a,count); 01231 for(int k=0;k<(sizeof(PACK_LONG2));k++){ 01232 uint8_t* pnt=(uint8_t*)(&txL); 01233 debug_print(pc,"%02X", *(pnt+k)); 01234 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01235 } 01236 debug_print(pc,"\n\r"); 01237 debug_print(pc,"LOAD PATCH LONG RESPONSE %d of fragment %d MISO: \n\r0x", a,count); 01238 for(int k=0;k<(sizeof(PACK_LONG2));k++){ 01239 uint8_t* pnt=(uint8_t*)(&rxL); 01240 debug_print(pc,"%02X", *(pnt+k)); 01241 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01242 } 01243 debug_print(pc,"\n\r-----------\n\r"); 01244 01245 */ 01246 01247 res = MLX_DecodeResL2(&rxL); 01248 if (res < 0){ 01249 debug_print(pc,"LONG WRITE ERROR: Stopped at the %d long message, res=%d\n", a, res); 01250 01251 debug_print(pc,"LOAD PATCH LONG RESPONSE %d MOSI: \n\r0x", a); 01252 for(int k=0;k<(sizeof(PACK_LONG2));k++){ 01253 uint8_t* pnt=(uint8_t*)(&txL); 01254 debug_print(pc,"%02X", *(pnt+k)); 01255 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01256 } 01257 debug_print(pc,"\n\r"); 01258 debug_print(pc,"LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", a); 01259 for(int k=0;k<(sizeof(PACK_LONG2));k++){ 01260 uint8_t* pnt=(uint8_t*)(&rxL); 01261 debug_print(pc,"%02X", *(pnt+k)); 01262 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01263 } 01264 debug_print(pc,"\n\r"); 01265 res = -9; 01266 goto END; 01267 } 01268 01269 nWordsRemaining = nWords - size; 01270 01271 } 01272 count++; 01273 //LAST STATUS LONG PACKET FROM 75320 to get status of last write long 01274 res = MLX_EncodeStatusL2(&txL, 0, 0); 01275 if (res < 0) { 01276 res = -7; 01277 goto END; 01278 } 01279 01280 // Clock the remaining long packets 01281 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2)); 01282 wait_us(11); 01283 if (res < 0){ 01284 debug_print(pc,"Err @ 4"); 01285 goto END; } 01286 01287 01288 01289 01290 memset(memory, 0, 512); 01291 wait_us(12); 01292 01293 01294 debug_print(pc,"Status long packet to check status after last fragment : %d, res=%d\n", count, res); 01295 01296 debug_print(pc,"LOAD PATCH LONG STATUS %d MOSI: \n\r0x", count); 01297 for(int k=0;k<(sizeof(PACK_LONG2));k++){ 01298 uint8_t* pnt=(uint8_t*)(&txL); 01299 debug_print(pc,"%02X", *(pnt+k)); 01300 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01301 } 01302 debug_print(pc,"\n\r"); 01303 debug_print(pc,"LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", count); 01304 for(int k=0;k<(sizeof(PACK_LONG2));k++){ 01305 uint8_t* pnt=(uint8_t*)(&rxL); 01306 debug_print(pc,"%02X", *(pnt+k)); 01307 if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r"); 01308 } 01309 debug_print(pc,"\n\r-----------\n\r"); 01310 01311 // Change jump table pointer. 01312 //res=LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc); 01313 //debug_print(pc,"Return from a loadPatchFragment: res = %d \n\r", res); 01314 wait_ms(100); 01315 01316 if (true){ 01317 debug_print(pc,"Change jumptable... \n\r"); 01318 res = WriteReg(0x1000, 0x7000, pc); // write addr: 0x1000 value:0x7000 01319 if (res < 0){ 01320 debug_print(pc,"Err @ 5"); 01321 goto END; 01322 } 01323 } 01324 else debug_print(pc,"Failed to read the file\n\r"); 01325 01326 END: 01327 return res; 01328 } 01329 01330 void LidarSpi::Trigger(int level){ 01331 trigger.write(level); 01332 } 01333 01334 01335 int LidarSpi::LoadPatchFragment(const char *patch, uint16_t *addrStart, uint16_t *startLine, uint16_t *nBytes, uint8_t *memory, Serial* pc){ 01336 char line[100]; 01337 FILE *fin; 01338 uint16_t addr, n, status; 01339 uint8_t bytes[256]; 01340 uint16_t i, total = 0, lineno = 1; 01341 uint16_t minaddr = 65535, maxaddr = 0; 01342 01343 uint16_t lineCount = *startLine; 01344 uint16_t lines = 0; 01345 01346 01347 *nBytes = 0; 01348 if (strlen(patch) == 0) { 01349 //printf(" Can't load a file without the filename."); 01350 return -9; 01351 } 01352 fin = fopen(patch, "r"); 01353 if (fin == NULL) { 01354 debug_print(pc," Can't open file '%s' for reading.\n\r", patch); 01355 return -10; 01356 } 01357 debug_print(pc,"Opened file\n\r"); 01358 // printf("Patch file %s opened\n", filename); 01359 01360 while (true) { 01361 line[0] = '\0'; 01362 fgets(line, 1000, fin); 01363 lines++; 01364 //debug_print(pc,"Startline: %d, lines: %d\n\r", *startLine,lines); 01365 01366 if (line[strlen(line) - 1] == '\n') line[strlen(line) - 1] = '\0'; 01367 if (line[strlen(line) - 1] == '\r') line[strlen(line) - 1] = '\0'; 01368 *addrStart= 0; 01369 //int res=parse_hex_line(line, bytes, &addr, &n, &status); 01370 //debug_print(pc,"Result of parse: %d\n\r", res); 01371 if (parse_hex_line(line, bytes, &addr, &n, &status)>0) { 01372 //cout << "Parse OK\n\r"; 01373 //debug_print(pc,"Line: %s\n\r", line); 01374 01375 if (*nBytes>15000) { 01376 *addrStart = minaddr; 01377 *startLine = lines; 01378 fclose(fin); 01379 debug_print(pc,"Close file\n\r"); 01380 return -5; 01381 } 01382 01383 if (status == 0) { /* data */ 01384 //cout << "Status=0"; 01385 //if (addr < minaddr) minaddr = addr; 01386 //debug_print(pc,"addr: %d, minaddr: %d\n\r", addr, minaddr); 01387 //isFirst = false; 01388 if (addr < minaddr) minaddr = addr; 01389 for (i = 0; i <= (n - 1); i++) { 01390 01391 01392 //prevAddr = addr; 01393 01394 if (i % 2 == 0) 01395 memory[addr-minaddr] = bytes[i + 1] & 255; //Assumption even no of bytes per line 01396 else 01397 memory[addr-minaddr] = bytes[i - 1] & 255; 01398 01399 if (addr < minaddr) minaddr = addr; 01400 if ((addr) > maxaddr) maxaddr = (addr); 01401 addr++; 01402 total++; 01403 } 01404 } 01405 if (status == 1) { /* end of file */ 01406 fclose(fin); 01407 debug_print(pc,"Closed file\n\r"); 01408 //printf("load_file parsed %d bytes between:", total); 01409 //printf(" %04X to %04X\n", minaddr, maxaddr); 01410 01411 *addrStart= minaddr; 01412 debug_print(pc,"minAddr: %d, maxAddr: %d\n\r", minaddr, maxaddr); 01413 *nBytes=maxaddr-minaddr; 01414 debug_print(pc,"End of file\n\r"); 01415 return total; 01416 } 01417 if (status == 2) /* begin of file */ 01418 goto NEXT; 01419 *nBytes =0; 01420 } 01421 else { 01422 printf(" Error: '%s', line: %d\n\r", patch, lineno); 01423 } 01424 lineCount++; 01425 NEXT: lineno++; 01426 }/* 01427 fclose(fin); 01428 debug_print(pc,"Close file\n\r"); 01429 return 0;*/ 01430 01431 } 01432 int LidarSpi::parse_hex_line(char *theline, uint8_t bytes[], uint16_t *addr, uint16_t *num, uint16_t *code){ 01433 unsigned int sum, len, cksum; 01434 unsigned int newAddr, newCode, newByte; 01435 01436 01437 char *ptr; 01438 //cout << "Start parsing\n\r"; 01439 *num = 0; 01440 if (theline[0] != ':') return 0; 01441 if (strlen(theline) < 11) return 0; 01442 ptr = theline + 1; 01443 if (!sscanf(ptr, "%02x", &len)) return 0; 01444 ptr += 2; 01445 if (strlen(theline) < (11 + (len * 2))) return 0; 01446 if (!sscanf(ptr, "%04x", &newAddr)) return 0; 01447 *addr=(uint16_t)newAddr; 01448 ptr += 4; 01449 //printf("Line: length=%d Addr=%d\n", len, *addr); 01450 if (!sscanf(ptr, "%02x", &newCode)) return 0; 01451 01452 *code=(uint16_t)newCode; 01453 ptr += 2; 01454 sum = (len & 255) + ((*addr >> 8) & 255) + (*addr & 255) + (*code & 255); 01455 while (*num != len) { 01456 if (!sscanf(ptr, "%02x", &newByte )) return 0; 01457 bytes[*num]=(uint8_t)newByte; 01458 ptr += 2; 01459 sum += bytes[*num] & 255; 01460 (*num)++; 01461 if (*num >= 256) return 0; 01462 } 01463 if (!sscanf(ptr, "%02x", &cksum)) return 0; 01464 if (((sum & 255) + (cksum & 255)) & 255) return 0; /* checksum error */ 01465 return 1; 01466 } 01467 01468 int LidarSpi::setTrace() 01469 { 01470 uint16_t val=148; 01471 int res=0; 01472 //debug_print(pc,"Write PORT_LONG_PACKET_SIZE: %d\n\r",val); 01473 //Trigger(1); 01474 res=WriteReg(0x1d8, val); 01475 //Trigger(0); 01476 if (res<0){ 01477 //debug_print(pc,"PORT_LONG_PACKET_SIZE Write error\n\r"); 01478 return res; 01479 } 01480 01481 //debug_print(pc,"Packet size: %d\n\r", val); 01482 01483 //debug_print(pc,"Write REG_PORT_CHSEL: 0xFFFF\n\r"); 01484 res=WriteReg(0x150, 0xffff); 01485 if (res<0) { 01486 //debug_print(pc,"REG_PORT_CHSEL Write error\n\r"); 01487 return res; 01488 } 01489 //debug_print(pc,"Write REG_PORT_OVR_ACCUM: 0x4B3,\n\r"); 01490 //res=WriteReg(0x14c, 1203); //0x4B3 01491 res=WriteReg(0x14c, 0x0fff); //0x4B3 01492 //res=WriteReg(0x14c, 0b0011); //0x4B3 01493 if (res<0) { 01494 //debug_print(pc,"REG_PORT_OVR_ACCUM Write error\n\r"); 01495 return res; 01496 } 01497 //debug_print(pc,"Write REG_PORT_FIXED_DIVIDER: 0x802\n\r"); 01498 res=WriteReg(0x14E, 0x0); //0x802 01499 if (res<0) { 01500 //debug_print(pc,"REG_PORT_FIXED_DIVIDER Write error\n\r"); 01501 return res; 01502 } 01503 01504 01505 //debug_print(pc,"Write REG_PORT_TRIGGER_PERIOD: 0x3E8\n\r"); 01506 res=WriteReg(0x148, 1000); //0x3e8 01507 if (res<0) { 01508 //debug_print(pc,"REG_PORT_TRIGGER_PERIOD Write error\n\r"); 01509 return res; 01510 } 01511 01512 //debug_print(pc,"Write REG_PORT_MEAS_DELAY: 0x0000\n\r"); 01513 res=WriteReg(0x32, 0); 01514 if (res<0) { 01515 //debug_print(pc,"REG_PORT_MEAS_DELAY Write error\n\r"); 01516 return res; 01517 } 01518 //debug_print(pc,"Write REG_PORT_MODE_EN: 0x0000\n\r"); 01519 res=WriteReg(0x30, 0x00); 01520 if (res<0) { 01521 //debug_print(pc,"REG_PORT_MODE_EN Write error\n\r"); 01522 return res; 01523 } 01524 01525 //debug_print(pc,"Write REG_MLX16_ITC_MASK0: 0x7C34\n\r"); 01526 res=WriteReg(0x6c, 0x7C34); //0x7C19 01527 if (res<0) { 01528 //debug_print(pc,"REG_MLX16_ITC_MASK0 Write error\n\r"); 01529 return res; 01530 } 01531 //debug_print(pc,"Write REG_STIMER2_VALUE: 0xBFFF\n\r"); 01532 res=WriteReg(0x22, 49151); //0xBFFF 01533 if (res<0) { 01534 //debug_print(pc,"REG_STIMER2_VALUE Write error\n\r"); 01535 return res; 01536 } 01537 /* 01538 //debug_print(pc,"Write REG_PORT_FIXED_DIVIDER: 0x802\n\r"); 01539 res=WriteReg(0x14E, 0x0); //0x802 01540 if (res<0) { 01541 //debug_print(pc,"REG_PORT_FIXED_DIVIDER Write error\n\r"); 01542 return res; 01543 } 01544 01545 //0x124=0xae40, and 0x126=0xae40 01546 //debug_print(pc,"Improvement registers for trace without patch\n\r"); 01547 res=WriteReg(0x124, 0xae40); //0x802 01548 if (res<0) { 01549 //debug_print(pc,"PORT SH1 Write error\n\r"); 01550 return res; 01551 } 01552 res=WriteReg(0x126, 0xae40); //0x802 01553 if (res<0) { 01554 //debug_print(pc,"PORT SH2 Write error\n\r"); 01555 return res; 01556 }*/ 01557 01558 01559 return 0; 01560 01561 } 01562 01563 int LidarSpi::setEcho(){ 01564 uint32_t val=128; 01565 int res; 01566 //pc.printf("Write PORT_LONG_PACKET_SIZE: %d\n\r",val); 01567 //lidar.Trigger(1); 01568 res=WriteReg(0x1d8, val); 01569 //lidar.Trigger(0); 01570 if (res<0)return res; 01571 res=ReadReg(0x1d8,&val); 01572 if (res<0)return res; 01573 val=val>>16; 01574 //pc.printf("Packet size: %d\n\r", val); 01575 01576 //pc.printf("Write REG_PORT_CHSEL: 0xFFFF\n\r"); 01577 res=WriteReg(0x150, 0xffff); 01578 if (res<0) return res; 01579 //pc.printf("Write REG_PORT_OVR_ACCUM: 0x4B3\n\r"); 01580 //res=WriteReg(0x14c, 1203); //0x4B3 01581 res=WriteReg(0x14c, 0x0fff); //0x4B3 01582 if (res<0) return res; 01583 01584 01585 //pc.printf("Write REG_PORT_SH1_CNTL: 0xAE40\n\r"); 01586 res=WriteReg(0x00124, 0xAE40); 01587 if (res<0) return res; 01588 //pc.printf("Write REG_PORT_SH2_CNTL: 0xAE40\n\r"); 01589 res=WriteReg(0x00126, 0xAE40); 01590 if (res<0) return res; 01591 01592 //pc.printf("Write REG_PORT_FIXED_DIVIDER: 0x802\n\r"); 01593 res=WriteReg(0x14E, 0xc00); //0x802 01594 if (res<0) return res; 01595 01596 //pc.printf("Write REG_PORT_TRIGGER_PERIOD: 0x3E8\n\r"); 01597 res=WriteReg(0x148, 1000); //0x3e8 01598 if (res<0) return res; 01599 01600 //pc.printf("Write REG_PORT_MEAS_DELAY: 0x0000\n\r"); 01601 res=WriteReg(0x32, 0xff); 01602 if (res<0) return res; 01603 //pc.printf("Write REG_PORT_MODE_EN: 0x0000\n\r"); 01604 res=WriteReg(0x30, 0xc); 01605 if (res<0) return res; 01606 01607 //pc.printf("Write REG_MLX16_ITC_MASK0: 0x7C34\n\r"); 01608 res=WriteReg(0x6c, 31796); //0x7C19 01609 if (res<0) return res; 01610 //pc.printf("Write REG_STIMER2_VALUE: 0xBFFF\n\r"); 01611 res=WriteReg(0x22, 49151); //0xBFFF 01612 if (res<0) return res; 01613 01614 01615 //Channel thresholds 01616 uint32_t strtAddr=0x1bd2; 01617 01618 for(uint32_t i =strtAddr;i< strtAddr+16*4; i=i+4){ 01619 res=WriteReg(i, 0x0); 01620 if (res<0) return res; 01621 wait_us(4); 01622 res=WriteReg(i+2, 0x5); 01623 if (res<0) return res; 01624 wait_us(4); 01625 } 01626 //Disable Multi-object 01627 res=WriteReg(0x1c52,0x9c); 01628 if (res<0) return res; 01629 01630 //Peak Min Pulse edge len 01631 res=WriteReg(0x1c80,0xe); 01632 return res; 01633 } 01634 01635 int LidarSpi::setLed(bool state){ 01636 int res=0; 01637 if(state){ 01638 WriteReg(0x1AC, 0x08); //PWM1 OFF 01639 res=WriteReg(0x154, 0x19); //LED1 ON 01640 } 01641 else{ 01642 WriteReg(0x1AC, 0x00); //PWM0 and 1 OFF 01643 res=WriteReg(0x154, 0x11); //LED1 OFF 01644 } 01645 return res; 01646 }
Generated on Sun Jul 17 2022 06:16:31 by
1.7.2