• R/O
  • SSH
  • HTTPS

scip: Commit


Commit MetaInfo

Revision28 (tree)
Time2009-06-13 18:24:55
Authory_o_s

Log Message

r236@vmGentooR5: yos | 2009-05-29 16:56:46 +0900
ME command handler

Change Summary

Incremental Difference

--- libscip2/trunk/include/scipio.h (revision 27)
+++ libscip2/trunk/include/scipio.h (revision 28)
@@ -37,7 +37,7 @@
3737 @retval !NULL I/Oハンドル
3838 @retval NULL 失敗
3939 */
40- LIBSCIP2_DECL tScipIO scipIOOpen(char *aDevName);
40+ LIBSCIP2_DECL tScipIO scipIOOpen(const char *aDevName);
4141
4242 //! Close オペレーション
4343 /*!
--- libscip2/trunk/src/scip2command.c (revision 27)
+++ libscip2/trunk/src/scip2command.c (revision 28)
@@ -12,7 +12,9 @@
1212 #include <stdio.h>
1313 #include <string.h>
1414
15+#define DEBUG
1516
17+
1618 //! 受信バッファなど内部で使用するデータ
1719 typedef struct {
1820 char *commBuff;
@@ -35,7 +37,7 @@
3537 MODE_MD=0x11, // 3Bytes 距離
3638 MODE_MR=0x12, // 反射係数 (3Bytes距離 + 2Bytes 反射係数)
3739 MODE_MI=0x13, // 受光強度 (3Bytes距離 + 4Bytes 受光レベル等)
38- MODE_ME=0x14, // HOKUYOモード
40+ MODE_ME=0x14, // HOKUYOモード (3Bytes距離 + 3Bytes 受光レベル等)
3941 MODE_FAULT=-1,
4042 };
4143
@@ -49,6 +51,10 @@
4951 // レンジデータ文字列の解釈
5052 int decodeRangeData(char * aBuff, int size, int aPts,int *aRange,int *aTimeStamp);
5153
54+// 複合データ文字列の解釈
55+int decodeMultiplexData(char * aBuff, int aSize, int AttrSize, int aPts,
56+ int *aRange,int *aAttr, int *aTimeStamp);
57+
5258 // 応答ステータスコードを数値にエンコード
5359 int encodeStatus(char *aStatus);
5460
@@ -59,7 +65,7 @@
5965 }
6066
6167 //! scip2デバイスを開き、scip2command ハンドルを作る
62-tScipSensor *scip2CommandOpen(char* aDevName)
68+tScipSensor *scip2CommandOpen(const char* aDevName)
6369 {
6470 tScipSensor *sensor;
6571 sensor=(tScipSensor*)malloc(sizeof(tScipSensor));
@@ -259,6 +265,12 @@
259265 }
260266 IMPL(aSensor)->mode=MODE_MD;
261267 IMPL(aSensor)->nPts=(aParam.end-aParam.start+1)/aParam.skip;
268+ if(IMPL(aSensor)->commBuffSize<IMPL(aSensor)->nPts*(3)+1)
269+ {
270+ free(IMPL(aSensor)->commBuff);
271+ IMPL(aSensor)->commBuff=malloc(IMPL(aSensor)->nPts*(3)+1);
272+ IMPL(aSensor)->commBuffSize=IMPL(aSensor)->nPts*(3)+1;
273+ }
262274 if(aScanNum==0)
263275 IMPL(aSensor)->leftScanCount=-1;
264276 else
@@ -283,6 +295,12 @@
283295 }
284296 IMPL(aSensor)->mode=MODE_ME;
285297 IMPL(aSensor)->nPts=(aParam.end-aParam.start+1)/aParam.skip;
298+ if(IMPL(aSensor)->commBuffSize<IMPL(aSensor)->nPts*(3+3)+1)
299+ {
300+ free(IMPL(aSensor)->commBuff);
301+ IMPL(aSensor)->commBuff=malloc(IMPL(aSensor)->nPts*(3+3)+1);
302+ IMPL(aSensor)->commBuffSize=IMPL(aSensor)->nPts*(3+3)+1;
303+ }
286304 if(aScanNum==0)
287305 IMPL(aSensor)->leftScanCount=-1;
288306 else
@@ -351,14 +369,22 @@
351369 int scip2GetRange(tScipSensor* aSensor, int* aRange, size_t aNum, int* aTimeStamp)
352370 {
353371 char echo[80],status[80];
354- int ret;
372+ int ret=0;
373+ int rangeSize=3;
355374
356375 if((IMPL(aSensor)->mode != MODE_MS)&&(IMPL(aSensor)->mode != MODE_MD))
357376 return 0;
358377
378+ if(IMPL(aSensor)->mode == MODE_MS)
379+ rangeSize=2;
380+
359381 if((ret=scipReceiveRecord(aSensor->dev,echo,status,
360382 IMPL(aSensor)->commBuff,IMPL(aSensor)->commBuffSize))>0)
361383 {
384+#ifdef DEBUG
385+ fprintf(stderr,"scip2GetRange>\nECHO:%s\nSTAT:%s\n,DATA------\n%s\n------\n",
386+ echo,status,IMPL(aSensor)->commBuff);
387+#endif
362388 if(strncmp(IMPL(aSensor)->lastCommand,echo,2+4+4+2+1)!=0)
363389 {
364390 // echo back not match
@@ -383,7 +409,7 @@
383409
384410 if(strlen(IMPL(aSensor)->commBuff))
385411 {
386- return decodeRangeData(IMPL(aSensor)->commBuff, 3,
412+ return decodeRangeData(IMPL(aSensor)->commBuff, rangeSize,
387413 IMPL(aSensor)->nPts,
388414 aRange,
389415 aTimeStamp);
@@ -406,7 +432,64 @@
406432 */
407433 int scip2GetMultiplex(tScipSensor* aSensor, int* aRange, int* aIntensity, size_t aNum,int *aTimeStamp)
408434 {
409-//#warning not implemented
435+ char echo[80],status[80];
436+ int ret, attrSize;
437+
438+ if((IMPL(aSensor)->mode != MODE_MR)&&
439+ (IMPL(aSensor)->mode != MODE_MI)&&
440+ (IMPL(aSensor)->mode != MODE_ME))
441+ return 0;
442+
443+ switch(IMPL(aSensor)->mode)
444+ {
445+ case MODE_MR:
446+ attrSize=2;
447+ break;
448+ case MODE_MI:
449+ attrSize=4;
450+ break;
451+ case MODE_ME:
452+ attrSize=3;
453+ break;
454+ }
455+
456+ if((ret=scipReceiveRecord(aSensor->dev,echo,status,
457+ IMPL(aSensor)->commBuff,IMPL(aSensor)->commBuffSize))>0)
458+ {
459+ if(strncmp(IMPL(aSensor)->lastCommand,echo,2+4+4+2+1)!=0)
460+ {
461+ // echo back not match
462+ fprintf(stderr,"echo back not match:\n %s !=\n %s \n",
463+ echo,IMPL(aSensor)->lastCommand);
464+ scipPurgeBuffers(aSensor->dev);
465+ return 0;
466+ }
467+ // 最後のスキャンを受信した時は OFF mode へ
468+ if(IMPL(aSensor)->leftScanCount!=0 && strncmp(&echo[2+4+4+2+1],"00",2)==0)
469+ {
470+ IMPL(aSensor)->mode=MODE_OFF;
471+ }
472+ if(strcmp(status,"99b\n")!=0)
473+ {
474+ fprintf(stderr,"status is not OK: %s\n",status);
475+ if(strcmp(status,"10Q")==0)
476+ IMPL(aSensor)->mode=MODE_OFF;
477+ strncpy(IMPL(aSensor)->lastStatus,status,4);
478+ return 0;
479+ }
480+
481+ if(strlen(IMPL(aSensor)->commBuff))
482+ {
483+ return decodeMultiplexData(IMPL(aSensor)->commBuff, 3, attrSize,
484+ IMPL(aSensor)->nPts,
485+ aRange, aIntensity,
486+ aTimeStamp);
487+ }
488+ return 0;
489+ }
490+ // receive record failed
491+ scipPurgeBuffers(aSensor->dev);
492+ return ret;
410493 }
411494
412495 //! 通信速度設定 (SS)
@@ -534,7 +617,7 @@
534617 return 0;
535618 }
536619 // 余った端数分のデータを eol+sum (2文字)分後ろにコピーし
537- // 次の行の行頭につなく
620+ // 次の行の行頭につなぐ
538621 for(i=1;i<=size;++i)
539622 {
540623 *(nextline-i)=*(nextline-i-2);
@@ -550,6 +633,94 @@
550633 return count;
551634 }
552635
636+//! インタリーブされたアトリビュート付き距離データをデコード
637+/*!
638+ @param[in] aBuff 入力文字列
639+ @param[in] aSize 距離データのサイズ
640+ @param[in] aAttrSize アトリビュートデータのサイズ
641+ @param[in] aPts データ点数
642+
643+ @param[out] aRange デコードした距離データ
644+ @param[out] aAttr デコードしたアトリビュートデータ
645+ @param[out] aTimeStamp デコードしたタイムスタンプ
646+ */
647+int decodeMultiplexData(char * aBuff, int aSize, int aAttrSize, int aPts,
648+ int *aRange,int *aAttr, int *aTimeStamp)
649+{
650+ // decode
651+ char *bol=aBuff;
652+ int size,count=0;
653+ int newsize;
654+
655+ size=scipCheckLine(bol,0);
656+ *aTimeStamp=scipDecode6(bol,4);
657+ bol+=size+2;
658+ size=scipCheckLine(bol,0);
659+ if(!size)
660+ {
661+ // checksum fail
662+ return 0;
663+ }
664+ while(count<aPts)
665+ {
666+ // レンジデータ
667+ if(size<aSize)
668+ {
669+ newsize=feedNextLine(&bol,size);
670+ if(!newsize)
671+ return 0;
672+ size+=newsize;
673+ }
674+ aRange[count]=scipDecode6(bol,aSize);
675+ size-=aSize;
676+ bol+=aSize;
677+
678+ // アトリビュートデータ
679+ if(size<aAttrSize)
680+ {
681+ newsize=feedNextLine(&bol,size);
682+ if(!newsize)
683+ return 0;
684+ size+=newsize;
685+ }
686+ aAttr[count]=scipDecode6(bol,aAttrSize);
687+ size-=aAttrSize;
688+ bol+=aAttrSize;
689+ ++count;
690+ }
691+ return count;
692+}
693+/*!
694+ @param[inout] bol 注目位置
695+ @param[in] bytesLeft 注目位置以降で同じ行にある有効なデータ文字数
696+
697+ @retval 0 行check sum 失敗
698+ @retval >0 残り文字数
699+ */
700+int feedNextLine(char **bol,int bytesLeft)
701+{
702+ // refill
703+ int check;
704+ char *nextline=*bol+bytesLeft+2; // 注目位置+残り文字数+sum+eol
705+ int i;
706+
707+ check=scipCheckLine(nextline,0);
708+ if(!check)
709+ {
710+ // checksum fail
711+ return 0;
712+ }
713+ // 余った端数分のデータを eol+sum (2文字)分後ろにコピーし
714+ // 次の行の行頭につなぐ
715+ for(i=1;i<=bytesLeft;++i)
716+ {
717+ *(nextline-i)=*(nextline-i-2);
718+ }
719+ *bol+=2;
720+ return check;
721+}
722+
723+
553724 // 応答ステータスコードを数値にエンコード
554725 int encodeStatus(char *aStatus)
555726 {
--- libscip2/trunk/src/scipio-linux.c (revision 27)
+++ libscip2/trunk/src/scipio-linux.c (revision 28)
@@ -48,7 +48,7 @@
4848 @retval !NULL I/Oハンドル
4949 @retval NULL 失敗
5050 */
51-tScipIO scipIOOpen(char *aDevName)
51+tScipIO scipIOOpen(const char *aDevName)
5252 {
5353 tScipIO handle=(tScipIOImpl*)malloc(sizeof(tScipIOImpl));
5454 if(!handle)
Show on old repository browser