Tom N
This is a class which contains function to interface with the MLX75320
Diff: LidarSpi.cpp
- Revision:
- 0:dfe498e03679
- Child:
- 1:e3ace426cee5
- Child:
- 2:7edbd37d81fd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LidarSpi.cpp Thu Feb 25 08:02:11 2016 +0000
@@ -0,0 +1,1145 @@
+#include "mbed.h"
+#include "LidarSpi.h"
+
+//#include "ToolsClass.h"
+#include <string.h>
+#include "LidarSpi.h"
+#include "MLX_BaseSPI.h"
+#define SPIFREQ 8000
+LidarSpi::LidarSpi(PinName mosi, PinName miso, PinName clk, PinName chipSelect, PinName dr, PinName rs, PinName tr):device(mosi, miso, clk), chipS(chipSelect), dataReady(dr),resetPin(rs), trigger(tr)
+{
+ //resetPin.write(1);
+ chipS.write(1);
+
+ //8 bit
+ //Mode 1: CPPOL=0, CLPHA=1 => Default of lidar, mbed side receiving is bad near 25Mhz
+ // -> 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.frequency(16000000);
+}
+
+
+int LidarSpi::BasicRead(){
+ return device.write(0x6B);
+
+}
+
+
+int LidarSpi::SpiSetting(long freq, int mode, Serial* pc){
+ int cPha= mode & 0x01;
+ int cPol=(mode>>1) & 0x01;
+ uint32_t val=0;
+ int res=0;
+ res=ReadReg(0x10E, &val,pc);
+ if(res<0) return -1;
+ val=val>>16;
+ val=val | (cPol<<5) | (cPha<<6);
+ wait_us(4);
+ res=WriteReg(0x10E, val, pc);
+ wait_us(4);
+ device.format(8, mode);
+ res=ReadReg(0x10E, &val,pc);
+
+ device.frequency(freq);
+ return res;
+}
+
+
+//int LidarSpi::BasicTransfer(uint8_t* rData, uint16_t rSz, const uint8_t* tData, uint16_t tSz, const event_callback_t callback){
+// device.transfer(tData,tSz,rData,rSz,callback,event=SPI_EVENT_COMPLETE);
+// //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)
+// device.transfer();
+// return 0;
+//}
+
+
+int LidarSpi::TxPacket(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz){
+ chipS=0;
+ int i =0;
+ for(i=0; i< tSz;i++){
+ *rData=device.write(*tData);
+ rData++;
+ tData++;
+ }
+ chipS=1;
+ *rSz=i;
+ return 0;
+}
+
+int LidarSpi::TxPacketWord(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz){
+ chipS=0;
+ int i =0;
+ uint16_t* recPoint=(uint16_t*)rData;
+ uint16_t* transPoint=(uint16_t*)tData;
+
+ for(i=0; i< tSz/2;i++){
+ *recPoint=device.write(*transPoint);
+ recPoint++;
+ transPoint++;
+ }
+ chipS=1;
+ *rSz=i*2;
+ return 0;
+}
+
+
+
+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: ");
+ 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));
+ //rData++;
+ //tData++;
+ }
+ //pc->printf("\n\r");
+ chipS=1;
+ *rSz=i;
+ return 0;
+}
+
+int LidarSpi::TxPacketSlow(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz, uint16_t usDelay){
+ int res=TxPacket(rData, rSz, tData, tSz);
+ wait_us(usDelay);
+ return res;
+}
+
+int LidarSpi::ReadReg(uint32_t reg, uint32_t *val)
+{
+ int res;
+ uint16_t rSz;
+ PACK_SHORT rx[2], tx[2];
+
+ // Ensure all packets are all zeros to not send trash
+ memset(tx, 0, sizeof(tx));
+
+ // Encode the request and send it
+ res = MLX_ReqReadReg(tx, (uint32_t)reg);
+ if (res < 0)
+ return -1;
+
+ // tx[1].hdr = 0x0040;
+ // tx[1].crc = 0x106a;
+ res = MLX_EncodeStatusS(tx + 1, 0, 0);
+ if (res < 0)
+ return -2;
+
+ res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx)/2);
+ wait_us(5);
+ res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2);
+ if (res < 0)
+ return -3;
+
+ // Decode the response packet with register value
+ res = MLX_DecodeResS(rx + 1, val);
+ if (res < 0)
+ return res;
+ //return sizeof(tx);
+
+ return 0;
+}
+
+int LidarSpi::ReadReg(uint32_t reg, uint32_t *val, Serial* pc)
+{
+ int res;
+ uint16_t rSz;
+ PACK_SHORT rx[2], tx[2];
+
+ // Ensure all packets are all zeros to not send trash
+ memset(tx, 0, sizeof(tx));
+
+ // Encode the request and send it
+ res = MLX_ReqReadReg(tx, (uint32_t)reg);
+ if (res < 0)
+ return -1;
+
+ // tx[1].hdr = 0x0040;
+ // tx[1].crc = 0x106a;
+ res = MLX_EncodeStatusS(tx + 1, 0, 0);
+ if (res < 0)
+ return -2;
+
+ res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc);
+ wait_us(5);
+ res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc);
+ if (res < 0)
+ return -3;
+
+ pc->printf("Read register request and response\n\r");
+
+ pc->printf("MOSI ReadRegRequest:\t");
+ for(int i=0;i<(sizeof(tx)/2);i++){
+ uint8_t* pnt=(uint8_t*)tx;
+ pc->printf("%02X ", *(pnt+i));
+ }
+ pc->printf("\n\r");
+ pc->printf("MISO ReadRegRequest:\t");
+ for(int i=0;i<(sizeof(tx)/2);i++){
+ uint8_t* pnt=(uint8_t*)rx;
+ pc->printf("%02X ", *(pnt+i));
+ }
+ pc->printf("\n\r");
+ pc->printf("-- -- -- -- -- -- --\n\r");
+
+
+ pc->printf("MOSI ReadRegResponse:\t");
+ for(int i=0;i<(sizeof(tx)/2);i++){
+ uint8_t* pnt=(uint8_t*)(tx+1);
+ pc->printf("%02X ", *(pnt+i));
+ }
+ pc->printf("\n\r");
+ pc->printf("MISO ReadRegResponse:\t");
+ for(int i=0;i<(sizeof(tx)/2);i++){
+ uint8_t* pnt=(uint8_t*)(rx+1);
+ pc->printf("%02X ", *(pnt+i));
+ }
+ pc->printf("\n\r");
+ pc->printf("-- -- -- -- -- -- --\n\r");
+
+ // Decode the response packet with register value
+ res = MLX_DecodeResS(rx + 1, val);
+ if (res < 0)
+ if(res==-6) {
+ 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);
+ return res;
+ }
+ else return res;
+ //return sizeof(tx);
+ return 0;
+}
+
+int LidarSpi::WriteReg(uint32_t reg, uint32_t val)
+{
+ return WriteRegSpeed(reg, val, START_DELAY);
+}
+
+// Add a speed input so that we can control delay between packets
+int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed)
+{
+ int res;
+ uint16_t rSz;
+ uint8_t iserr;
+ uint16_t error;
+ PACK_SHORT rx[2], tx[2];
+
+ // Ensure all packets are all zeros to not send trash
+ memset(tx, 0, sizeof(tx));
+
+ // Encode the request and send it
+ res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val);
+ if (res < 0)
+ return res;
+
+ res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2);
+ wait_us(5);
+ res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2);
+ if (res < 0)
+ return res;
+
+
+
+ // Decode response (a status packet)
+ res = MLX_DecodeStatusS(rx + 1, &iserr, &error);
+ if (res < 0 || iserr)
+ return res;
+
+ return 0;
+}
+
+int LidarSpi::WriteReg(uint32_t reg, uint32_t val, Serial* pc)
+{
+ return WriteRegSpeed(reg, val, START_DELAY, pc);
+}
+
+// Add a speed input so that we can control delay between packets
+int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed, Serial* pc)
+{
+ int res;
+ uint16_t rSz;
+ uint8_t iserr;
+ uint16_t error;
+ PACK_SHORT rx[2], tx[2];
+
+ // Ensure all packets are all zeros to not send trash
+ memset(tx, 0, sizeof(tx));
+
+ // Encode the request and send it
+ res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val);
+ if (res < 0)
+ return res;
+
+ /*
+ res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), speed);
+ if (res < 0)
+ return res;
+ */
+ res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc);
+ wait_us(5);
+ res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc);
+ if (res < 0)
+ return -3;
+
+ //pc->printf("Write register request and response\n\r");
+
+ pc->printf("MOSI WriteRegRequest:\t 0x");
+ for(int i=0;i<(sizeof(tx)/2);i++){
+ uint8_t* pnt=(uint8_t*)tx;
+ pc->printf("%02X", *(pnt+i));
+ }
+ pc->printf("\n\r");
+ pc->printf("MISO WriteRegRequest:\t 0x");
+ for(int i=0;i<(sizeof(tx)/2);i++){
+ uint8_t* pnt=(uint8_t*)rx;
+ pc->printf("%02X", *(pnt+i));
+ }
+ pc->printf("\n\r");
+ pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
+
+
+ pc->printf("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));
+ }
+ pc->printf("\n\r");
+ pc->printf("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));
+ }
+ pc->printf("\n\r");
+ pc->printf("==========================================================\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);
+ return res;}
+
+ return 0;
+}
+int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, uint16_t mode)
+{
+ trigger.write(0);
+ int res, a, b;
+ uint16_t rSz;
+ uint32_t cnt;
+ PACK_SHORT rx[1], tx[1];
+ PACK_LONG2 rxL, txL;
+ uint8_t iserr;
+ uint16_t err;
+ uint16_t idx = 0;
+ uint32_t val = 0;
+ uint16_t buf[MAXCH*4*BYTES_PER_ECH/2];
+
+ uint16_t * u16ptr ;
+
+
+ // Required buffer space
+ if (maxN < MAXECH)
+ return -1;
+
+ // Echo data is transmitted in 128 word payload => PACK_LONG2
+ // Each echo is X bytes, divide by payload size to get number of packets
+ const int nEchoes = MAXCH * 4;
+ const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ;
+ const int WORDS_PER_ECH = BYTES_PER_ECH/2;
+
+ // Ensure transmitted packet is all zeros to not send trash
+ memset(&txL, 0, sizeof(PACK_LONG2));
+ memset(tx, 0, sizeof(tx));
+
+ //res = MLXSPI::SetConfig(0); //debug
+
+ // Write 1 to PORT_ACQU register and then wait
+ res = ReadReg(0x146, &val); // PORT_ACQU
+ if (res < 0)
+ goto END;
+ val = (val >> 16) | 1;
+ trigger.write(1);
+ res = WriteReg(0x146, val); // PORT_ACQU
+ if (res < 0)
+ 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++;
+ }
+ trigger.write(0);
+ // Encode the request and send it
+ res = MLX_ReqReadEch(tx);
+ if (res < 0)
+ goto END;
+
+ //--------------------------------------------------------------------------------------
+ //----SHORT PACKET EXCHANGE--------
+ res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
+ 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*)rxL.data;
+
+
+ res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
+ if (res < 0){
+ res = -7;
+ goto END;
+ }
+ for (a = 0; a < nPack; ++a)
+ {
+
+ wait_us(10);
+ //Tools::Wait(10);
+ // Clock the remaining long packets
+ //--------------------------------------------------------------------------------------
+ //----LONG PACKET EXCHANGE--------
+ res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
+ if (res < 0)
+ goto END;
+
+ // Decode the long responses, then extract data values
+ res = MLX_DecodeResL2(&rxL);
+ if (res < 0)
+ goto END;
+
+ // 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)
+ {
+ //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
+ }
+ }
+
+ // Do something with the data... decode an echo, manage the empty echo case
+ for (b = 0; b < nEchoes; ++b) {
+
+ ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
+ ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
+ ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
+ ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
+ ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
+ ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
+ ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
+ ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
+
+ ++idx;
+ }
+
+ res = idx;
+
+END:
+
+ return res;
+}
+
+
+int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, uint16_t mode, Serial* pc)
+{
+ trigger.write(0);
+ int res, a, b;
+ uint16_t rSz;
+ uint32_t cnt;
+ PACK_SHORT rx[1], tx[1];
+ PACK_LONG2 rxL, txL;
+ uint8_t iserr;
+ uint16_t err;
+ uint16_t idx = 0;
+ uint32_t val = 0;
+ uint16_t buf[MAXCH*4*BYTES_PER_ECH/2];
+ memset(buf, 0, MAXCH*4*BYTES_PER_ECH/2);
+ uint16_t * u16ptr ;
+
+
+ // Required buffer space
+ if (maxN < MAXECH){
+ pc->printf("maxN too small\n\r");
+ return -1;
+ }
+
+ // Echo data is transmitted in 128 word payload => PACK_LONG2
+ // Each echo is X bytes, divide by payload size to get number of packets
+ const int nEchoes = MAXCH * 4;
+ const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ;
+ const int WORDS_PER_ECH = BYTES_PER_ECH/2;
+
+ // Ensure transmitted packet is all zeros to not send trash
+ memset(&txL, 0, sizeof(PACK_LONG2));
+ memset(tx, 0, sizeof(tx));
+
+ //res = MLXSPI::SetConfig(0); //debug
+
+ // Write 1 to PORT_ACQU register and then wait
+ pc->printf("\tRead PORT_ACQU\n\r");
+ res = ReadReg(0x146, &val); // PORT_ACQU
+ if (res < 0){
+ pc->printf("ReadReg Error\n\r");
+ goto END;}
+ val = (val >> 16) | 1;
+ pc->printf("\tWrite 1 to PORT_ACQU\n\r");
+ trigger.write(1);
+ res = WriteReg(0x146, val); // PORT_ACQU
+ if (res < 0){
+ pc->printf("WriteReg Error\n\r");
+ goto END;}
+
+ // Wait till PORT_READY bit is set.
+
+ pc->printf("\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;
+ while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
+ wait_us(50);
+ res = ReadReg(470, &val); // PORT_READY
+ while(res<0){
+ wait_us(4);
+ res = ReadReg(470, &val);
+ }
+ cnt++;
+ }
+ pc->printf("Counter: %d\n\r", cnt);
+
+ // Encode the request and send it
+ res = MLX_ReqReadEch(tx);
+ if (res < 0){
+ pc->printf("ReqreadEch error\n\r");
+ goto END;}
+
+ //--------------------------------------------------------------------------------------
+ //----SHORT PACKET EXCHANGE--------
+ pc->printf("\tSend ReqReadEch\n\r");
+ res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
+ if (res < 0){
+ pc->printf("txPacketSlow Error\n\r");
+ goto END;}
+ /*
+ pc->printf("\tFirmware read request - processed\n\r");
+ pc->printf("Firmware readRequest MOSI: \t");
+ for(int i =0; i<sizeof(tx);i++) {
+ uint8_t* pnt=(uint8_t*)(&tx);
+ pc->printf("%02X ", *(pnt+i));
+ }
+ pc->printf("\n\r");
+ pc->printf("Firmware readRequest MISO: \t");
+ for(int i =0; i<sizeof(rx);i++) {
+ uint8_t* pnt=(uint8_t*)(&rx);
+ pc->printf("%02X ", *(pnt+i));
+ }
+ pc->printf("\n\r");
+ pc->printf("-- -- -- -- -- -- --\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");
+ goto END;}
+
+ // 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)
+ {
+
+ if (res < 0){
+ res = -7;
+ pc->printf("Problem creating echo status\n\r");
+ goto END;
+ }
+ wait_us(10);
+ //Tools::Wait(10);
+ // Clock the remaining long packets
+ //--------------------------------------------------------------------------------------
+ //----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);
+ goto END;}
+
+
+ // Decode the long responses, then extract data values
+ res = MLX_DecodeResL2(&rxL);
+ if ((res < 0)|| a==5){
+
+ pc->printf("\n\r");
+ pc->printf("-- -- -- -- -- -- --\n\r");
+ pc->printf("Packet #%d => Decode long response error \n\r", a);
+
+ pc->printf("TXLONG MOSI Response: ");
+ for(int i=0;i<(sizeof(PACK_LONG2));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_LONG2));i++){
+ uint8_t* pnt=(uint8_t*)(&rxL);
+ pc->printf("%02X ", *(pnt+i));
+
+ }
+ pc->printf("\n\r");
+ pc->printf("-- -- -- -- -- -- --\n\r");
+ pc->printf("Packet #%d => Decode long response error \n\r", a);
+ goto END;}
+
+ // 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)
+ {
+ //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
+ }
+ //prevTX=txL;
+ //prevRX=rxL;
+ }
+
+ // Do something with the data... decode an echo, manage the empty echo case
+ for (b = 0; b < nEchoes; ++b) {
+
+ ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
+ ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
+ ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
+ ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
+ ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
+ ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
+ ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
+ ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
+
+ ++idx;
+ }
+
+ res = idx;
+ trigger.write(0);
+
+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;
+}
+
+
+int LidarSpi::LoadPatch ( uint16_t address, uint8_t *buf, uint16_t nBytes){
+ int res;
+ uint16_t rSz;
+ PACK_SHORT rx[1], tx[1];
+ PACK_LONG2 rxL, txL;
+ uint8_t iserr;
+ uint16_t err;
+ //uint32_t val = 0;
+ uint16_t nWordsRemaining;
+ uint16_t nPack;
+ uint16_t nWords = nBytes / 2;
+
+
+ // Ensure all packets are all zeros to not send trash
+ memset(tx, 0, sizeof(tx));
+ memset(&rxL, 0, sizeof(rxL));
+
+ //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
+ //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
+ nPack = nWords / (MLX_LONG2_DATA_SZ / 2) + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0 :1);
+
+ // Encode the request and send it
+ res = MLX_ReqWriteFW(tx, nPack, address);
+ if (res < 0)
+ goto END;
+
+ res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
+ if (res < 0)
+ goto END;
+
+ // Optional status decoding
+ res = MLX_DecodeStatusS(rx, &iserr, &err);
+ if (res < 0 || iserr)
+ goto END;
+
+ nWordsRemaining = nWords;
+ for (uint a = 0; a < nPack; ++a)
+ {
+ uint size;
+ if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
+ size = MLX_LONG2_DATA_SZ / 2;
+ else
+ size = nWordsRemaining;
+
+ res = MLX_WriteDataL2(&txL, size, a, &buf[a*MLX_LONG2_DATA_SZ]);
+ if (res < 0){
+ res = -7;
+ goto END;
+ }
+
+ //Tools::Wait(10);
+
+ // Clock the remaining long packets
+ res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
+
+ // Decode the long responses
+ res = MLX_DecodeResL2(&rxL);
+ if (res < 0){
+ printf("LONG WRITE ERROR: Stopped at the %d long message\n", a);
+ res = -9;
+ goto END;
+ }
+
+ nWordsRemaining = nWords - size;
+ }
+
+ //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
+ res = MLX_EncodeStatusL2(&txL, 0, 0);
+ if (res < 0) {
+ res = -7;
+ goto END;
+ }
+
+ //Tools::Wait(10);
+ // Clock the remaining long packets
+ res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
+
+ if (res < 0)
+ goto END;
+
+ // Change jump table pointer.
+ res = WriteReg(0x1000, 0x7000); // write addr: 0x1000 value:0x7000
+ if (res < 0)
+ goto END;
+
+END:
+ return res;
+}
+
+void LidarSpi::Trigger(int level){
+ trigger.write(level);
+}