File content as of revision 13:ccf4ab73c33d:

#include "mbed.h"
#include "LidarSpi.h"

//#include "ToolsClass.h"
#include <string.h>
#include "LidarSpi.h"
#include "MLX_BaseSPI.h"
#include <inttypes.h>
#define SPIFREQ 8000

//Set DEBUG to 1 to enable debug printf over Serial interface
#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);
    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=0, CLPHA=1
    device.frequency(16000000);
    resetPin.write(0);
    wait_ms(300);
    resetPin.write(1);
    
}


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;
    //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));
        //debug_print(pc,"%02X", *(rData+i));
        //rData++;
        //tData++;
    }
    //debug_print(pc,"\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;
        
    debug_print(pc,"Read register request and response\n\r");
    
    debug_print(pc,"MOSI ReadRegRequest:\t");
    for(int i=0;i<(sizeof(tx)/2);i++){
        uint8_t* pnt=(uint8_t*)tx;
        debug_print(pc,"%02X ", *(pnt+i));   
    }
    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;
        debug_print(pc,"%02X ", *(pnt+i));   
    }
    debug_print(pc,"\n\r");
    debug_print(pc,"-- -- -- -- -- -- --\n\r");
    
    
    debug_print(pc,"MOSI ReadRegResponse:\t");
    for(int i=0;i<(sizeof(tx)/2);i++){
        uint8_t* pnt=(uint8_t*)(tx+1);
        debug_print(pc,"%02X ", *(pnt+i));   
    }
    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);
        debug_print(pc,"%02X ", *(pnt+i));   
    }
    debug_print(pc,"\n\r");
    debug_print(pc,"-- -- -- -- -- -- --\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);
            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;
        //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;

    //debug_print(pc,"Write register request and response\n\r");
    
    debug_print(pc,"MOSI WriteRegRequest:\t 0x");
    for(int i=0;i<(sizeof(tx)/2);i++){
        uint8_t* pnt=(uint8_t*)tx;
        debug_print(pc,"%02X", *(pnt+i));   
    }
    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;
        debug_print(pc,"%02X", *(pnt+i));   
    }
    debug_print(pc,"\n\r");
    debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
    
    
    debug_print(pc,"MOSI WriteRegResponse:\t 0x");
    for(int i=0;i<(sizeof(tx)/2);i++){
        uint8_t* pnt=(uint8_t*)(tx+1);
        debug_print(pc,"%02X", *(pnt+i));   
    }
    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);
        debug_print(pc,"%02X", *(pnt+i));   
    }
    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){
        debug_print(pc,"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)
{
    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;
    //debug_print(pc,"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, 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){
        debug_print(pc,"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
    debug_print(pc,"\tRead PORT_ACQU\n\r");
    res = ReadReg(0x146, &val);    // PORT_ACQU
    wait_us(4);
    if (res < 0){
        debug_print(pc,"ReadReg Error\n\r");
        goto END;}
    val = (val >> 16) | 1;
    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){
        debug_print(pc,"WriteReg Error\n\r");
        goto END;}

    // Wait till PORT_READY bit is set.
    cnt = 0;
    debug_print(pc,"\tWait for PORT_READY bit\n\r");
    res = ReadReg(470, &val);     // PORT_READY 
    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.


    //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++;
    } */   

    
    
    res = ReadReg(470, &val);     // PORT_READY 
    wait_us(4);
    val=val>>16;
    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){
        debug_print(pc,"ReqreadEch error\n\r");
        goto END;}

    //--------------------------------------------------------------------------------------
    //----SHORT PACKET EXCHANGE--------
    debug_print(pc,"\tSend ReqReadEch\n\r");
    res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
    if (res < 0){
        debug_print(pc,"txPacketSlow Error\n\r");
        goto END;}
    wait_us(4);
    /*
    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);
        debug_print(pc,"%02X ", *(pnt+i));     
    }
    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);
        debug_print(pc,"%02X ", *(pnt+i));     
    }
    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){
        debug_print(pc,"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));
    debug_print(pc,"Value of npack:%d \n\r", nPack);
    for (a = 0; a < nPack; ++a)
    {
        
        if (res < 0){
            res = -7;
            debug_print(pc,"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){
            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)){

            debug_print(pc,"\n\r");
            debug_print(pc,"-- -- -- -- -- -- --\n\r");
            debug_print(pc,"Packet #%d => Decode long response error \n\r", a);
            
            debug_print(pc,"TXLONG MOSI Response:        ");
            for(int i=0;i<(sizeof(PACK_LONG2));i++){
                uint8_t* pnt=(uint8_t*)(&txL);
                debug_print(pc,"%02X ", *(pnt+i));   
            }
            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);
                debug_print(pc,"%02X ", *(pnt+i));   
            
            }
            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.
        //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,Serial* pc){
    trigger.write(0);
    int res, a, b;
    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;
    int nBytes=0;
    
    uint16_t * u16ptr;
    debug_print(pc,"Buffer space required: %d\n\r", MAXTRCLEN);
    // Required buffer space
    if (maxN < MAXTRCLEN){
        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;
    debug_print(pc,"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;
    debug_print(pc,"chsel = %d\n", val);
    cnt = 0;
    // Count how many channels are selected 
    for (int i = 0; i < 16; i++) {
        if (val & 0x1)
            cnt++;
        val >>= 1;
    }
    
    nPack *= cnt;
    nPack /= 16;
    
    res = ReadReg(332, &val);    // PORT_OVR_ACCUM_ACQ_OVR
//  debug_print(pc,"PORT_OVR  = %d\n", (val >> 16));
    val = (val >> 16) & 3;               // Get bits 0 and 1

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

        // Decode the long responses, then extract data values
        res = MLX_DecodeResL1(&rxL);
        if ((res < 0)){
            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);
                debug_print(pc,"%02X ", *(pnt+i));   
            }
            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);
                debug_print(pc,"%02X ", *(pnt+i));   
            
            }
            debug_print(pc,"\n\r");
            debug_print(pc,"-- -- -- -- -- -- --\n\r");
            //printf("last RXL = \n");
            //for (i = 0; i < 300; i++) {
            //  debug_print(pc,"%02x", rxL.buf[i]);
            //  if (((i + 1) % 8) == 0)
            //      debug_print(pc,"\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];
            nBytes++;
        }
        
    }
    trigger.write(0);
    device.format(8,1);
    res=nBytes;

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;
    //debug_print(pc,"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;
    //debug_print(pc,"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;
    //debug_print(pc,"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
//  debug_print(pc,"PORT_OVR  = %d\n", (val >> 16));
    val = (val >> 16) & 3;               // Get bits 0 and 1

    if (res < 0){
        debug_print(pc,"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 {
        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){
        debug_print(pc,"ReadReg Error2\n\r");
        goto END;}
    val = (val>>16) | 1;
    res = WriteReg(0x146, val);     // PORT_ACQU
    if (res < 0){
        debug_print(pc,"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;
      
    debug_print(pc,"MOSI ReqReadTrace:\t 0x");
    for(int i=0;i<(sizeof(tx));i++){
        uint8_t* pnt=(uint8_t*)tx;
        debug_print(pc,"%02X", *(pnt+i));   
    }
    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;
        debug_print(pc,"%02X", *(pnt+i));   
    }
    debug_print(pc,"\n");
    debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\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));
        //      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;
        }
        

        // Decode the long responses, then extract data values
        res = MLX_DecodeResL1(&rxL);
        if ((res < 0)){
            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);
                debug_print(pc,"%02X ", *(pnt+i));   
            }
            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);
                debug_print(pc,"%02X ", *(pnt+i));   
            
            }
            debug_print(pc,"\n\r");
            debug_print(pc,"-- -- -- -- -- -- --\n\r");
            //printf("last RXL = \n");
            //for (i = 0; i < 300; i++) {
            //  debug_print(pc,"%02x", rxL.buf[i]);
            //  if (((i + 1) % 8) == 0)
            //      debug_print(pc,"\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){
            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);
                debug_print(pc,"%02X", *(pnt+i));   
                if(((i %30) ==0)&&(i>0))debug_print(pc,"\n\t\t\t\t\t");
            }
            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);
                debug_print(pc,"%02X", *(pnt+i));   
                if(((i %30) ==0)&&(i>0))debug_print(pc,"\n\t\t\t\t\t");
            }
            debug_print(pc,"\n\r");
            debug_print(pc,"-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");    
        }
        
    }
    trigger.write(0);
    device.format(8,1);

END:
    device.format(8,1);
    return res;    
}


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];
    PACK_LONG2 rxL, txL;
    uint8_t iserr;
    uint16_t err;
    //uint32_t val = 0;
    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);
    
    int count=0;
    res=WriteReg(0x1d8, 128);
    if (res<0){
        debug_print(pc,"PORT_LONG_PACKET_SIZE Write error\n\r");
        goto END;
    }
    
    res=WriteReg(0x6c, 31796); //0x7C19
    if (res<0) {
        debug_print(pc,"REG_MLX16_ITC_MASK0 Write error\n\r");
    }
    
    wait_us(5);
    debug_print(pc,"Start loading fragments \n\r");
    
    LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
    Trigger(0);
    debug_print(pc,"Load fragment %d\n\r", count);
    for(int i =0; i<nBytes;i++){
        //debug_print(pc,"Addr[%d] : %d\n\r", addrStart+i, memory[i] );    
    }
    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
    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);
    debug_print(pc,"npack: %d\n\r", nPack);
    // Encode the request and send it
    res = MLX_ReqWriteFW(tx, nPack, addrStart);
    if (res < 0){
        debug_print(pc,"Err @ 1");
        goto END;   }
        
    wait_us(16);
    res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx));
    if (res < 0){
        debug_print(pc,"Err @ 2");
        goto END;   }
    //Trigger(1);
          
    debug_print(pc,"ReqWriteFirmware MOSI: \n\r0x");
    for(int k=0;k<(sizeof(tx));k++){
        uint8_t* pnt=(uint8_t*)(&tx);
        debug_print(pc,"%02X", *(pnt+k));   
        if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
    }
    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);
        debug_print(pc,"%02X", *(pnt+k));   
        if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
    }
    debug_print(pc,"\n\r-------------------\n\r");
    
    
    
    wait_us(5);

    // Optional status decoding
    res = MLX_DecodeStatusS(rx, &iserr, &err);
    if (res < 0 || iserr){
        debug_print(pc,"Err @ 3");
        
        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);
                debug_print(pc,"%02X", *(pnt+k));   
                if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
            }
        debug_print(pc,"\n\r");
        
        
        goto END;   }

    nWordsRemaining = nWords;
    for (uint a = 0; a < nPack; ++a)
    {
        uint16_t size; 
        if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
            size = MLX_LONG2_DATA_SZ / 2;
        else
            size = nWordsRemaining;

        res = MLX_WriteDataL2(&txL, size, a, &memory[a*MLX_LONG2_DATA_SZ]);
        if (res < 0){
            res = -7;
            goto END;
        }

        res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));

        //Trigger(0);
        wait_us(12);
        /*
            
        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);
            debug_print(pc,"%02X", *(pnt+k));   
            if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
        }
        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);
            debug_print(pc,"%02X", *(pnt+k));   
            if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
        }
        debug_print(pc,"\n\r-----------\n\r");
        
        */
        
        res = MLX_DecodeResL2(&rxL);
        if (res < 0){
            debug_print(pc,"LONG WRITE ERROR: Stopped at the %d long message, res=%d\n", a, res);
            
            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);
                debug_print(pc,"%02X", *(pnt+k));   
                if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
            }
            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);
                debug_print(pc,"%02X", *(pnt+k));   
                if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
            }
            debug_print(pc,"\n\r");
            res = -9;
            goto END;
        }

        nWordsRemaining = nWords - size;
        
    }
    count++;
    //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;
    }

    // Clock the remaining long packets
    res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
    wait_us(11);
    if (res < 0){
        debug_print(pc,"Err @ 4");
        goto END;   }
    
    
    
    
    memset(memory, 0, 512);
    wait_us(12);
    
    
    debug_print(pc,"Status long packet to check status after last fragment : %d, res=%d\n", count, res);
                
    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);
        debug_print(pc,"%02X", *(pnt+k));   
        if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
    }
    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);
        debug_print(pc,"%02X", *(pnt+k));   
        if(((k %30) ==0)&&(k>0)) debug_print(pc,"\n\r");
    }
    debug_print(pc,"\n\r-----------\n\r");

    // Change jump table pointer.
    //res=LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
    //debug_print(pc,"Return from a loadPatchFragment: res = %d \n\r", res);
    wait_ms(100);
    
    if (true){
        debug_print(pc,"Change jumptable... \n\r");
        res = WriteReg(0x1000, 0x7000, pc);     // write addr: 0x1000 value:0x7000
        if (res < 0){
            debug_print(pc,"Err @ 5");
            goto END;   
        }
    }
    else debug_print(pc,"Failed to read the file\n\r");
    
