main.cpp

00001 #include "mbed.h"
00002 #include "LidarSpi.h"
00003 #include "LidarFunc.h"
00004 #include "USBSerial.h"
00005 #include "tests.h"
00006 //DigitalOut myled(LED1);
00007 
00008 /*Protocol:
00009 SEND: 
00010     "CMD PAR1 PAR2\n"
00011 RETURN
00012     status(S) nrOfResponseBytes(L) Response(R)
00013     SLLLLR..R
00014 */
00015 
00016 Serial pc(USBTX, USBRX);
00017 USBSerial serial;
00018 
00019 LocalFileSystem local("local"); 
00020 
00021 int main() {
00022 
00023     LidarSpi lidar(p5, p6, p7, p8, p15, p21, p22, p23);
00024     LidarFunc sens(&lidar);
00025     
00026     char cmd[10];
00027     char msg[50];
00028     uint32_t par1=0;
00029     uint32_t par2=0;
00030     int res;
00031     int cnt=0;
00032     int n;
00033     uint32_t size;
00034     uint32_t val;
00035     uint8_t * pntrS;
00036     uint8_t * pntrV;
00037     pc.baud(115200);
00038     while(1){
00039       
00040         pc.printf("Awaiting commands...\n\r");
00041 
00042         do{
00043             msg[cnt]=serial.getc();
00044             cnt++;
00045             msg[cnt]='\0';
00046             //pc.printf("getc:  %s", msg);
00047         } while (msg[cnt-1]!='\n' && cnt<50);
00048         cnt=0;
00049         pc.printf("getc:  %s\n\r", msg);
00050         n = sscanf(msg,
00051                            "%9[^' '] %x %x\n", 
00052                            cmd,
00053                            &par1,
00054                            &par2
00055                         );
00056         
00057     
00058         
00059         if((strcmp(cmd, "RREG")==0) && n==2){  //ReadRegister Address
00060             pc.printf("ReadRegister, address: %x", par1);
00061             
00062             res=lidar.ReadReg(par1, &val);
00063             val=(val>>16)&0xffff;
00064             
00065             
00066             if(res<0){
00067                 size=0x0000;
00068                 pntrS=(uint8_t *)&size;
00069                 serial.putc(1);
00070                 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
00071                 pc.printf("Read Error\n\r");
00072             }
00073             else{
00074                 size=sizeof(val);
00075                 pntrS=(uint8_t *) &size;
00076                 pntrV=(uint8_t *) &val;
00077                 pc.printf("size: %d\n\r", *pntrS);
00078                 serial.putc(0);
00079                 for(int i=0;i<sizeof(size);i++) serial.putc(*(pntrS+i));
00080                 for(int i=0;i<sizeof(size);i++) pc.printf("putc Val: [%d] %d\n\r",i,pntrV[i]);
00081                 for(int i=0;i<sizeof(val);i++) serial.putc(*(pntrV+i));
00082                 pc.printf("Address Value: %x  ,   sizeof(val)= %d\n\r", *pntrV, sizeof(val));
00083                 
00084             }
00085             
00086     
00087         }
00088         else if((strcmp(cmd, "WREG")==0) && n==3){  //WriteRegister Address Value
00089             pc.printf("WriteRegister, address: %x, value:%x", par1, par2);
00090             
00091             res=lidar.WriteReg(par1, par2);
00092             if(res<0){
00093 
00094                 size=0;
00095                 pntrS=(uint8_t *)&size;
00096                 serial.putc(1);
00097                 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
00098             }
00099             else{
00100   
00101                 size=0;
00102                 pntrS=(uint8_t *)&size;
00103                 serial.putc(0);
00104                 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
00105             }
00106     
00107         }
00108         else if((strcmp(cmd, "PTCH")==0) && n==1){  //WriteRegister Address Value
00109             pc.printf("LoadPatch\n\r");
00110             
00111             res=lidar.LoadPatch("/local/patch.hex", &pc);
00112             if(res<0){
00113 
00114                 size=0;
00115                 pntrS=(uint8_t *)&size;
00116                 serial.putc(1);
00117                 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
00118                 pc.printf("Patch load failed\n\r");
00119             }
00120             else{
00121   
00122                 size=0;
00123                 pntrS=(uint8_t *)&size;
00124                 serial.putc(0);
00125                 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
00126                 pc.printf("Patch load Success\n\r");
00127             }
00128     
00129         }
00130         else if((strcmp(cmd, "TRCE")==0) && n==2){  //WriteRegister Address Value
00131             pc.printf("getTrace\n\r");
00132             bool isEnabled;
00133             if (par1>0) isEnabled=true;
00134             else isEnabled=false;
00135             res=sens.setLed(isEnabled);
00136             res=lidar.setTrace();
00137             res=traceTest(&lidar, &pc, &serial);
00138         }
00139         else
00140         {
00141             pc.printf("INVALID COMMAND\n\r");
00142             size=0;
00143             pntrS=(uint8_t *)&size;
00144             serial.putc(1);
00145             for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]); 
00146         }
00147     }
00148     
00149     
00150     
00151     
00152 }