File content as of revision 5:87e211a23654:

#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, PinName smpl):device(mosi, miso, clk), chipS(chipSelect), dataReady(dr),resetPin(rs), trigger(tr), sampling(smpl)
{
    //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);
    wait_us(5);
    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);
    wait_us(5);
    return 0;
}

int LidarSpi::WriteReg(uint32_t reg, uint32_t val)
{
    int res=WriteRegSpeed(reg, val, START_DELAY);
    wait_us(5);
    return res;
    
}

// 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)
{
    int res=WriteRegSpeed(reg, val, START_DELAY, pc);
    wait_us(5);
    return res;
}

// 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.
    trigger.write(1);
    // Wait till PORT_READY bit is set.


    //res = ReadReg(470, &val);     // PORT_READY 
    cnt = 0;
    
    /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
        wait_us(50);
        res = ReadReg(470, &val);     // PORT_READY
        cnt++;
    } */   
    while((!dataReady.read())&& (cnt<2000)){
        wait_us(50);
        cnt++;
    }
    
    
    res = ReadReg(470, &val);     // PORT_READY 
    val=val>>16;
    pc->printf("PORT READY: %x\n\r",val);
    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;
    trigger.write(1);
    // Wait till PORT_READY bit is set.


    //res = ReadReg(470, &val);     // PORT_READY 
    cnt = 0;
    
    /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
        wait_us(50);
        res = ReadReg(470, &val);     // PORT_READY
        cnt++;
    } */   
    while((!dataReady.read())&& (cnt<2000)){
        wait_us(50);
        cnt++;
    }
    
    
    res = ReadReg(470, &val);     // PORT_READY 
    val=val>>16;
    pc->printf("PORT READY: %x\n\r",val);
    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)){

            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); ++b)
        {
            //buf[a*(MLX_LONG2_DATA_SZ / 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;
    }

    // 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){
    trigger.write(0);
    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;}
    trigger.write(1);
    // Wait till PORT_READY bit is set.


    //res = ReadReg(470, &val);     // PORT_READY 
    cnt = 0;
    
    /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
        wait_us(50);
        res = ReadReg(470, &val);     // PORT_READY
        cnt++;
    } */   
    while((!dataReady.read())&& (cnt<2000)){
        wait_us(50);
        cnt++;
    }
    
    
    res = ReadReg(470, &val);     // PORT_READY 
    val=val>>16;
    pc->printf("PORT READY: %x\n\r",val);
    
    trigger.write(0);
    pc-printf("Count: %d\n\r", 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);    
}