• R/O
  • SSH
  • HTTPS

scip: Commit


Commit MetaInfo

Revision26 (tree)
Time2009-05-19 18:43:54
Authory_o_s

Log Message

supported MS/MD commands

Change Summary

Incremental Difference

--- libscip2/trunk/src/scip2command.c (revision 25)
+++ libscip2/trunk/src/scip2command.c (revision 26)
@@ -18,12 +18,34 @@
1818 char *commBuff;
1919 int commBuffSize;
2020 char lastStatus[4];
21+ char lastCommand[80];
22+ int mode;
23+ // Mx 受信用
24+ int leftScanCount;
25+ int nPts;
26+ // ME/MI/MR 受信用
27+ long long *workBuff;
2128 } tScipCommandImpl;
2229
30+enum {
31+ MODE_OFF=0,
32+ MODE_ON=1,
33+ MODE_MX_MASK=0x10, // Mx モードマスク
34+ MODE_MS=0x10, // 2Bytes 距離
35+ MODE_MD=0x11, // 3Bytes 距離
36+ MODE_MR=0x12, // 反射係数 (3Bytes距離 + 2Bytes 反射係数)
37+ MODE_MI=0x13, // 受光強度 (3Bytes距離 + 4Bytes 受光レベル等)
38+ MODE_ME=0x14, // HOKUYOモード
39+ MODE_FAULT=-1,
40+};
41+
2342 #define INITIAL_COMM_BUFF_SIZE (4096)
2443
2544 #define IMPL(x) ((tScipCommandImpl*)(x->detail))
2645
46+// MS/MD/ME/MR/MI コマンド
47+int scip2Do_Mx(tScipSensor* aSensor, tScipScanParam aParam, int aScanNum, int aScanSkip, char aSubcommand);
48+
2749 // レンジデータ文字列の解釈
2850 int decodeRangeData(char * aBuff, int size, int aPts,int *aRange,int *aTimeStamp);
2951
@@ -50,6 +72,8 @@
5072 sensor->detail=(tScipCommandImpl*)malloc(sizeof(tScipCommandImpl));
5173 IMPL(sensor)->commBuff=malloc(INITIAL_COMM_BUFF_SIZE);
5274 IMPL(sensor)->commBuffSize=INITIAL_COMM_BUFF_SIZE;
75+ IMPL(sensor)->mode=MODE_OFF;
76+ IMPL(sensor)->leftScanCount=0;
5377 return sensor;
5478 }
5579
@@ -73,6 +97,7 @@
7397 int scip2Do_QT(tScipSensor* aSensor)
7498 {
7599 #warning not implemented
100+ IMPL(aSensor)->mode=MODE_OFF;
76101 }
77102
78103 //! OFFモードに遷移 (RT)
@@ -83,6 +108,7 @@
83108 int scip21Do_RT(tScipSensor* aSensor)
84109 {
85110 #warning not implemented
111+ IMPL(aSensor)->mode=MODE_OFF;
86112 }
87113
88114 //! 初期状態に復帰 (RS)
@@ -93,6 +119,7 @@
93119 int scip2Do_RS(tScipSensor* aSensor)
94120 {
95121 #warning not implemented
122+ IMPL(aSensor)->mode=MODE_OFF;
96123 }
97124
98125 //! ONモードに遷移して事前測定開始
@@ -105,6 +132,7 @@
105132 char echo[80],status[80];
106133 int ret;
107134 scipSendCommand(aSensor->dev,"BM\n");
135+ strcpy(IMPL(aSensor)->lastCommand,"BM\n");
108136 if((ret=scipReceiveRecord(aSensor->dev,echo,status,
109137 IMPL(aSensor)->commBuff,IMPL(aSensor)->commBuffSize))>0)
110138 {
@@ -113,7 +141,10 @@
113141
114142 // 既にレーザが指示された状態であるときも、成功とみなす
115143 if(strcmp("00P\n",status)==0 || strcmp("02R\n",status)==0)
116- return 1;
144+ {
145+ IMPL(aSensor)->mode=MODE_ON;
146+ return 1;
147+ }
117148 return encodeStatus(status);
118149 }
119150 return ret;
@@ -138,6 +169,7 @@
138169 aParam.skip=1;
139170
140171 sprintf(line,"GD%04d%04d%02d;0X\n",aParam.start,aParam.end,aParam.skip);
172+ strcpy(IMPL(aSensor)->lastCommand,line);
141173 scipSendCommand(aSensor->dev,line);
142174
143175 if((ret=scipReceiveRecord(aSensor->dev,echo,status,
@@ -153,6 +185,8 @@
153185 if(strcmp(status,"00P\n")!=0)
154186 {
155187 fprintf(stderr,"status is not OK: %s\n",status);
188+ if(strcmp(status,"10Q")==0)
189+ IMPL(aSensor)->mode=MODE_OFF;
156190 strncpy(IMPL(aSensor)->lastStatus,status,4);
157191 return 0;
158192 }
@@ -196,23 +230,112 @@
196230 */
197231 int scip2Do_MS(tScipSensor* aSensor, tScipScanParam aParam, int aScanNum, int aScanSkip)
198232 {
199-#warning not implemented
233+ if(!scip2Do_Mx(aSensor,aParam,aScanNum,aScanSkip,'S'))
234+ {
235+ return 0;
236+ }
237+ IMPL(aSensor)->mode=MODE_MS;
238+ IMPL(aSensor)->nPts=(aParam.end-aParam.start+1)/aParam.skip;
239+ if(aScanNum==0)
240+ IMPL(aSensor)->leftScanCount=-1;
241+ else
242+ IMPL(aSensor)->leftScanCount=aScanNum;
243+ return 1;
200244 }
245+//! Mxモードに遷移して測定、データ送信開始(MD/MS/ME/MI/MR)
246+/*!
247+ @param aSensor センサハンドル
248+ @param aParam スキャンパラメータ
249+ @param aScanNum 取得するスキャン数
250+ @param aScanSkip 間引きスキャン数
251+ @retval >0 成功
252+ @retval <=0 失敗
253+*/
201254 int scip2Do_MD(tScipSensor* aSensor, tScipScanParam aParam, int aScanNum, int aScanSkip)
202255 {
203-#warning not implemented
256+ if(!scip2Do_Mx(aSensor,aParam,aScanNum,aScanSkip,'D'))
257+ {
258+ return 0;
259+ }
260+ IMPL(aSensor)->mode=MODE_MD;
261+ IMPL(aSensor)->nPts=(aParam.end-aParam.start+1)/aParam.skip;
262+ if(aScanNum==0)
263+ IMPL(aSensor)->leftScanCount=-1;
264+ else
265+ IMPL(aSensor)->leftScanCount=aScanNum;
266+ return 1;
204267 }
268+
269+//! Mxモードに遷移して測定、データ送信開始(MD/MS/ME/MI/MR)
270+/*!
271+ @param aSensor センサハンドル
272+ @param aParam スキャンパラメータ
273+ @param aScanNum 取得するスキャン数
274+ @param aScanSkip 間引きスキャン数
275+ @retval >0 成功
276+ @retval <=0 失敗
277+*/
205278 int scip2Do_ME(tScipSensor* aSensor, tScipScanParam aParam, int aScanNum, int aScanSkip)
206279 {
207-#warning not implemented
280+ if(!scip2Do_Mx(aSensor,aParam,aScanNum,aScanSkip,'E'))
281+ {
282+ return 0;
283+ }
284+ IMPL(aSensor)->mode=MODE_ME;
285+ IMPL(aSensor)->nPts=(aParam.end-aParam.start+1)/aParam.skip;
286+ if(aScanNum==0)
287+ IMPL(aSensor)->leftScanCount=-1;
288+ else
289+ IMPL(aSensor)->leftScanCount=aScanNum;
290+ return 1;
208291 }
292+
293+//! Mxモードに遷移して測定、データ送信開始(MD/MS/ME/MI/MR)
294+/*!
295+ @param aSensor センサハンドル
296+ @param aParam スキャンパラメータ
297+ @param aScanNum 取得するスキャン数
298+ @param aScanSkip 間引きスキャン数
299+ @retval >0 成功
300+ @retval <=0 失敗
301+*/
209302 int scip2Do_MI(tScipSensor* aSensor, tScipScanParam aParam, int aScanNum, int aScanSkip)
210303 {
211-#warning not implemented
304+ if(!scip2Do_Mx(aSensor,aParam,aScanNum,aScanSkip,'I'))
305+ {
306+ return 0;
307+ }
308+ IMPL(aSensor)->mode=MODE_MI;
309+ IMPL(aSensor)->nPts=(aParam.end-aParam.start+1)/aParam.skip;
310+ if(aScanNum==0)
311+ IMPL(aSensor)->leftScanCount=-1;
312+ else
313+ IMPL(aSensor)->leftScanCount=aScanNum;
314+ return 1;
212315 }
316+
317+//! Mxモードに遷移して測定、データ送信開始(MD/MS/ME/MI/MR)
318+/*!
319+ @param aSensor センサハンドル
320+ @param aParam スキャンパラメータ
321+ @param aScanNum 取得するスキャン数
322+ @param aScanSkip 間引きスキャン数
323+ @retval >0 成功
324+ @retval <=0 失敗
325+*/
213326 int scip2Do_MR(tScipSensor* aSensor, tScipScanParam aParam, int aScanNum, int aScanSkip)
214327 {
215-#warning not implemented
328+ if(!scip2Do_Mx(aSensor,aParam,aScanNum,aScanSkip,'R'))
329+ {
330+ return 0;
331+ }
332+ IMPL(aSensor)->mode=MODE_MR;
333+ IMPL(aSensor)->nPts=(aParam.end-aParam.start+1)/aParam.skip;
334+ if(aScanNum==0)
335+ IMPL(aSensor)->leftScanCount=-1;
336+ else
337+ IMPL(aSensor)->leftScanCount=aScanNum;
338+ return 1;
216339 }
217340
218341 //! Mxモードにおいて送られてくるレンジデータを取得する
@@ -227,7 +350,48 @@
227350 */
228351 int scip2GetRange(tScipSensor* aSensor, int* aRange, size_t aNum, int* aTimeStamp)
229352 {
230-#warning not implemented
353+ char echo[80],status[80];
354+ int ret;
355+
356+ if((IMPL(aSensor)->mode != MODE_MS)&&(IMPL(aSensor)->mode != MODE_MD))
357+ return 0;
358+
359+ if((ret=scipReceiveRecord(aSensor->dev,echo,status,
360+ IMPL(aSensor)->commBuff,IMPL(aSensor)->commBuffSize))>0)
361+ {
362+ if(strncmp(IMPL(aSensor)->lastCommand,echo,2+4+4+2+1)!=0)
363+ {
364+ // echo back not match
365+ fprintf(stderr,"echo back not match:\n %s !=\n %s \n",
366+ echo,IMPL(aSensor)->lastCommand);
367+ scipPurgeBuffers(aSensor->dev);
368+ return 0;
369+ }
370+ // 最後のスキャンを受信した時は OFF mode へ
371+ if(IMPL(aSensor)->leftScanCount!=0 && strncmp(&echo[2+4+4+2+1],"00",2)==0)
372+ {
373+ IMPL(aSensor)->mode=MODE_OFF;
374+ }
375+ if(strcmp(status,"99b\n")!=0)
376+ {
377+ fprintf(stderr,"status is not OK: %s\n",status);
378+ if(strcmp(status,"10Q")==0)
379+ IMPL(aSensor)->mode=MODE_OFF;
380+ strncpy(IMPL(aSensor)->lastStatus,status,4);
381+ return 0;
382+ }
383+
384+ if(strlen(IMPL(aSensor)->commBuff))
385+ {
386+ return decodeRangeData(IMPL(aSensor)->commBuff, 3,
387+ IMPL(aSensor)->nPts,
388+ aRange,
389+ aTimeStamp);
390+ }
391+ return 0;
392+ }
393+ scipPurgeBuffers(aSensor->dev);
394+ return ret;
231395 }
232396
233397 //! Mxモードにおいて送られてくる複合データを取得する
@@ -242,6 +406,7 @@
242406 */
243407 int scip2GetMultiplex(tScipSensor* aSensor, int* aRange, int* aIntensity, size_t aNum,int *aTimeStamp)
244408 {
409+
245410 #warning not implemented
246411 }
247412
@@ -307,6 +472,40 @@
307472
308473
309474 // ------------------------------------------------------------
475+
476+//! MS/MD/ME/MR/MI リクエスト発行
477+int scip2Do_Mx(tScipSensor* aSensor, tScipScanParam aParam, int aScanNum, int aScanSkip, char aSubcommand)
478+{
479+ char line[80],echo[80],status[80];
480+ int ret;
481+ if(aParam.skip==0)
482+ aParam.skip=1;
483+
484+ sprintf(line,"M%c%04d%04d%02d%01d%02d;0X\n",
485+ aSubcommand,aParam.start,aParam.end,aParam.skip,aScanSkip,aScanNum);
486+ strcpy(IMPL(aSensor)->lastCommand,line);
487+ scipSendCommand(aSensor->dev,line);
488+
489+ if((ret=scipReceiveRecord(aSensor->dev,echo,status,
490+ IMPL(aSensor)->commBuff,IMPL(aSensor)->commBuffSize))>0)
491+ {
492+ if(strcmp(line,echo)!=0)
493+ {
494+ // echo back not match
495+ fprintf(stderr,"echo back not match:\n %s !=\n %s \n",echo,line);
496+ scipPurgeBuffers(aSensor->dev);
497+ return 0;
498+ }
499+ if(strcmp(status,"00P\n")!=0)
500+ {
501+ fprintf(stderr,"status is not OK: %s\n",status);
502+ strncpy(IMPL(aSensor)->lastStatus,status,4);
503+ return 0;
504+ }
505+ }
506+ return 1;
507+}
508+
310509 int decodeRangeData(char * aBuff, int aSize, int aPts,int *aRange,int *aTimeStamp)
311510 {
312511 // decode
--- libscip2/trunk/src/scipio-linux.c (revision 25)
+++ libscip2/trunk/src/scipio-linux.c (revision 26)
@@ -125,7 +125,7 @@
125125 pf.revents=0;
126126 if(poll(&pf,1,aTimeoutMs))
127127 {
128- if(pf.revents|POLLIN)
128+ if(pf.revents&POLLIN)
129129 return 1;
130130 return 0;
131131 }
@@ -304,6 +304,7 @@
304304 tio.c_cc[VMIN] =0;
305305
306306 aDev->timeoutMs=aTimeoutMs;
307+ return 1;
307308
308309 // Commit
309310 if(tcsetattr(aDev->fd,TCSADRAIN,&tio)==0)
--- libscip2/trunk/CMakeLists.txt (revision 25)
+++ libscip2/trunk/CMakeLists.txt (revision 26)
@@ -4,3 +4,4 @@
44 set(CMAKE_BUILD_TYPE Debug)
55 add_subdirectory(src)
66 add_subdirectory(test)
7+add_subdirectory(include)
Show on old repository browser