END:
    return res;     
}

void LidarSpi::Trigger(int level){
    trigger.write(level);    
}


int LidarSpi::LoadPatchFragment(const char *patch, uint16_t *addrStart, uint16_t *startLine, uint16_t *nBytes, uint8_t *memory, Serial* pc){
    char line[100];
    FILE *fin;
    uint16_t addr, n, status;
    uint8_t bytes[256];
    uint16_t i, total = 0, lineno = 1;
    uint16_t minaddr = 65535, maxaddr = 0;
    
    uint16_t lineCount = *startLine;
    uint16_t lines = 0;
    

    *nBytes = 0;
    if (strlen(patch) == 0) {
        //printf("   Can't load a file without the filename.");
        return -9;
    }
    fin = fopen(patch, "r");
    if (fin == NULL) {
        debug_print(pc,"   Can't open file '%s' for reading.\n\r", patch);
        return -10;
    }
    debug_print(pc,"Opened file\n\r");
    //  printf("Patch file %s opened\n", filename);
    
    while (true) {
        line[0] = '\0';
        fgets(line, 1000, fin);
        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);
        //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";
            //debug_print(pc,"Line: %s\n\r", line);

            if (*nBytes>15000) {
                *addrStart = minaddr;
                *startLine = lines;
                fclose(fin);
                debug_print(pc,"Close file\n\r");
                return -5;
            }
            
            if (status == 0) {  /* data */
                                //cout << "Status=0";
                //if (addr < minaddr) minaddr = addr;
                //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;
                    
                    if (i % 2 == 0)
                        memory[addr-minaddr] = bytes[i + 1] & 255;  //Assumption even no of bytes per line
                    else
                        memory[addr-minaddr] = bytes[i - 1] & 255;
                    
                    if (addr < minaddr) minaddr = addr;
                    if ((addr) > maxaddr) maxaddr = (addr);
                    addr++;
                    total++;
                }
            }
            if (status == 1) {  /* end of file */
                fclose(fin);
                debug_print(pc,"Closed file\n\r");
                //printf("load_file parsed %d bytes between:", total);
                //printf(" %04X to %04X\n", minaddr, maxaddr);
                
                *addrStart= minaddr;
                debug_print(pc,"minAddr: %d, maxAddr: %d\n\r", minaddr, maxaddr);
                *nBytes=maxaddr-minaddr;
                debug_print(pc,"End of file\n\r");
                return total;
            }
            if (status == 2)  /* begin of file */
                goto NEXT;
            *nBytes =0;
        }
        else {
            printf("   Error: '%s', line: %d\n\r", patch, lineno);
        }
        lineCount++;
    NEXT:   lineno++;
    }/*
    fclose(fin);
    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){
    unsigned int sum, len, cksum;
    unsigned int newAddr, newCode, newByte;
    

    char *ptr;
    //cout << "Start parsing\n\r";
    *num = 0;
    if (theline[0] != ':') return 0;
    if (strlen(theline) < 11) return 0;
    ptr = theline + 1;
    if (!sscanf(ptr, "%02x", &len)) return 0;
    ptr += 2;
    if (strlen(theline) < (11 + (len * 2))) return 0;
    if (!sscanf(ptr, "%04x", &newAddr)) return 0;
    *addr=(uint16_t)newAddr;
    ptr += 4;
    //printf("Line: length=%d Addr=%d\n", len, *addr);
    if (!sscanf(ptr, "%02x", &newCode)) return 0;

    *code=(uint16_t)newCode;
    ptr += 2;
    sum = (len & 255) + ((*addr >> 8) & 255) + (*addr & 255) + (*code & 255);
    while (*num != len) {
        if (!sscanf(ptr, "%02x", &newByte )) return 0;
        bytes[*num]=(uint8_t)newByte;
        ptr += 2;
        sum += bytes[*num] & 255;
        (*num)++;
        if (*num >= 256) return 0;
    }
    if (!sscanf(ptr, "%02x", &cksum)) return 0;
    if (((sum & 255) + (cksum & 255)) & 255) return 0; /* checksum error */
    return 1;
}

