Tom N / Mbed 2 deprecated MLX75320_API

API to interface sensor with host PC

Dependencies:   LidarSpi USBDevice mbed

main.cpp@0:10bd8964965a, 2016-04-05 (annotated)

Committer:
TNU
Date:
Tue Apr 05 08:16:30 2016 +0000
Revision:
0:10bd8964965a
Fixed tests.cpp;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TNU 0:10bd8964965a 1 #include "mbed.h"
TNU 0:10bd8964965a 2 #include "LidarSpi.h"
TNU 0:10bd8964965a 3 #include "LidarFunc.h"
TNU 0:10bd8964965a 4 #include "USBSerial.h"
TNU 0:10bd8964965a 5 #include "tests.h"
TNU 0:10bd8964965a 6 //DigitalOut myled(LED1);
TNU 0:10bd8964965a 7
TNU 0:10bd8964965a 8 /*Protocol:
TNU 0:10bd8964965a 9 SEND:
TNU 0:10bd8964965a 10 "CMD PAR1 PAR2\n"
TNU 0:10bd8964965a 11 RETURN
TNU 0:10bd8964965a 12 status(S) nrOfResponseBytes(L) Response(R)
TNU 0:10bd8964965a 13 SLLLLR..R
TNU 0:10bd8964965a 14 */
TNU 0:10bd8964965a 15
TNU 0:10bd8964965a 16 Serial pc(USBTX, USBRX);
TNU 0:10bd8964965a 17 USBSerial serial;
TNU 0:10bd8964965a 18
TNU 0:10bd8964965a 19 LocalFileSystem local("local");
TNU 0:10bd8964965a 20
TNU 0:10bd8964965a 21 int main() {
TNU 0:10bd8964965a 22
TNU 0:10bd8964965a 23 LidarSpi lidar(p5, p6, p7, p8, p15, p21, p22, p23);
TNU 0:10bd8964965a 24 LidarFunc sens(&lidar);
TNU 0:10bd8964965a 25
TNU 0:10bd8964965a 26 char cmd[10];
TNU 0:10bd8964965a 27 char msg[50];
TNU 0:10bd8964965a 28 uint32_t par1=0;
TNU 0:10bd8964965a 29 uint32_t par2=0;
TNU 0:10bd8964965a 30 int res;
TNU 0:10bd8964965a 31 int cnt=0;
TNU 0:10bd8964965a 32 int n;
TNU 0:10bd8964965a 33 uint32_t size;
TNU 0:10bd8964965a 34 uint32_t val;
TNU 0:10bd8964965a 35 uint8_t * pntrS;
TNU 0:10bd8964965a 36 uint8_t * pntrV;
TNU 0:10bd8964965a 37 pc.baud(115200);
TNU 0:10bd8964965a 38 while(1){
TNU 0:10bd8964965a 39
TNU 0:10bd8964965a 40 pc.printf("Awaiting commands...\n\r");
TNU 0:10bd8964965a 41
TNU 0:10bd8964965a 42 do{
TNU 0:10bd8964965a 43 msg[cnt]=serial.getc();
TNU 0:10bd8964965a 44 cnt++;
TNU 0:10bd8964965a 45 msg[cnt]='\0';
TNU 0:10bd8964965a 46 //pc.printf("getc: %s", msg);
TNU 0:10bd8964965a 47 } while (msg[cnt-1]!='\n' && cnt<50);
TNU 0:10bd8964965a 48 cnt=0;
TNU 0:10bd8964965a 49 pc.printf("getc: %s\n\r", msg);
TNU 0:10bd8964965a 50 n = sscanf(msg,
TNU 0:10bd8964965a 51 "%9[^' '] %x %x\n",
TNU 0:10bd8964965a 52 cmd,
TNU 0:10bd8964965a 53 &par1,
TNU 0:10bd8964965a 54 &par2
TNU 0:10bd8964965a 55 );
TNU 0:10bd8964965a 56
TNU 0:10bd8964965a 57
TNU 0:10bd8964965a 58
TNU 0:10bd8964965a 59 if((strcmp(cmd, "RREG")==0) && n==2){ //ReadRegister Address
TNU 0:10bd8964965a 60 pc.printf("ReadRegister, address: %x", par1);
TNU 0:10bd8964965a 61
TNU 0:10bd8964965a 62 res=lidar.ReadReg(par1, &val);
TNU 0:10bd8964965a 63 val=(val>>16)&0xffff;
TNU 0:10bd8964965a 64
TNU 0:10bd8964965a 65
TNU 0:10bd8964965a 66 if(res<0){
TNU 0:10bd8964965a 67 size=0x0000;
TNU 0:10bd8964965a 68 pntrS=(uint8_t *)&size;
TNU 0:10bd8964965a 69 serial.putc(1);
TNU 0:10bd8964965a 70 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
TNU 0:10bd8964965a 71 pc.printf("Read Error\n\r");
TNU 0:10bd8964965a 72 }
TNU 0:10bd8964965a 73 else{
TNU 0:10bd8964965a 74 size=sizeof(val);
TNU 0:10bd8964965a 75 pntrS=(uint8_t *) &size;
TNU 0:10bd8964965a 76 pntrV=(uint8_t *) &val;
TNU 0:10bd8964965a 77 pc.printf("size: %d\n\r", *pntrS);
TNU 0:10bd8964965a 78 serial.putc(0);
TNU 0:10bd8964965a 79 for(int i=0;i<sizeof(size);i++) serial.putc(*(pntrS+i));
TNU 0:10bd8964965a 80 for(int i=0;i<sizeof(size);i++) pc.printf("putc Val: [%d] %d\n\r",i,pntrV[i]);
TNU 0:10bd8964965a 81 for(int i=0;i<sizeof(val);i++) serial.putc(*(pntrV+i));
TNU 0:10bd8964965a 82 pc.printf("Address Value: %x , sizeof(val)= %d\n\r", *pntrV, sizeof(val));
TNU 0:10bd8964965a 83
TNU 0:10bd8964965a 84 }
TNU 0:10bd8964965a 85
TNU 0:10bd8964965a 86
TNU 0:10bd8964965a 87 }
TNU 0:10bd8964965a 88 else if((strcmp(cmd, "WREG")==0) && n==3){ //WriteRegister Address Value
TNU 0:10bd8964965a 89 pc.printf("WriteRegister, address: %x, value:%x", par1, par2);
TNU 0:10bd8964965a 90
TNU 0:10bd8964965a 91 res=lidar.WriteReg(par1, par2);
TNU 0:10bd8964965a 92 if(res<0){
TNU 0:10bd8964965a 93
TNU 0:10bd8964965a 94 size=0;
TNU 0:10bd8964965a 95 pntrS=(uint8_t *)&size;
TNU 0:10bd8964965a 96 serial.putc(1);
TNU 0:10bd8964965a 97 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
TNU 0:10bd8964965a 98 }
TNU 0:10bd8964965a 99 else{
TNU 0:10bd8964965a 100
TNU 0:10bd8964965a 101 size=0;
TNU 0:10bd8964965a 102 pntrS=(uint8_t *)&size;
TNU 0:10bd8964965a 103 serial.putc(0);
TNU 0:10bd8964965a 104 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
TNU 0:10bd8964965a 105 }
TNU 0:10bd8964965a 106
TNU 0:10bd8964965a 107 }
TNU 0:10bd8964965a 108 else if((strcmp(cmd, "PTCH")==0) && n==1){ //WriteRegister Address Value
TNU 0:10bd8964965a 109 pc.printf("LoadPatch\n\r");
TNU 0:10bd8964965a 110
TNU 0:10bd8964965a 111 res=lidar.LoadPatch("/local/patch.hex", &pc);
TNU 0:10bd8964965a 112 if(res<0){
TNU 0:10bd8964965a 113
TNU 0:10bd8964965a 114 size=0;
TNU 0:10bd8964965a 115 pntrS=(uint8_t *)&size;
TNU 0:10bd8964965a 116 serial.putc(1);
TNU 0:10bd8964965a 117 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
TNU 0:10bd8964965a 118 pc.printf("Patch load failed\n\r");
TNU 0:10bd8964965a 119 }
TNU 0:10bd8964965a 120 else{
TNU 0:10bd8964965a 121
TNU 0:10bd8964965a 122 size=0;
TNU 0:10bd8964965a 123 pntrS=(uint8_t *)&size;
TNU 0:10bd8964965a 124 serial.putc(0);
TNU 0:10bd8964965a 125 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
TNU 0:10bd8964965a 126 pc.printf("Patch load Success\n\r");
TNU 0:10bd8964965a 127 }
TNU 0:10bd8964965a 128
TNU 0:10bd8964965a 129 }
TNU 0:10bd8964965a 130 else if((strcmp(cmd, "TRCE")==0) && n==2){ //WriteRegister Address Value
TNU 0:10bd8964965a 131 pc.printf("getTrace\n\r");
TNU 0:10bd8964965a 132 bool isEnabled;
TNU 0:10bd8964965a 133 if (par1>0) isEnabled=true;
TNU 0:10bd8964965a 134 else isEnabled=false;
TNU 0:10bd8964965a 135 res=sens.setLed(isEnabled);
TNU 0:10bd8964965a 136 res=lidar.setTrace();
TNU 0:10bd8964965a 137 res=traceTest(&lidar, &pc, &serial);
TNU 0:10bd8964965a 138 }
TNU 0:10bd8964965a 139 else
TNU 0:10bd8964965a 140 {
TNU 0:10bd8964965a 141 pc.printf("INVALID COMMAND\n\r");
TNU 0:10bd8964965a 142 size=0;
TNU 0:10bd8964965a 143 pntrS=(uint8_t *)&size;
TNU 0:10bd8964965a 144 serial.putc(1);
TNU 0:10bd8964965a 145 for(int i=0;i<sizeof(size);i++) serial.putc(pntrS[i]);
TNU 0:10bd8964965a 146 }
TNU 0:10bd8964965a 147 }
TNU 0:10bd8964965a 148
TNU 0:10bd8964965a 149
TNU 0:10bd8964965a 150
TNU 0:10bd8964965a 151
TNU 0:10bd8964965a 152 }