File content as of revision 0:10bd8964965a:

#include "mbed.h"
#include "LidarSpi.h"
#include "LidarFunc.h"
#include "USBSerial.h"
#include "tests.h"
//DigitalOut myled(LED1);

/*Protocol:
SEND: 
    "CMD PAR1 PAR2\n"
RETURN
    status(S) nrOfResponseBytes(L) Response(R)
    SLLLLR..R
*/

Serial pc(USBTX, USBRX);
USBSerial serial;

LocalFileSystem local("local"); 

int main() {

    LidarSpi lidar(p5, p6, p7, p8, p15, p21, p22, p23);
    LidarFunc sens(&lidar);
    
    char cmd[10];
    char msg[50];
    uint32_t par1=0;
    uint32_t par2=0;
    int res;
    int cnt=0;
    int n;
    uint32_t size;
    uint32_t val;
    uint8_t * pntrS;
    uint8_t * pntrV;
    pc.baud(115200);
    while(1){
      
        pc.printf("Awaiting commands...\n\r");

        do{
            msg[cnt]=serial.getc();
            cnt++;
            msg[cnt]='\0';
            //pc.printf("getc:  %s", msg);
        } while (msg[cnt-1]!='\n' && cnt<50);
        cnt=0;
        pc.printf("getc:  %s\n\r", msg);
        n = sscanf(msg,
                           "%9[^' '] %x %x\n", 
                           cmd,
                           &par1,
                           &par2
                        );
        
    
        
        if((strcmp(cmd, "RREG")==0) && n==2){  //ReadRegister Address
            pc.printf("ReadRegister, address: %x", par1);
            
            res=lidar.ReadReg(par1, &val);
            val=(val>>16)&0xffff;
            
            
            if(res<0){
                size=0x0000;
                pntrS=(uint8_t *)&size;
                serial.putc(1);
                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
                pc.printf("Read Error\n\r");
            }
            else{
                size=sizeof(val);
                pntrS=(uint8_t *) &size;
                pntrV=(uint8_t *) &val;
                pc.printf("size: %d\n\r", *pntrS);
                serial.putc(0);
                for(int i=0;i<sizeof(size);i++) serial.putc(*(pntrS+i));
                for(int i=0;i<sizeof(size);i++) pc.printf("putc Val: [%d] %d\n\r",i,pntrV[i]);
                for(int i=0;i<sizeof(val);i++) serial.putc(*(pntrV+i));
                pc.printf("Address Value: %x  ,   sizeof(val)= %d\n\r", *pntrV, sizeof(val));
                
            }
            
    
        }
        else if((strcmp(cmd, "WREG")==0) && n==3){  //WriteRegister Address Value
            pc.printf("WriteRegister, address: %x, value:%x", par1, par2);
            
            res=lidar.WriteReg(par1, par2);
            if(res<0){

                size=0;
                pntrS=(uint8_t *)&size;
                serial.putc(1);
                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
            }
            else{
  
                size=0;
                pntrS=(uint8_t *)&size;
                serial.putc(0);
                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
            }
    
        }
        else if((strcmp(cmd, "PTCH")==0) && n==1){  //WriteRegister Address Value
            pc.printf("LoadPatch\n\r");
            
            res=lidar.LoadPatch("/local/patch.hex", &pc);
            if(res<0){

                size=0;
                pntrS=(uint8_t *)&size;
                serial.putc(1);
                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
                pc.printf("Patch load failed\n\r");
            }
            else{
  
                size=0;
                pntrS=(uint8_t *)&size;
                serial.putc(0);
                for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
                pc.printf("Patch load Success\n\r");
            }
    
        }
        else if((strcmp(cmd, "TRCE")==0) && n==2){  //WriteRegister Address Value
            pc.printf("getTrace\n\r");
            bool isEnabled;
            if (par1>0) isEnabled=true;
            else isEnabled=false;
            res=sens.setLed(isEnabled);
            res=lidar.setTrace();
            res=traceTest(&lidar, &pc, &serial);
        }
        else
        {
            pc.printf("INVALID COMMAND\n\r");
            size=0;
            pntrS=(uint8_t *)&size;
            serial.putc(1);
            for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]); 
        }
    }
    
    
    
    
}