int LidarSpi::setTrace()
{
    uint16_t val=148;
    int res=0;
    //debug_print(pc,"Write PORT_LONG_PACKET_SIZE: %d\n\r",val);
    //Trigger(1);
    res=WriteReg(0x1d8, val);
    //Trigger(0);
    if (res<0){
        //debug_print(pc,"PORT_LONG_PACKET_SIZE Write error\n\r");
        return res;
    }

    //debug_print(pc,"Packet size: %d\n\r", val);
    
    //debug_print(pc,"Write REG_PORT_CHSEL: 0xFFFF\n\r");
    res=WriteReg(0x150, 0xffff);
    if (res<0) {
        //debug_print(pc,"REG_PORT_CHSEL Write error\n\r");
        return res;
    }
    //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) {
        //debug_print(pc,"REG_PORT_OVR_ACCUM Write error\n\r");
        return res;
    }
    //debug_print(pc,"Write REG_PORT_FIXED_DIVIDER: 0x802\n\r");
    res=WriteReg(0x14E, 0x0); //0x802
    if (res<0) {
        //debug_print(pc,"REG_PORT_FIXED_DIVIDER Write error\n\r");
        return res;
    }
    
    
    //debug_print(pc,"Write REG_PORT_TRIGGER_PERIOD: 0x3E8\n\r");
    res=WriteReg(0x148, 1000); //0x3e8
    if (res<0) {
        //debug_print(pc,"REG_PORT_TRIGGER_PERIOD Write error\n\r");
        return res;
    }
    
    //debug_print(pc,"Write REG_PORT_MEAS_DELAY: 0x0000\n\r");
    res=WriteReg(0x32, 0);
    if (res<0) {
        //debug_print(pc,"REG_PORT_MEAS_DELAY Write error\n\r");
        return res;
    }
    //debug_print(pc,"Write REG_PORT_MODE_EN: 0x0000\n\r");
    res=WriteReg(0x30, 0x00);
    if (res<0) {
        //debug_print(pc,"REG_PORT_MODE_EN Write error\n\r");
        return res;
    }
    
    //debug_print(pc,"Write REG_MLX16_ITC_MASK0: 0x7C34\n\r");
    res=WriteReg(0x6c, 0x7C34); //0x7C19
    if (res<0) {
        //debug_print(pc,"REG_MLX16_ITC_MASK0 Write error\n\r");
        return res;
    }
    //debug_print(pc,"Write REG_STIMER2_VALUE: 0xBFFF\n\r");
    res=WriteReg(0x22, 49151); //0xBFFF
    if (res<0) {
        //debug_print(pc,"REG_STIMER2_VALUE Write error\n\r");
        return res;
    }
    /*
    //debug_print(pc,"Write REG_PORT_FIXED_DIVIDER: 0x802\n\r");
    res=WriteReg(0x14E, 0x0); //0x802
    if (res<0) {
        //debug_print(pc,"REG_PORT_FIXED_DIVIDER Write error\n\r");
        return res;
    }
    
    //0x124=0xae40, and 0x126=0xae40
    //debug_print(pc,"Improvement registers for trace without patch\n\r");
    res=WriteReg(0x124, 0xae40); //0x802
    if (res<0) {
        //debug_print(pc,"PORT SH1 Write error\n\r");
        return res;
    }
    res=WriteReg(0x126, 0xae40); //0x802
    if (res<0) {
        //debug_print(pc,"PORT SH2 Write error\n\r");
        return res;
    }*/
    
    
    return 0;
               
}

