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