Tom N
This is a class which contains function to interface with the MLX75320
Diff: LidarSpi.cpp
- Revision:
- 12:d1767e2bd3a8
- Parent:
- 11:bd2dee9957a9
- Child:
- 13:ccf4ab73c33d
diff -r bd2dee9957a9 -r d1767e2bd3a8 LidarSpi.cpp
--- a/LidarSpi.cpp Fri Apr 15 14:07:49 2016 +0000
+++ b/LidarSpi.cpp Fri Apr 29 13:39:19 2016 +0000
@@ -8,6 +8,13 @@
#include <inttypes.h>
#define SPIFREQ 8000
+
+#ifndef DEBUG
+#define DEBUG 0
+#define debug_print(pntPc, ...) \
+ do { if (DEBUG) (Serial*)pntPc->printf(__VA_ARGS__); } while (0) //Arguments: Serial pointer and printf arguments
+#endif
+
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)
{
//resetPin.write(1);
@@ -18,10 +25,10 @@
// -> mbed does not read first bit, this shifts the entire message on MISO 1 bit
//Mode 3: CLPOL=1, CLPHA=1 => Transmission on mbed side good, Lidar chip returns CRC errors on the succesful tranmissions
// -> Cant't send anything to lidar without changing lidar SPI mode
- device.format(8,1); //8 bit, CLPOL=1, CLPHA=1
+ device.format(8,1); //8 bit, CLPOL=0, CLPHA=1
device.frequency(16000000);
resetPin.write(0);
- wait_us(100);
+ wait_ms(300);
resetPin.write(1);
}
@@ -95,16 +102,16 @@
int LidarSpi::TxPacket(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz, Serial* pc){
chipS=0;
int i =0;
- //pc->printf("Transmitting %d bytes...\n\r",tSz);
- //pc->printf("Received: ");
+ //debug_print(pc,"Transmitting %d bytes...\n\r",tSz);
+ //debug_print(pc,"Received: ");
for(i=0; i< tSz;i++){
//*(tData+i)=*(tData+i)+1; //<================Uncomment to write gibberish with wrong CRC
*(rData+i)=device.write(*(tData+i));
- //pc->printf("%02X", *(rData+i));
+ //debug_print(pc,"%02X", *(rData+i));
//rData++;
//tData++;
}
- //pc->printf("\n\r");
+ //debug_print(pc,"\n\r");
chipS=1;
*rSz=i;
return 0;
@@ -177,36 +184,36 @@
if (res < 0)
return -3;
- pc->printf("Read register request and response\n\r");
+ debug_print(pc,"Read register request and response\n\r");
- pc->printf("MOSI ReadRegRequest:\t");
+ debug_print(pc,"MOSI ReadRegRequest:\t");
for(int i=0;i<(sizeof(tx)/2);i++){
uint8_t* pnt=(uint8_t*)tx;
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("MISO ReadRegRequest:\t");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"MISO ReadRegRequest:\t");
for(int i=0;i<(sizeof(tx)/2);i++){
uint8_t* pnt=(uint8_t*)rx;
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
- pc->printf("MOSI ReadRegResponse:\t");
+ debug_print(pc,"MOSI ReadRegResponse:\t");
for(int i=0;i<(sizeof(tx)/2);i++){
uint8_t* pnt=(uint8_t*)(tx+1);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("MISO ReadRegResponse:\t");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"MISO ReadRegResponse:\t");
for(int i=0;i<(sizeof(tx)/2);i++){
uint8_t* pnt=(uint8_t*)(rx+1);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
// Decode the response packet with register value
res = MLX_DecodeResS(rx + 1, val);
@@ -215,7 +222,7 @@
uint8_t isser=0;
uint16_t err=0;
MLX_DecodeStatusS(rx+1,&isser, &err);
- pc->printf("Not a valid ReadRequestResponse-> Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,isser,err);
+ debug_print(pc,"Not a valid ReadRequestResponse-> Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,isser,err);
return res;
}
else return res;
@@ -300,43 +307,43 @@
if (res < 0)
return -3;
- //pc->printf("Write register request and response\n\r");
+ //debug_print(pc,"Write register request and response\n\r");
- pc->printf("MOSI WriteRegRequest:\t 0x");
+ debug_print(pc,"MOSI WriteRegRequest:\t 0x");
for(int i=0;i<(sizeof(tx)/2);i++){
uint8_t* pnt=(uint8_t*)tx;
- pc->printf("%02X", *(pnt+i));
+ debug_print(pc,"%02X", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("MISO WriteRegRequest:\t 0x");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"MISO WriteRegRequest:\t 0x");
for(int i=0;i<(sizeof(tx)/2);i++){
uint8_t* pnt=(uint8_t*)rx;
- pc->printf("%02X", *(pnt+i));
+ debug_print(pc,"%02X", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
- pc->printf("MOSI WriteRegResponse:\t 0x");
+ debug_print(pc,"MOSI WriteRegResponse:\t 0x");
for(int i=0;i<(sizeof(tx)/2);i++){
uint8_t* pnt=(uint8_t*)(tx+1);
- pc->printf("%02X", *(pnt+i));
+ debug_print(pc,"%02X", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("MISO WriteRegResponse:\t 0x");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"MISO WriteRegResponse:\t 0x");
for(int i=0;i<(sizeof(tx)/2);i++){
uint8_t* pnt=(uint8_t*)(rx+1);
- pc->printf("%02X", *(pnt+i));
+ debug_print(pc,"%02X", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("==========================================================\n\r");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"==========================================================\n\r");
// Decode response (a status packet)
res = MLX_DecodeStatusS(rx + 1, &iserr, &error);
if (res < 0 || iserr){
- pc->printf("Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,iserr,error);
+ debug_print(pc,"Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,iserr,error);
return res;}
return 0;
@@ -405,7 +412,7 @@
res = ReadReg(470, &val); // PORT_READY
val=val>>16;
- //pc->printf("PORT READY: %x\n\r",val);
+ //debug_print(pc,"PORT READY: %x\n\r",val);
trigger.write(0);
// Encode the request and send it
res = MLX_ReqReadEch(tx);
@@ -501,7 +508,7 @@
// Required buffer space
if (maxN < MAXECH){
- pc->printf("maxN too small\n\r");
+ debug_print(pc,"maxN too small\n\r");
return -1;
}
@@ -518,29 +525,32 @@
//res = MLXSPI::SetConfig(0); //debug
// Write 1 to PORT_ACQU register and then wait
- pc->printf("\tRead PORT_ACQU\n\r");
+ debug_print(pc,"\tRead PORT_ACQU\n\r");
res = ReadReg(0x146, &val); // PORT_ACQU
+ wait_us(4);
if (res < 0){
- pc->printf("ReadReg Error\n\r");
+ debug_print(pc,"ReadReg Error\n\r");
goto END;}
val = (val >> 16) | 1;
- pc->printf("\tWrite 1 to PORT_ACQU\n\r");
+ debug_print(pc,"\tWrite 1 to PORT_ACQU\n\r");
trigger.write(1);
res = WriteReg(0x146, val); // PORT_ACQU
+ wait_us(4);
if (res < 0){
- pc->printf("WriteReg Error\n\r");
+ debug_print(pc,"WriteReg Error\n\r");
goto END;}
// Wait till PORT_READY bit is set.
-
- pc->printf("\tWait for PORT_READY bit\n\r");
+ cnt = 0;
+ debug_print(pc,"\tWait for PORT_READY bit\n\r");
res = ReadReg(470, &val); // PORT_READY
- while(res<0){
- pc->printf("Error: reread state of ready PIN\n\r");
- wait_us(4);
- res=ReadReg(470, &val);
- }
- cnt = 0;
+ wait_us(4);
+ while((!dataReady.read())&& (cnt<2000)){
+ wait_us(50);
+ cnt++;
+ }
+ if (cnt>=2000)
+ debug_print(pc,"Port ready pin timeout\n\r");
trigger.write(1);
// Wait till PORT_READY bit is set.
@@ -553,52 +563,51 @@
res = ReadReg(470, &val); // PORT_READY
cnt++;
} */
- while((!dataReady.read())&& (cnt<2000)){
- wait_us(50);
- cnt++;
- }
+
res = ReadReg(470, &val); // PORT_READY
+ wait_us(4);
val=val>>16;
- pc->printf("PORT READY: %x\n\r",val);
- pc->printf("Counter: %d\n\r", cnt);
+ debug_print(pc,"PORT READY: %x\n\r",val);
+ debug_print(pc,"Counter: %d\n\r", cnt);
// Encode the request and send it
res = MLX_ReqReadEch(tx);
if (res < 0){
- pc->printf("ReqreadEch error\n\r");
+ debug_print(pc,"ReqreadEch error\n\r");
goto END;}
//--------------------------------------------------------------------------------------
//----SHORT PACKET EXCHANGE--------
- pc->printf("\tSend ReqReadEch\n\r");
+ debug_print(pc,"\tSend ReqReadEch\n\r");
res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
if (res < 0){
- pc->printf("txPacketSlow Error\n\r");
+ debug_print(pc,"txPacketSlow Error\n\r");
goto END;}
+ wait_us(4);
/*
- pc->printf("\tFirmware read request - processed\n\r");
- pc->printf("Firmware readRequest MOSI: \t");
+ debug_print(pc,"\tFirmware read request - processed\n\r");
+ debug_print(pc,"Firmware readRequest MOSI: \t");
for(int i =0; i<sizeof(tx);i++) {
uint8_t* pnt=(uint8_t*)(&tx);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("Firmware readRequest MISO: \t");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"Firmware readRequest MISO: \t");
for(int i =0; i<sizeof(rx);i++) {
uint8_t* pnt=(uint8_t*)(&rx);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");*/
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");*/
//----PACKET EXCHANGE
// Optional status decoding
res = MLX_DecodeStatusS(rx, &iserr, &err);
if (res < 0 || iserr){
- pc->printf("Short status decode: Iss err?\n\r");
+ debug_print(pc,"Short status decode: Iss err?\n\r");
goto END;}
// Temporary pointer to uint16 data, for simplicity purpose
@@ -608,13 +617,13 @@
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);
+ debug_print(pc,"Value of npack:%d \n\r", nPack);
for (a = 0; a < nPack; ++a)
{
if (res < 0){
res = -7;
- pc->printf("Problem creating echo status\n\r");
+ debug_print(pc,"Problem creating echo status\n\r");
goto END;
}
wait_us(10);
@@ -624,34 +633,34 @@
//----LONG PACKET EXCHANGE--------
res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
if (res < 0){
- pc->printf("Packet #%d => txPacket_long error\n\r",a);
+ debug_print(pc,"Packet #%d => txPacket_long error\n\r",a);
goto END;}
-
+ wait_us(12);
// Decode the long responses, then extract data values
res = MLX_DecodeResL2(&rxL);
if ((res < 0)){
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
- pc->printf("Packet #%d => Decode long response error \n\r", a);
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"Packet #%d => Decode long response error \n\r", a);
- pc->printf("TXLONG MOSI Response: ");
+ debug_print(pc,"TXLONG MOSI Response: ");
for(int i=0;i<(sizeof(PACK_LONG2));i++){
uint8_t* pnt=(uint8_t*)(&txL);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
- pc->printf("TXLONG MISO Response: ");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"TXLONG MISO Response: ");
for(int i=0;i<(sizeof(PACK_LONG2));i++){
uint8_t* pnt=(uint8_t*)(&rxL);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
- pc->printf("Packet #%d => Decode long response error \n\r", a);
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"Packet #%d => Decode long response error \n\r", a);
goto END;}
// Gather all of the echo data in a buffer.
@@ -693,7 +702,7 @@
int LidarSpi::GetTrace ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx, Serial* pc){
trigger.write(0);
- int res, a, b, i;
+ int res, a, b;
uint32_t cnt;
uint16_t rSz;
PACK_SHORT rx[1], tx[1];
@@ -704,17 +713,17 @@
int nBytes=0;
uint16_t * u16ptr;
- pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
+ debug_print(pc,"Buffer space required: %d\n\r", MAXTRCLEN);
// Required buffer space
if (maxN < MAXTRCLEN){
- pc-printf("NOT ENOUGH BUFFER SPACEn\r");
+ debug_print(pc,"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);
+ debug_print(pc,"npack: %d", nPack);
// Ensure transmitted packet is all zeros to not send trash
memset(&txL, 0, sizeof(PACK_LONG1));
@@ -726,7 +735,7 @@
if (res < 0)
goto END;
val >>= 16;
- pc->printf("chsel = %d\n", val);
+ debug_print(pc,"chsel = %d\n", val);
cnt = 0;
// Count how many channels are selected
for (int i = 0; i < 16; i++) {
@@ -739,11 +748,11 @@
nPack /= 16;
res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
-// pc->printf("PORT_OVR = %d\n", (val >> 16));
+// debug_print(pc,"PORT_OVR = %d\n", (val >> 16));
val = (val >> 16) & 3; // Get bits 0 and 1
if (res < 0){
- pc->printf("ReadReg Error1\n\r");
+ debug_print(pc,"ReadReg Error1\n\r");
goto END;}
if (val == 0){ //00 = 1
@@ -759,20 +768,20 @@
nPack /= 1;
}
else {
- pc->printf("GetTrace: bad value\n");
+ debug_print(pc,"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");
+ debug_print(pc,"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");
+ debug_print(pc,"WriteReg Error3\n\r");
goto END;}
trigger.write(1);
@@ -785,10 +794,10 @@
res = ReadReg(470, &val); // PORT_READY
val=val>>16;
- pc->printf("PORT READY: %x\n\r",val);
+ debug_print(pc,"PORT READY: %x\n\r",val);
trigger.write(0);
- pc-printf("Count: %d\n\r", cnt);
+ debug_print(pc,"Count: %d\n\r", cnt);
// Encode the request and send it
res = MLX_ReqReadTrc(tx);
@@ -828,8 +837,8 @@
// 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]);
+ // 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]);
+ // 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]);
if (res < 0){
res = -8;
goto END;
@@ -838,27 +847,27 @@
// 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: ");
+ debug_print(pc,"LONG READ ERROR: Stopped at the %d long message\n", a);
+ debug_print(pc,"TXLONG MOSI Response: ");
for(int i=0;i<(sizeof(PACK_LONG1));i++){
uint8_t* pnt=(uint8_t*)(&txL);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
- pc->printf("TXLONG MISO Response: ");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"TXLONG MISO Response: ");
for(int i=0;i<(sizeof(PACK_LONG1));i++){
uint8_t* pnt=(uint8_t*)(&rxL);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
//printf("last RXL = \n");
//for (i = 0; i < 300; i++) {
- // pc->printf("%02x", rxL.buf[i]);
+ // debug_print(pc,"%02x", rxL.buf[i]);
// if (((i + 1) % 8) == 0)
- // pc->printf("\n");
+ // debug_print(pc,"\n");
//}
//printf("\n");
res = -9;
@@ -895,7 +904,7 @@
uint32_t val = 0;
uint16_t * u16ptr;
- //pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
+ //debug_print(pc,"Buffer space required: %d\n\r", MAXTRCLEN);
// Required buffer space
if (maxN < MAXTRCLEN){
pc-printf("NOT ENOUGH BUFFER SPACEn\r");
@@ -905,7 +914,7 @@
// 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);
+ //debug_print(pc,"npack: %d", nPack);
// Ensure transmitted packet is all zeros to not send trash
memset(&txL, 0, sizeof(PACK_LONG1));
@@ -917,7 +926,7 @@
if (res < 0)
goto END;
val >>= 16;
- //pc->printf("chsel = %d\n", val);
+ //debug_print(pc,"chsel = %d\n", val);
cnt = 0;
// Count how many channels are selected
for (i = 0; i < 16; i++) {
@@ -929,11 +938,11 @@
nPack /= 16;
res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
-// pc->printf("PORT_OVR = %d\n", (val >> 16));
+// debug_print(pc,"PORT_OVR = %d\n", (val >> 16));
val = (val >> 16) & 3; // Get bits 0 and 1
if (res < 0){
- pc->printf("ReadReg Error1\n\r");
+ debug_print(pc,"ReadReg Error1\n\r");
goto END;}
if (val == 0){ //00 = 1
@@ -949,19 +958,19 @@
nPack /= 1;
}
else {
- pc->printf("GetTrace: bad value\n");
+ debug_print(pc,"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");
+ debug_print(pc,"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");
+ debug_print(pc,"WriteReg Error3\n\r");
goto END;}
// Wait till PORT_READY bit is set.
@@ -985,19 +994,19 @@
if (res < 0)
goto END;
- pc->printf("MOSI ReqReadTrace:\t 0x");
+ debug_print(pc,"MOSI ReqReadTrace:\t 0x");
for(int i=0;i<(sizeof(tx));i++){
uint8_t* pnt=(uint8_t*)tx;
- pc->printf("%02X", *(pnt+i));
+ debug_print(pc,"%02X", *(pnt+i));
}
- pc->printf("\n");
- pc->printf("MISO ReqReadTrace:\t 0x");
+ debug_print(pc,"\n");
+ debug_print(pc,"MISO ReqReadTrace:\t 0x");
for(int i=0;i<(sizeof(tx));i++){
uint8_t* pnt=(uint8_t*)rx;
- pc->printf("%02X", *(pnt+i));
+ debug_print(pc,"%02X", *(pnt+i));
}
- pc->printf("\n");
- pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n");
+ debug_print(pc,"\n");
+ debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n");
//----PACKET EXCHANGE
//--------------------------------------------------------------------------------------
@@ -1025,8 +1034,8 @@
// 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]);
+ // 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]);
+ // 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]);
if (res < 0){
res = -8;
goto END;
@@ -1036,27 +1045,27 @@
// 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: ");
+ debug_print(pc,"LONG READ ERROR: Stopped at the %d long message\n", a);
+ debug_print(pc,"TXLONG MOSI Response: ");
for(int i=0;i<(sizeof(PACK_LONG1));i++){
uint8_t* pnt=(uint8_t*)(&txL);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
- pc->printf("TXLONG MISO Response: ");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"TXLONG MISO Response: ");
for(int i=0;i<(sizeof(PACK_LONG1));i++){
uint8_t* pnt=(uint8_t*)(&rxL);
- pc->printf("%02X ", *(pnt+i));
+ debug_print(pc,"%02X ", *(pnt+i));
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- --\n\r");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- --\n\r");
//printf("last RXL = \n");
//for (i = 0; i < 300; i++) {
- // pc->printf("%02x", rxL.buf[i]);
+ // debug_print(pc,"%02x", rxL.buf[i]);
// if (((i + 1) % 8) == 0)
- // pc->printf("\n");
+ // debug_print(pc,"\n");
//}
//printf("\n");
res = -9;
@@ -1071,21 +1080,21 @@
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);
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+i));
+ if(((i %30) ==0)&&(i>0))debug_print(pc,"\n\t\t\t\t\t");
}
- pc->printf("\n\r");
- pc->printf("Trace packet %d MISO: \n\t\t\t\t\t0x", a);
+ debug_print(pc,"\n\r");
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+i));
+ if(((i %30) ==0)&&(i>0))debug_print(pc,"\n\t\t\t\t\t");
}
- pc->printf("\n\r");
- pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
}
}
@@ -1099,6 +1108,7 @@
int LidarSpi::LoadPatch (const char *patch, Serial * pc){
+ debug_print(pc,"Entered LiadPatch method....\n\r");
int res=0;
uint16_t rSz;
PACK_SHORT rx[1], tx[1];
@@ -1109,8 +1119,9 @@
uint16_t nWordsRemaining;
uint16_t nPack;
uint16_t nWords;
-
+ debug_print(pc,"Allocating memory....\n\r");
uint8_t memory[15000];
+ debug_print(pc,"Memory allocated\n\r");
uint16_t addrStart, startLine, nBytes ;
startLine=0;
memset(memory, 0, 15000);
@@ -1118,25 +1129,25 @@
int count=0;
res=WriteReg(0x1d8, 128);
if (res<0){
- pc->printf("PORT_LONG_PACKET_SIZE Write error\n\r");
+ debug_print(pc,"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");
+ debug_print(pc,"REG_MLX16_ITC_MASK0 Write error\n\r");
}
wait_us(5);
- pc->printf("Start loading fragments \n\r");
+ debug_print(pc,"Start loading fragments \n\r");
LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
Trigger(0);
- pc->printf("Load fragment %d\n\r", count);
+ debug_print(pc,"Load fragment %d\n\r", count);
for(int i =0; i<nBytes;i++){
- //pc->printf("Addr[%d] : %d\n\r", addrStart+i, memory[i] );
+ //debug_print(pc,"Addr[%d] : %d\n\r", addrStart+i, memory[i] );
}
- pc->printf("addrStart: %d, startLine %d, nBytes: %d\n\r",addrStart, startLine, nBytes);
+ debug_print(pc,"addrStart: %d, startLine %d, nBytes: %d\n\r",addrStart, startLine, nBytes);
nWords= nBytes / 2;
// Ensure all packets are all zeros to not send trash
@@ -1146,34 +1157,34 @@
//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);
+ debug_print(pc,"npack: %d\n\r", nPack);
// Encode the request and send it
res = MLX_ReqWriteFW(tx, nPack, addrStart);
if (res < 0){
- pc->printf("Err @ 1");
+ debug_print(pc,"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");
+ debug_print(pc,"Err @ 2");
goto END; }
//Trigger(1);
- pc->printf("ReqWriteFirmware MOSI: \n\r0x");
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r");
- pc->printf("ReqWriteFirmware MISO: \n\r0x");
+ debug_print(pc,"\n\r");
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r-------------------\n\r");
+ debug_print(pc,"\n\r-------------------\n\r");
@@ -1182,15 +1193,15 @@
// Optional status decoding
res = MLX_DecodeStatusS(rx, &iserr, &err);
if (res < 0 || iserr){
- pc->printf("Err @ 3");
+ debug_print(pc,"Err @ 3");
- pc->printf("LOAD PATCH REQ response MISO: \n\r0x");
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r");
+ debug_print(pc,"\n\r");
goto END; }
@@ -1216,41 +1227,41 @@
wait_us(12);
/*
- pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d MOSI: \n\r0x", a,count);
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r");
- pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d MISO: \n\r0x", a,count);
+ debug_print(pc,"\n\r");
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r-----------\n\r");
+ debug_print(pc,"\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);
+ debug_print(pc,"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);
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r");
- pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", a);
+ debug_print(pc,"\n\r");
+ debug_print(pc,"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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r");
+ debug_print(pc,"\n\r");
res = -9;
goto END;
}
@@ -1270,7 +1281,7 @@
res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
wait_us(11);
if (res < 0){
- pc->printf("Err @ 4");
+ debug_print(pc,"Err @ 4");
goto END; }
@@ -1280,37 +1291,37 @@
wait_us(12);
- pc->printf("Status long packet to check status after last fragment : %d, res=%d\n", count, res);
+ debug_print(pc,"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);
+ debug_print(pc,"LOAD PATCH LONG STATUS %d MOSI: \n\r0x", 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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r");
- pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", count);
+ debug_print(pc,"\n\r");
+ debug_print(pc,"LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", 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");
+ debug_print(pc,"%02X", *(pnt+k));
+ if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
}
- pc->printf("\n\r-----------\n\r");
+ debug_print(pc,"\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);
+ //debug_print(pc,"Return from a loadPatchFragment: res = %d \n\r", res);
wait_ms(100);
if (true){
- pc->printf("Change jumptable... \n\r");
+ debug_print(pc,"Change jumptable... \n\r");
res = WriteReg(0x1000, 0x7000, pc); // write addr: 0x1000 value:0x7000
if (res < 0){
- pc->printf("Err @ 5");
+ debug_print(pc,"Err @ 5");
goto END;
}
}
- else pc->printf("Failed to read the file\n\r");
+ else debug_print(pc,"Failed to read the file\n\r");
END:
return res;
@@ -1329,12 +1340,10 @@
uint16_t i, total = 0, lineno = 1;
uint16_t minaddr = 65535, maxaddr = 0;
- uint16_t prevAddr = 0;
uint16_t lineCount = *startLine;
uint16_t lines = 0;
- bool isFirst = true;
-
+
*nBytes = 0;
if (strlen(patch) == 0) {
//printf(" Can't load a file without the filename.");
@@ -1342,45 +1351,45 @@
}
fin = fopen(patch, "r");
if (fin == NULL) {
- pc->printf(" Can't open file '%s' for reading.\n\r", patch);
+ debug_print(pc," Can't open file '%s' for reading.\n\r", patch);
return -10;
}
- pc->printf("Opened file\n\r");
+ debug_print(pc,"Opened file\n\r");
// printf("Patch file %s opened\n", filename);
while (true) {
line[0] = '\0';
fgets(line, 1000, fin);
lines++;
- //pc->printf("Startline: %d, lines: %d\n\r", *startLine,lines);
+ //debug_print(pc,"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;
//int res=parse_hex_line(line, bytes, &addr, &n, &status);
- //pc->printf("Result of parse: %d\n\r", res);
+ //debug_print(pc,"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);
+ //debug_print(pc,"Line: %s\n\r", line);
if (*nBytes>15000) {
*addrStart = minaddr;
*startLine = lines;
fclose(fin);
- pc->printf("Close file\n\r");
+ debug_print(pc,"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;
+ //debug_print(pc,"addr: %d, minaddr: %d\n\r", addr, minaddr);
+ //isFirst = false;
if (addr < minaddr) minaddr = addr;
for (i = 0; i <= (n - 1); i++) {
- prevAddr = addr;
+ //prevAddr = addr;
if (i % 2 == 0)
memory[addr-minaddr] = bytes[i + 1] & 255; //Assumption even no of bytes per line
@@ -1395,14 +1404,14 @@
}
if (status == 1) { /* end of file */
fclose(fin);
- pc->printf("Closed file\n\r");
+ debug_print(pc,"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);
+ debug_print(pc,"minAddr: %d, maxAddr: %d\n\r", minaddr, maxaddr);
*nBytes=maxaddr-minaddr;
- pc->printf("End of file\n\r");
+ debug_print(pc,"End of file\n\r");
return total;
}
if (status == 2) /* begin of file */
@@ -1414,10 +1423,10 @@
}
lineCount++;
NEXT: lineno++;
- }
+ }/*
fclose(fin);
- pc->printf("Close file\n\r");
- return 0;
+ debug_print(pc,"Close file\n\r");
+ return 0;*/
}
int LidarSpi::parse_hex_line(char *theline, uint8_t bytes[], uint16_t *addr, uint16_t *num, uint16_t *code){
@@ -1460,89 +1469,89 @@
{
uint16_t val=148;
int res=0;
- //pc->printf("Write PORT_LONG_PACKET_SIZE: %d\n\r",val);
+ //debug_print(pc,"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");
+ //debug_print(pc,"PORT_LONG_PACKET_SIZE Write error\n\r");
return res;
}
- //pc->printf("Packet size: %d\n\r", val);
+ //debug_print(pc,"Packet size: %d\n\r", val);
- //pc->printf("Write REG_PORT_CHSEL: 0xFFFF\n\r");
+ //debug_print(pc,"Write REG_PORT_CHSEL: 0xFFFF\n\r");
res=WriteReg(0x150, 0xffff);
if (res<0) {
- //pc->printf("REG_PORT_CHSEL Write error\n\r");
+ //debug_print(pc,"REG_PORT_CHSEL Write error\n\r");
return res;
}
- //pc->printf("Write REG_PORT_OVR_ACCUM: 0x4B3,\n\r");
+ //debug_print(pc,"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");
+ //debug_print(pc,"REG_PORT_OVR_ACCUM Write error\n\r");
return res;
}
- //pc->printf("Write REG_PORT_FIXED_DIVIDER: 0x802\n\r");
+ //debug_print(pc,"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");
+ //debug_print(pc,"REG_PORT_FIXED_DIVIDER Write error\n\r");
return res;
}
- //pc->printf("Write REG_PORT_TRIGGER_PERIOD: 0x3E8\n\r");
+ //debug_print(pc,"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");
+ //debug_print(pc,"REG_PORT_TRIGGER_PERIOD Write error\n\r");
return res;
}
- //pc->printf("Write REG_PORT_MEAS_DELAY: 0x0000\n\r");
+ //debug_print(pc,"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");
+ //debug_print(pc,"REG_PORT_MEAS_DELAY Write error\n\r");
return res;
}
- //pc->printf("Write REG_PORT_MODE_EN: 0x0000\n\r");
+ //debug_print(pc,"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");
+ //debug_print(pc,"REG_PORT_MODE_EN Write error\n\r");
return res;
}
- //pc->printf("Write REG_MLX16_ITC_MASK0: 0x7C34\n\r");
+ //debug_print(pc,"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");
+ //debug_print(pc,"REG_MLX16_ITC_MASK0 Write error\n\r");
return res;
}
- //pc->printf("Write REG_STIMER2_VALUE: 0xBFFF\n\r");
+ //debug_print(pc,"Write REG_STIMER2_VALUE: 0xBFFF\n\r");
res=WriteReg(0x22, 49151); //0xBFFF
if (res<0) {
- //pc->printf("REG_STIMER2_VALUE Write error\n\r");
+ //debug_print(pc,"REG_STIMER2_VALUE Write error\n\r");
return res;
}
/*
- //pc->printf("Write REG_PORT_FIXED_DIVIDER: 0x802\n\r");
+ //debug_print(pc,"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");
+ //debug_print(pc,"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");
+ //debug_print(pc,"Improvement registers for trace without patch\n\r");
res=WriteReg(0x124, 0xae40); //0x802
if (res<0) {
- //pc->printf("PORT SH1 Write error\n\r");
+ //debug_print(pc,"PORT SH1 Write error\n\r");
return res;
}
res=WriteReg(0x126, 0xae40); //0x802
if (res<0) {
- //pc->printf("PORT SH2 Write error\n\r");
+ //debug_print(pc,"PORT SH2 Write error\n\r");
return res;
}*/
@@ -1620,10 +1629,18 @@
//Peak Min Pulse edge len
res=WriteReg(0x1c80,0xe);
- if (res<0) return res;
-
-
-
-
+ return res;
}
+int LidarSpi::setLed(bool state){
+ int res=0;
+ if(state){
+ WriteReg(0x1AC, 0x08); //PWM1 OFF
+ res=WriteReg(0x154, 0x19); //LED1 ON
+ }
+ else{
+ WriteReg(0x1AC, 0x00); //PWM0 and 1 OFF
+ res=WriteReg(0x154, 0x11); //LED1 OFF
+ }
+ return res;
+}