int LidarSpi::setEcho(){
    uint32_t val=128;
    int res;
    //pc.printf("Write PORT_LONG_PACKET_SIZE: %d\n\r",val);
    //lidar.Trigger(1);
    res=WriteReg(0x1d8, val);
    //lidar.Trigger(0);
    if (res<0)return res;
    res=ReadReg(0x1d8,&val);
    if (res<0)return res;
    val=val>>16;
    //pc.printf("Packet size: %d\n\r", val);
    
    //pc.printf("Write REG_PORT_CHSEL: 0xFFFF\n\r");
    res=WriteReg(0x150, 0xffff);
    if (res<0) return res;
    //pc.printf("Write REG_PORT_OVR_ACCUM: 0x4B3\n\r");
    //res=WriteReg(0x14c, 1203); //0x4B3
    res=WriteReg(0x14c, 0x0fff); //0x4B3
    if (res<0) return res;
    
    
    //pc.printf("Write REG_PORT_SH1_CNTL: 0xAE40\n\r");
    res=WriteReg(0x00124, 0xAE40); 
    if (res<0) return res;
    //pc.printf("Write REG_PORT_SH2_CNTL: 0xAE40\n\r");
    res=WriteReg(0x00126, 0xAE40); 
    if (res<0) return res;
    
    //pc.printf("Write REG_PORT_FIXED_DIVIDER: 0x802\n\r");
    res=WriteReg(0x14E, 0xc00); //0x802
    if (res<0) return res;
    
    //pc.printf("Write REG_PORT_TRIGGER_PERIOD: 0x3E8\n\r");
    res=WriteReg(0x148, 1000); //0x3e8
    if (res<0) return res;
    
    //pc.printf("Write REG_PORT_MEAS_DELAY: 0x0000\n\r");
    res=WriteReg(0x32, 0xff);
    if (res<0) return res;
    //pc.printf("Write REG_PORT_MODE_EN: 0x0000\n\r");
    res=WriteReg(0x30, 0xc);
    if (res<0) return res;
    
    //pc.printf("Write REG_MLX16_ITC_MASK0: 0x7C34\n\r");
    res=WriteReg(0x6c, 31796); //0x7C19
    if (res<0) return res;
    //pc.printf("Write REG_STIMER2_VALUE: 0xBFFF\n\r");
    res=WriteReg(0x22, 49151); //0xBFFF
    if (res<0) return res;
    
    
    //Channel thresholds
    uint32_t strtAddr=0x1bd2;
    
    for(uint32_t i =strtAddr;i< strtAddr+16*4; i=i+4){
        res=WriteReg(i, 0x0);
        if (res<0) return res;
        wait_us(4);
        res=WriteReg(i+2, 0x5);   
        if (res<0) return res;
        wait_us(4);
    }
    //Disable Multi-object
    res=WriteReg(0x1c52,0x9c);
    if (res<0) return res;
    
    //Peak Min Pulse edge len
    res=WriteReg(0x1c80,0xe);
    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;
}