00001
00015 #include "kb_stargazer.h"
00016 #include "korebot.h"
00017
00018 #include <termio.h>
00019 #include <math.h>
00020
00021
00022 #define PORT_NAME "/dev/ttyS2" // port S0 on PC
00023
00024
00025 #define BATTERY_GPIO 16
00026
00027
00028 #define CMD_CALC_STOP "~#CalcStop`"
00029 #define CMD_CALC_STOP_ACK "~!CalcStop`"
00030 #define CMD_CALC_START "~#CalcStart`"
00031 #define CMD_CALC_START_ACK "~!CalcStart`"
00032 #define CMD_VERSION "~@Version`"
00033 #define CMD_VERSION_ACK "~$Version|"
00034 #define CMD_DEADZONE_ACK "DeadZone"
00035 #define CMD_MAPID_ACK "MAPID"
00036
00037
00038 #define CMD_LANDMARK_NB "IDNum"
00039 #define CMD_REFERENCE_ID "RefID"
00040 #define CMD_HEIGHT_FIX "HeightFix"
00041 #define CMD_MARK_TYPE "MarkType"
00042 #define CMD_MARK_MODE "MarkMode"
00043 #define CMD_SET_END "SetEnd"
00044 #define CMD_PARAM_UPDATE "~!ParameterUpdate`"
00045 #define CMD_MAP_MODE_START "~#MapMode|Start`"
00046
00047 #define ACK_CHAR '!'
00048
00049 static int SerialDeviceHandle=-1;
00050
00051
00052
00053 const char *kb_gazer_landmark_types[] = {"HLD1S","HLD1L","HLD2S","HLD2L","HLD3S","HLD3L"};
00054 const char *kb_gazer_landmark_modes[] = {"Alone","Map"};
00055 const char *kb_gazer_height_fix_modes[] = {"No","Yes"};
00056
00057
00058 #define MAX_STOP 10 // number times of tyring to stop the Stargazer of sending data
00059
00060 #define MAX_BUF 2048 // buffer size for char array
00061
00062
00063 double a_axis,b_axis,center_x0,center_y0,angle_rot;
00064
00065
00066 #undef DEBUG
00067 #undef DEBUG_FULL
00068
00069 #ifdef DEBUG
00070 static FILE *debug_file=NULL;
00071 #endif
00072
00073
00074
00075 #define BOOKSTEIN 0
00076 #define TAUBIN 1
00077 #define FPF 2
00078
00079
00080
00081 #define CONFIGURE_TIMEOUT 60 // timeout until calibration error [s]
00082 #define STOP_THRESHOLD_ANGLE 5 // difference threshold of angle when the robot stop after overpassing it
00083 #define CALIBRATION_SPEED 4500 // calibration speed
00084 #define MAX_CALIB_DATA 2048 // maximum number of data for calibration
00085
00086
00087
00088
00089
00097 void write_serial(int fd, char *text)
00098 {
00099 int i;
00100
00101 for(i = 0; i < (int)strlen(text) ; i++) {
00102 write( fd , &text[i], 1 );
00103 usleep(30000);
00104 }
00105
00106
00107 }
00108
00109
00117 int openserial(int *fd, const char *name)
00118 {
00119 struct termios tios;
00120
00121
00122 if ((*fd=open(name,O_RDWR | O_NOCTTY | O_NDELAY))<0) {
00123 return -1;
00124 }
00125
00126 tcgetattr(*fd , &tios);
00127 tios.c_cflag = ( CS8 | CLOCAL | CREAD | B115200 );
00128 tios.c_iflag = IGNPAR;
00129 tios.c_oflag = 0;
00130 tios.c_lflag = 0;
00131 tios.c_cc[VMIN] = 0;
00132 tios.c_cc[VTIME] = 0;
00133 tcsetattr( *fd , TCSANOW , &tios );
00134
00135 fcntl(*fd, F_SETFL, 0);
00136
00137 #ifdef DEBUG
00138 debug_file=fopen("debug.dat","w");
00139 #endif
00140 return 0;
00141 }
00142
00143
00144
00145
00159 void rotate(double *A, int i, int j, int k, int l, double tau, double s,int n)
00160 {
00161 double g,h;
00162 g=A[i*(n+1)+j];
00163 h=A[k*(n+1)+l];
00164 A[i*(n+1)+j]=g-s*(h+g*tau);
00165 A[k*(n+1)+l]=h+s*(g-h*tau);
00166 }
00167
00168
00181 int jacobi(double *a, int n, double *d , double *v, int *nrot)
00182 {
00183 int j,iq,ip,i;
00184 double tresh,theta,tau,t,sm,s,h,g,c;
00185
00186 double b[6+1]={0};
00187 double z[6+1]={0};
00188
00189 if (n>6)
00190 return;
00191
00192 for (ip=1;ip<=n;ip++) {
00193 for (iq=1;iq<=n;iq++) v[ip*(n+1)+iq]=0.0;
00194 v[ip*(n+1)+ip]=1.0;
00195 }
00196
00197 for (ip=1;ip<=n;ip++) {
00198 b[ip]=d[ip]=a[ip*(n+1)+ip];
00199 z[ip]=0.0;
00200 }
00201
00202 *nrot=0;
00203 for (i=1;i<=50;i++) {
00204 sm=0.0;
00205 for (ip=1;ip<=n-1;ip++) {
00206 for (iq=ip+1;iq<=n;iq++)
00207 sm += fabs(a[ip*(n+1)+iq]);
00208 }
00209 if (sm == 0.0) {
00210
00211
00212 return 0;
00213 }
00214 if (i < 4)
00215 tresh=0.2*sm/(n*n);
00216 else
00217 tresh=0.0;
00218 for (ip=1;ip<=n-1;ip++) {
00219 for (iq=ip+1;iq<=n;iq++) {
00220 g=100.0*fabs(a[ip*(n+1)+iq]);
00221 if (i > 4 && fabs(d[ip])+g == fabs(d[ip])
00222 && fabs(d[iq])+g == fabs(d[iq]))
00223 a[ip*n+iq]=0.0;
00224 else if (fabs(a[ip*(n+1)+iq]) > tresh) {
00225 h=d[iq]-d[ip];
00226 if (fabs(h)+g == fabs(h))
00227 t=(a[ip*(n+1)+iq])/h;
00228 else {
00229 theta=0.5*h/(a[ip*(n+1)+iq]);
00230 t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
00231 if (theta < 0.0) t = -t;
00232 }
00233 c=1.0/sqrt(1+t*t);
00234 s=t*c;
00235 tau=s/(1.0+c);
00236 h=t*a[ip*(n+1)+iq];
00237 z[ip] -= h;
00238 z[iq] += h;
00239 d[ip] -= h;
00240 d[iq] += h;
00241 a[ip*(n+1)+iq]=0.0;
00242 for (j=1;j<=ip-1;j++) {
00243 rotate(&a[0],j,ip,j,iq,tau,s,n);
00244 }
00245 for (j=ip+1;j<=iq-1;j++) {
00246 rotate(&a[0],ip,j,j,iq,tau,s,n);
00247 }
00248 for (j=iq+1;j<=n;j++) {
00249 rotate(&a[0],ip,j,iq,j,tau,s,n);
00250 }
00251 for (j=1;j<=n;j++) {
00252 rotate(&v[0],j,ip,j,iq,tau,s,n);
00253 }
00254 (*nrot)++;
00255 }
00256 }
00257 }
00258 for (ip=1;ip<=n;ip++) {
00259 b[ip] += z[ip];
00260 d[ip]=b[ip];
00261 z[ip]=0.0;
00262 }
00263 }
00264
00265 return -1;
00266 }
00267
00268
00269
00279 int choldc(double *a, int n, double *l)
00280 {
00281 int i,j,k;
00282 double sum;
00283 double p[6+1];
00284
00285 if (n>6)
00286 return;
00287
00288 for (i=1; i<=n; i++) {
00289 for (j=i; j<=n; j++) {
00290 for (sum=a[i*(n+1)+j],k=i-1;k>=1;k--) sum -= a[i*(n+1)+k]*a[j*(n+1)+k];
00291 if (i == j) {
00292 if (sum<=0.0)
00293 {
00294 return -1;
00295 }
00296 else
00297 p[i]=sqrt(sum);
00298 }
00299 else
00300 {
00301 a[j*(n+1)+i]=sum/p[i];
00302 }
00303 }
00304 }
00305 for (i=1; i<=n; i++)
00306 for (j=i; j<=n; j++)
00307 if (i==j)
00308 l[i*(n+1)+i] = p[i];
00309 else
00310 {
00311 l[j*(n+1)+i]=a[j*(n+1)+i];
00312 l[i*(n+1)+j]=0.0;
00313 }
00314
00315 return 0;
00316 }
00317
00318
00329 int inverse(double *TB, double *InvB, int N) {
00330 int k,i,j,p,q;
00331 double mult;
00332 double D,temp;
00333 double maxpivot;
00334 int npivot;
00335 double B[(6+1)*(6+2)]={0};
00336 double A[(6+1)*(2*6+2)]={0};
00337 double C[(6+1)*(6+1)]={0};
00338 double eps = 10e-20;
00339
00340
00341 if (N>6)
00342 return -2;
00343
00344 for(k=1;k<=N;k++)
00345 for(j=1;j<=N;j++)
00346 B[k*(N+2)+j]=TB[k*(N+1)+j];
00347
00348
00349
00350
00351 for (k=1;k<=N;k++)
00352 {
00353 for (j=1;j<=N+1;j++)
00354 A[k*(2*N+2)+j]=B[k*(N+2)+j];
00355
00356
00357 for (j=N+2;j<=2*N+1;j++)
00358 A[k*(2*N+2)+j]=0;
00359 A[k*(2*N+2)+k-1+N+2]=1;
00360 }
00361
00362
00363
00364 for (k=1;k<=N;k++)
00365 {
00366 maxpivot=fabs(A[k*(2*N+2)+k]);
00367 npivot=k;
00368
00369
00370 for (i=k;i<=N;i++) {
00371
00372 if (maxpivot<fabs(A[i*(2*N+2)+k]))
00373 {
00374 maxpivot=fabs(A[i*(2*N+2)+k]);
00375 npivot=i;
00376
00377 }
00378 }
00379 if (maxpivot>=eps)
00380 {
00381 if (npivot!=k)
00382 for (j=k;j<=2*N+1;j++)
00383 {
00384
00385 temp=A[npivot*(2*N+2)+j];
00386 A[npivot*(2*N+2)+j]=A[k*(2*N+2)+j];
00387 A[k*(2*N+2)+j]=temp;
00388 } ;
00389
00390
00391
00392 D=A[k*(2*N+2)+k];
00393 for (j=2*N+1;j>=k;j--)
00394 A[k*(2*N+2)+j]=A[k*(2*N+2)+j]/D;
00395
00396
00397
00398 for (i=1;i<=N;i++)
00399 {
00400 if (i!=k)
00401 {
00402 mult=A[i*(2*N+2)+k];
00403 for (j=2*N+1;j>=k;j--)
00404 A[i*(2*N+2)+j]=A[i*(2*N+2)+j]-mult*A[k*(2*N+2)+j] ;
00405 }
00406 }
00407
00408 }
00409 else
00410 {
00411 return(-1);
00412 };
00413 }
00414
00415
00416 for (k=1,p=1;k<=N;k++,p++)
00417 for (j=N+2,q=1;j<=2*N+1;j++,q++)
00418 InvB[p*(N+1)+q]=A[k*(2*N+2)+j];
00419 return(0);
00420 }
00421
00422
00423
00427 void AperB(double *_A, double *_B, double *_res,
00428 int _righA, int _colA, int _righB, int _colB)
00429 {
00430 int p,q,l;
00431 for (p=1;p<=_righA;p++)
00432 for (q=1;q<=_colB;q++)
00433 {
00434 _res[p*(_colB+1)+q]=0.0;
00435 for (l=1;l<=_colA;l++)
00436 _res[p*(_colB+1)+q]=_res[p*(_colB+1)+q]+_A[p*(_colA+1)+l]*_B[l*(_colB+1)+q];
00437 }
00438 }
00439
00440
00444 void A_TperB(double *_A, double *_B, double *_res,
00445 int _righA, int _colA, int _righB, int _colB)
00446 {
00447 int p,q,l;
00448 for (p=1;p<=_colA;p++)
00449 for (q=1;q<=_colB;q++)
00450 { _res[p*(_colB+1)+q]=0.0;
00451 for (l=1;l<=_righA;l++)
00452 _res[p*(_colB+1)+q]=_res[p*(_colB+1)+q]+_A[l*(_colA+1)+p]*_B[l*(_colB+1)+q];
00453 }
00454 }
00455
00456
00460 void AperB_T(double *_A, double *_B, double *_res,
00461 int _righA, int _colA, int _righB, int _colB)
00462 {
00463 int p,q,l;
00464 for (p=1;p<=_colA;p++)
00465 for (q=1;q<=_colB;q++)
00466 {
00467 _res[p*(_colB+1)+q]=0.0;
00468 for (l=1;l<=_righA;l++)
00469 _res[p*(_colB+1)+q]=_res[p*(_colB+1)+q]+_A[p*(_righA+1)+l]*_B[q*(_righA+1)+l];
00470 }
00471 }
00472
00473
00474
00490 int compute_ellipse(double x[], double y[],int np,double *_x0,double *_y0,double *_angle,double *_a,double *_b)
00491 {
00492
00493 double D[MAX_CALIB_DATA+1][7]={0};
00494 double S[7][7]={0};
00495 double Const[7][7]={0};
00496 double temp[7][7]={0};
00497 double L[7][7]={0};
00498 double C[7][7]={0};
00499
00500 double invL[7][7]={0};
00501 double dd[7]={0};
00502 double V[7][7]={0};
00503 double sol[7][7]={0};
00504 double tx,ty;
00505 int nrot=0;
00506 int npts=50;
00507
00508 double XY[3][MAX_CALIB_DATA+1];
00509 double pvec[7];
00510
00511
00512 int mode,i,j,solind;
00513
00514 double mod, zero,minev;
00515
00516
00517 double x0,y0,a,b,c,d,f,g,a2,b2,angle;
00518
00519
00520 mode=BOOKSTEIN;
00521
00522 switch (mode) {
00523 case (FPF):
00524 Const[1][3]=-2;
00525 Const[2][2]=1;
00526 Const[3][1]=-2;
00527 break;
00528 case(TAUBIN):
00529 break;
00530 case(BOOKSTEIN):
00531 Const[1][1]=2;
00532 Const[2][2]=1;
00533 Const[3][3]=2;
00534 break;
00535 default:
00536 break;
00537 }
00538
00539 if (np<6)
00540 return;
00541
00542
00543 for (i=1; i <= np; i++)
00544 {
00545 tx = x[i-1];
00546 ty = y[i-1];
00547 D[i][1] = tx*tx;
00548 D[i][2] = tx*ty;
00549 D[i][3] = ty*ty;
00550 D[i][4] = tx;
00551 D[i][5] = ty;
00552 D[i][6] = 1.0;
00553 }
00554
00555
00556
00557
00558 A_TperB(&D[0][0],&D[0][0],&S[0][0],np,6,np,6);
00559
00560
00561 choldc(&S[0][0],6,&L[0][0]);
00562
00563
00564 if (inverse(&L[0][0],&invL[0][0],6) !=0)
00565 {
00566 printf("ERROR inverting matrix L may be singular!\n");
00567
00568
00569 return -1;
00570 }
00571
00572
00573 AperB_T(&Const[0][0],&invL[0][0],&temp[0][0],6,6,6,6);
00574 AperB(&invL[0][0],&temp[0][0],&C[0][0],6,6,6,6);
00575
00576
00577 jacobi(&C[0][0],6,&dd[0],&V[0][0],&nrot);
00578
00579
00580
00581 A_TperB(&invL[0][0],&V[0][0],&sol[0][0],6,6,6,6);
00582
00583
00584
00585 for (j=1;j<=6;j++)
00586 {
00587 mod = 0.0;
00588 for (i=1;i<=6;i++)
00589 mod += sol[i][j]*sol[i][j];
00590 for (i=1;i<=6;i++)
00591 sol[i][j] /= sqrt(mod);
00592 }
00593
00594
00595
00596 zero=10e-20;
00597 minev=10e+20;
00598 solind=0;
00599
00600 switch (mode) {
00601 case(BOOKSTEIN):
00602 for (i=1; i<=6; i++)
00603 if (dd[i]<minev && fabs(dd[i])>zero)
00604 solind = i;
00605 break;
00606 case(FPF):
00607 for (i=1; i<=6; i++)
00608 if (dd[i]<0 && fabs(dd[i])>zero)
00609 solind = i;
00610 }
00611
00612
00613 for (j=1;j<=6;j++)
00614 pvec[j] = sol[j][solind];
00615
00616
00617
00618
00619 g = pvec[6];
00620 d = pvec[4]/2;
00621 f = pvec[5]/2;
00622 a = pvec[1];
00623 c = pvec[3];
00624 b = pvec[2]/2;
00625
00626
00627
00628
00629 x0=(c*d-b*f)/(b*b-a*c);
00630 y0=(a*f-b*d)/(b*b-a*c);
00631
00632
00633 a2=sqrt( 2*(a*f*f+c*d*d+g*b*b-2*b*d*f-a*c*g)/((b*b-a*c)*(sqrt((a-c)*(a-c)+4*b*b)-(a+c))));
00634 b2=sqrt( 2*(a*f*f+c*d*d+g*b*b-2*b*d*f-a*c*g)/((b*b-a*c)*(-sqrt((a-c)*(a-c)+4*b*b)-(a+c))));
00635
00636
00637 angle=0;
00638
00639 if((b== 0) && (a<c))
00640 {
00641 angle=0;
00642 }
00643 else if((b==0) && (a>c))
00644 {
00645 angle= 90;
00646 }
00647 else if((b!=0) && (a<c))
00648 {
00649 angle= (0.5*atan((2*b)/(a-c)))*180.0/M_PI;
00650 }
00651 else if((b!=0) && (a>c))
00652 {
00653 angle= 90+(0.5*atan((2*b)/(a-c)))*180.0/M_PI;
00654 }
00655
00656
00657
00658 *_x0=x0;
00659 *_y0=y0;
00660 *_a=a2;
00661 *_b=b2;
00662 *_angle=angle;
00663
00664 return 0;
00665
00666 }
00667
00668
00669
00670
00671
00681 int kb_gazer_get_version(char *version)
00682 {
00683 int nbytes;
00684 char cread[MAX_BUF];
00685 char *dch, *ech;
00686
00687 if (SerialDeviceHandle<0)
00688 return -1;
00689
00690 write_serial(SerialDeviceHandle , CMD_VERSION );
00691
00692
00693 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
00694 read(SerialDeviceHandle,cread,nbytes);
00695 cread[nbytes]='\0';
00696
00697 dch = strstr(cread,CMD_VERSION_ACK);
00698
00699 #ifdef DEBUG
00700 printf("DEBUG: kb_gazer_get_version cread: %s\n",cread);
00701 #endif
00702
00703 if (dch != NULL )
00704 {
00705 dch+=10;
00706 ech=strchr(dch,'`');
00707
00708 if (ech != NULL) {
00709
00710 if (version == NULL)
00711 {
00712 return -2;
00713 }
00714
00715 memcpy(version,dch,ech-dch);
00716 version[ech-dch]='\0';
00717
00718 #ifdef DEBUG
00719 printf("DEBUG: kb_gazer_get_version version: %s\n",version);
00720 #endif
00721
00722 return 0;
00723 }
00724 }
00725 return -3;
00726 }
00727
00728
00736 int kb_gazer_stop_computation()
00737 {
00738 int nbytes;
00739 char cread[MAX_BUF];
00740 char *dch;
00741
00742 if (SerialDeviceHandle<0)
00743 {
00744 return -1;
00745 }
00746
00747 write_serial(SerialDeviceHandle , CMD_CALC_STOP );
00748
00749 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
00750 read(SerialDeviceHandle,cread,nbytes);
00751 cread[nbytes]='\0';
00752
00753 #ifdef DEBUG
00754 printf("DEBUG: kb_gazer_stop_computation cread (nb chars: %d): %s\n",nbytes,cread);
00755 #endif
00756
00757 dch = strstr(cread,CMD_CALC_STOP_ACK);
00758
00759 if (dch != NULL )
00760 {
00761 return 0;
00762 }
00763 return -2;
00764 }
00765
00766
00774 int kb_gazer_wait_stop_computation()
00775 {
00776 int i,ret;
00777
00778
00779
00780 for(i=0;i<MAX_STOP;i++)
00781 {
00782 if ((ret=kb_gazer_stop_computation()) == 0)
00783 {
00784 #ifdef DEBUG
00785 printf("DEBUG: kb_gazer_wait_stop_computation stop after %d time(s)\n",i+1);
00786 #endif
00787 return 0;
00788 }
00789
00790 usleep(100000);
00791 }
00792
00793 return ret;
00794 }
00795
00796
00797
00805 int kb_gazer_start_computation()
00806 {
00807 int nbytes;
00808 char cread[MAX_BUF];
00809 char *dch;
00810
00811 if (SerialDeviceHandle<0)
00812 {
00813 return -1;
00814 }
00815
00816 write_serial(SerialDeviceHandle, CMD_CALC_START );
00817
00818 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
00819 read(SerialDeviceHandle,cread,nbytes);
00820 cread[nbytes]='\0';
00821
00822 dch = strstr(cread,CMD_CALC_START_ACK);
00823
00824 if (dch == NULL )
00825 {
00826 return -2;
00827 }
00828
00829 usleep(200000);
00830
00831 return 0;
00832 }
00833
00834
00846 int kb_stargazer_Init(char *DeviceName)
00847 {
00848 int recv_n = 0;
00849 int ret=0,i;
00850
00851
00852
00853 if (kb_gpio_init()!=0)
00854 {
00855 return -1;
00856 }
00857
00858
00859 kb_gpio_function(BATTERY_GPIO,0);
00860 kb_gpio_dir(BATTERY_GPIO,0);
00861 kb_gpio_set(BATTERY_GPIO);
00862
00863 sleep(3);
00864 usleep(500000);
00865
00866
00867 if (openserial(&SerialDeviceHandle,PORT_NAME)<0)
00868 {
00869 return -2;
00870 }
00871
00872 if (kb_gazer_wait_stop_computation() == 0)
00873 return 0;
00874
00875 return -3;
00876 }
00877
00878
00887 int kb_gazer_set_landmark_number(int number)
00888 {
00889 int nbytes;
00890 char buf[MAX_BUF],rep[MAX_BUF];
00891 char *dch;
00892
00893 if (SerialDeviceHandle<0)
00894 {
00895 return -1;
00896 }
00897
00898
00899 sprintf(buf,"~#%s|%d`",CMD_LANDMARK_NB,number);
00900
00901 write_serial(SerialDeviceHandle , buf );
00902
00903 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
00904 read(SerialDeviceHandle,rep,nbytes);
00905 rep[nbytes]='\0';
00906
00907
00908 buf[1]='!';
00909
00910 dch = strstr(rep,buf);
00911
00912 if (dch != NULL )
00913 {
00914 return 0;
00915 }
00916 return -2;
00917 }
00918
00919
00928 int kb_gazer_get_landmark_number(int *number)
00929 {
00930 int nbytes;
00931 char buf[MAX_BUF],rep[MAX_BUF];
00932 char *dch;
00933
00934 if (SerialDeviceHandle<0)
00935 {
00936 return -1;
00937 }
00938
00939
00940 sprintf(buf,"~@%s`",CMD_LANDMARK_NB);
00941
00942 write_serial(SerialDeviceHandle , buf );
00943
00944 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
00945 read(SerialDeviceHandle,rep,nbytes);
00946 rep[nbytes]='\0';
00947
00948 sprintf(buf,"~$%s|",CMD_LANDMARK_NB);
00949
00950 dch = strstr(rep,buf);
00951
00952 if (dch != NULL )
00953 {
00954 if (sscanf(dch,"~$IDNum|%d",number)==1)
00955 {
00956 return 0;
00957 }
00958 }
00959 return -2;
00960 }
00961
00962
00971 int kb_gazer_set_ref_id(int refid)
00972 {
00973 int nbytes;
00974 char buf[MAX_BUF],rep[MAX_BUF];
00975 char *dch;
00976
00977 if (SerialDeviceHandle<0)
00978 {
00979 return -1;
00980 }
00981
00982
00983 sprintf(buf,"~#%s|%d`",CMD_REFERENCE_ID,refid);
00984
00985 write_serial(SerialDeviceHandle , buf );
00986
00987 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
00988 read(SerialDeviceHandle,rep,nbytes);
00989 rep[nbytes]='\0';
00990
00991
00992 buf[1]='!';
00993
00994 dch = strstr(rep,buf);
00995
00996 if (dch != NULL )
00997 {
00998 return 0;
00999 }
01000 return -2;
01001 }
01002
01003
01012 int kb_gazer_get_ref_id(int *refid)
01013 {
01014 int nbytes;
01015 char buf[MAX_BUF],rep[MAX_BUF];
01016 char *dch;
01017
01018 if (SerialDeviceHandle<0)
01019 {
01020 return -1;
01021 }
01022
01023
01024 sprintf(buf,"~@%s`",CMD_REFERENCE_ID);
01025
01026 write_serial(SerialDeviceHandle , buf );
01027
01028 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01029 read(SerialDeviceHandle,rep,nbytes);
01030 rep[nbytes]='\0';
01031
01032 sprintf(buf,"~$%s|",CMD_REFERENCE_ID);
01033
01034 dch = strstr(rep,buf);
01035
01036 if (dch != NULL )
01037 {
01038 if (sscanf(dch,"~$RefID|%d",refid)==1)
01039 {
01040 return 0;
01041 }
01042 }
01043 return -2;
01044 }
01045
01046
01056 int kb_gazer_set_landmark_type(int type)
01057 {
01058 int nbytes;
01059 char buf[MAX_BUF],rep[MAX_BUF];
01060 char *dch;
01061
01062 if (SerialDeviceHandle<0)
01063 {
01064 return -1;
01065 }
01066
01067
01068 if ((type < 0) || (type >5))
01069 return -3;
01070
01071 sprintf(buf,"~#%s|%s`",CMD_MARK_TYPE,kb_gazer_landmark_types[type]);
01072
01073 write_serial(SerialDeviceHandle , buf );
01074
01075 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01076 read(SerialDeviceHandle,rep,nbytes);
01077 rep[nbytes]='\0';
01078
01079
01080 buf[1]='!';
01081
01082 dch = strstr(rep,buf);
01083
01084 if (dch != NULL )
01085 {
01086 return 0;
01087 }
01088 return -2;
01089 }
01090
01091
01101 int kb_gazer_get_landmark_type(int *type)
01102 {
01103 int nbytes,i;
01104 char buf[MAX_BUF],rep[MAX_BUF];
01105 char *dch;
01106
01107 if (SerialDeviceHandle<0)
01108 {
01109 return -1;
01110 }
01111
01112
01113 sprintf(buf,"~@%s`",CMD_MARK_TYPE);
01114
01115 write_serial(SerialDeviceHandle , buf );
01116
01117 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01118 read(SerialDeviceHandle,rep,nbytes);
01119 rep[nbytes]='\0';
01120
01121 sprintf(buf,"~$%s|",CMD_MARK_TYPE);
01122
01123 dch = strstr(rep,buf);
01124
01125 if (dch != NULL )
01126 {
01127
01128 for (i=0;i<NB_MARK_TYPES;i++)
01129 {
01130 if (strstr(rep,kb_gazer_landmark_types[i]) != NULL)
01131 {
01132 *type=i;
01133 return 0;
01134 }
01135 }
01136
01137 *type=0;
01138 return -3;
01139
01140
01141 }
01142 return -2;
01143 }
01144
01145
01155 int kb_gazer_set_landmark_mode(int mode)
01156 {
01157 int nbytes;
01158 char buf[MAX_BUF],rep[MAX_BUF];
01159 char *dch;
01160
01161 if (SerialDeviceHandle<0)
01162 {
01163 return -1;
01164 }
01165
01166
01167 if ((mode < 0) || (mode >1))
01168 return -3;
01169
01170 sprintf(buf,"~#%s|%s`",CMD_MARK_MODE,kb_gazer_landmark_modes[mode]);
01171
01172 write_serial(SerialDeviceHandle , buf );
01173
01174 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01175 read(SerialDeviceHandle,rep,nbytes);
01176 rep[nbytes]='\0';
01177
01178
01179 buf[1]='!';
01180
01181 dch = strstr(rep,buf);
01182
01183 if (dch != NULL )
01184 {
01185 return 0;
01186 }
01187 return -2;
01188 }
01189
01190
01200 int kb_gazer_get_landmark_mode(int *mode)
01201 {
01202 int nbytes,i;
01203 char buf[MAX_BUF],rep[MAX_BUF];
01204 char *dch;
01205
01206 if (SerialDeviceHandle<0)
01207 {
01208 return -1;
01209 }
01210
01211
01212 sprintf(buf,"~@%s`",CMD_MARK_MODE);
01213
01214 write_serial(SerialDeviceHandle , buf );
01215
01216 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01217 read(SerialDeviceHandle,rep,nbytes);
01218 rep[nbytes]='\0';
01219
01220 sprintf(buf,"~$%s|",CMD_MARK_MODE);
01221
01222 dch = strstr(rep,buf);
01223
01224 if (dch != NULL )
01225 {
01226
01227 for (i=0;i<NB_MARK_MODES;i++)
01228 {
01229 if (strstr(rep,kb_gazer_landmark_modes[i]) != NULL)
01230 {
01231 *mode=i;
01232 return 0;
01233 }
01234 }
01235
01236 *mode=0;
01237 return -3;
01238 }
01239
01240 return -2;
01241 }
01242
01243
01244
01254 int kb_gazer_set_height_fix_mode(int mode)
01255 {
01256 int nbytes;
01257 char buf[MAX_BUF],rep[MAX_BUF];
01258 char *dch;
01259
01260 if (SerialDeviceHandle<0)
01261 {
01262 return -1;
01263 }
01264
01265
01266 if ((mode < 0) || (mode >1))
01267 return -3;
01268
01269 sprintf(buf,"~#%s|%s`",CMD_HEIGHT_FIX,kb_gazer_height_fix_modes[mode]);
01270
01271 write_serial(SerialDeviceHandle , buf );
01272
01273 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01274 read(SerialDeviceHandle,rep,nbytes);
01275 rep[nbytes]='\0';
01276
01277
01278 buf[1]=ACK_CHAR;
01279
01280
01281 dch = strstr(rep,buf);
01282
01283 if (dch != NULL )
01284 {
01285 return 0;
01286 }
01287 return -2;
01288 }
01289
01290
01300 int kb_gazer_get_height_fix_mode(int *mode)
01301 {
01302 int nbytes,i;
01303 char buf[MAX_BUF],rep[MAX_BUF];
01304 char *dch;
01305
01306 if (SerialDeviceHandle<0)
01307 {
01308 return -1;
01309 }
01310
01311
01312 sprintf(buf,"~@%s`",CMD_HEIGHT_FIX);
01313
01314 write_serial(SerialDeviceHandle , buf );
01315
01316 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01317 read(SerialDeviceHandle,rep,nbytes);
01318 rep[nbytes]='\0';
01319
01320 sprintf(buf,"~$%s|",CMD_HEIGHT_FIX);
01321
01322 dch = strstr(rep,buf);
01323
01324 if (dch != NULL )
01325 {
01326
01327 for (i=0;i<NB_HEIGHT_FIX_MODES;i++)
01328 {
01329 if (strstr(rep,kb_gazer_height_fix_modes[i]) != NULL)
01330 {
01331 *mode=i;
01332 return 0;
01333 }
01334 }
01335
01336 *mode=0;
01337 return -3;
01338 }
01339
01340 return -2;
01341 }
01342
01343
01344
01354 int kb_gazer_start_map_mode()
01355 {
01356 int nbytes;
01357 char buf[MAX_BUF],rep[MAX_BUF];
01358 char *dch;
01359
01360 if (SerialDeviceHandle<0)
01361 {
01362 return -1;
01363 }
01364
01365 if (kb_gazer_wait_stop_computation()!=0)
01366 {
01367 return -2;
01368 }
01369
01370
01371 sprintf(buf,"%s",CMD_MAP_MODE_START);
01372
01373 write_serial(SerialDeviceHandle , buf );
01374
01375 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01376 read(SerialDeviceHandle,rep,nbytes);
01377 rep[nbytes]='\0';
01378
01379
01380 buf[1]=ACK_CHAR;
01381
01382
01383 dch = strstr(rep,buf);
01384
01385 if (dch != NULL )
01386 {
01387 return 0;
01388 }
01389 return -4;
01390 }
01391
01392
01401 int kb_gazer_set_end_command()
01402 {
01403 int nbytes;
01404 char buf[MAX_BUF],rep[MAX_BUF];
01405 char *dch;
01406
01407 if (SerialDeviceHandle<0)
01408 {
01409 return -1;
01410 }
01411
01412
01413 sprintf(buf,"~#%s`",CMD_SET_END);
01414
01415 write_serial(SerialDeviceHandle , buf );
01416
01417 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01418 read(SerialDeviceHandle,rep,nbytes);
01419 rep[nbytes]='\0';
01420
01421
01422 buf[1]='!';
01423
01424 dch = strstr(rep,buf);
01425
01426 if (dch == NULL )
01427 {
01428 return -2;
01429 }
01430
01431
01432
01433 sleep(3);
01434 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01435 read(SerialDeviceHandle,rep,nbytes);
01436 rep[nbytes]='\0';
01437
01438 dch = strstr(rep,CMD_PARAM_UPDATE);
01439
01440 if (dch == NULL )
01441 {
01442 return -3;
01443 }
01444
01445
01446 return 0;
01447 }
01448
01449
01477 int kb_stargazer_read_data(double *x,double *y,double *z,double *angle, int *idnum, char *cmode, int corr)
01478 {
01479 int nbytes,ret,id;
01480 static int deb=0;
01481 static char cread[MAX_BUF];
01482 char *dch;
01483 double xc=0,yc=0;
01484
01485 if (SerialDeviceHandle<0)
01486 {
01487 return -1;
01488 }
01489
01490 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01491
01492 if ((nbytes+deb)>=MAX_BUF)
01493 {
01494
01495 tcflush(SerialDeviceHandle,TCIFLUSH);
01496 deb=0;
01497 ioctl(SerialDeviceHandle, FIONREAD, &nbytes);
01498
01499 if (nbytes>=MAX_BUF)
01500 {
01501 return -3;
01502 }
01503 }
01504 read(SerialDeviceHandle,cread+deb,nbytes);
01505
01506 cread[nbytes+deb]='\0';
01507 #ifdef DEBUG_FULL
01508 kb_erase_line(4);
01509 printf("DEBUG: kb_stargazer_read_data cread (nb chars: %d): %s",nbytes,cread);
01510 #endif
01511
01512
01513
01514 if (nbytes>0)
01515 {
01516 if (strstr(cread,CMD_DEADZONE_ACK) != NULL)
01517 {
01518 return -6;
01519 }
01520 else
01521
01522
01523 if ((strstr(cread,CMD_MAPID_ACK) != NULL) && (cread[nbytes+deb-1]=='`'))
01524 {
01525
01526 if ((ret=sscanf(cread+8,"%d",&id)) != 1)
01527 {
01528 return -9;
01529 }
01530
01531 return id;
01532 }
01533 else
01534
01535 if (strstr(cread,CMD_PARAM_UPDATE) != NULL)
01536 {
01537 return 1;
01538 }
01539 else
01540
01541 if ((cread[0]=='~') && (cread[1]=='^') && (cread[nbytes+deb-1]=='`') )
01542 {
01543
01544
01545
01546 if ((ret=sscanf(cread,"%*c%*c%c%d%*c%lf%*c%lf%*c%lf%*c%lf",cmode,idnum,angle,x,y,z)) != 6)
01547 {
01548
01549
01550
01551
01552 return -5;
01553 }
01554
01555
01556
01557
01558
01559
01560
01561
01562 *angle = -(*angle-90);
01563
01564
01565 if (*angle <0)
01566 (*angle)+=360;
01567 else
01568 if (*angle >360)
01569 (*angle)-=360;
01570
01571 #ifdef DEBUG
01572 fprintf(debug_file,"%.1f\t%.1f\t%.1f\t%.1f\t%d\n",*x,*y,*z,*angle,*idnum);
01573 #endif
01574
01575
01576 if (corr==1)
01577 {
01578 xc=a_axis*cos((*angle+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*cos(angle_rot*M_PI/180.0)-b_axis*sin((*angle+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*sin(angle_rot*M_PI/180.0);
01579
01580 yc=a_axis*cos((*angle+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*sin(angle_rot*M_PI/180.0)+b_axis*sin((*angle+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*cos(angle_rot*M_PI/180.0);
01581 }
01582
01583 (*x)-=xc;
01584 (*y)-=yc;
01585
01586 deb=0;
01587 return 0;
01588 }
01589 else
01590 {
01591
01592 return -7;
01593 }
01594 }
01595
01596
01597 return -8;
01598 }
01599
01600
01605 void kb_stargazer_Close()
01606 {
01607 if (SerialDeviceHandle>=0)
01608 {
01609 close(SerialDeviceHandle);
01610 }
01611 #ifdef DEBUG
01612 if (debug_file != NULL)
01613 fclose(debug_file);
01614 #endif
01615
01616
01617 kb_gpio_clear(BATTERY_GPIO);
01618 kb_gpio_cleanup();
01619
01620 }
01621
01622
01645 int kb_gazer_calibration(knet_dev_t * mot1,knet_dev_t * mot2,double *_center_x0,double *_center_y0,double *_angle_rot,double *_a_axis, double *_b_axis, double *_stddev_x, double *_stddev_y)
01646 {
01647 double x,y,z,angle, angle_start;
01648 int idnum,stop=0,ret,nb_data=0;
01649 char cmode;
01650
01651
01652 int i;
01653 double xc,yc,stddevx,stddevy;
01654
01655 struct timeval startTime,endTime;
01656
01657 #ifdef DEBUG
01658 FILE *file;
01659 #endif
01660
01661 double data_x[MAX_CALIB_DATA],data_y[MAX_CALIB_DATA],angle_t[MAX_CALIB_DATA];
01662
01663
01664
01665
01666
01667 ret=kb_stargazer_read_data(&x,&y,&z,&angle_start,&idnum,&cmode,0);
01668
01669
01670 if (ret!=0)
01671 {
01672
01673 return ret-4;
01674 }
01675
01676 #ifdef DEBUG
01677 file=fopen("revolution.dat","w");
01678 #endif
01679
01680
01681 kmot_SetPoint( mot1 , kMotRegSpeed , -CALIBRATION_SPEED);
01682 kmot_SetPoint( mot2 , kMotRegSpeed , CALIBRATION_SPEED );
01683
01684 usleep(500000);
01685
01686 gettimeofday(&startTime, NULL);
01687
01688 while (!stop)
01689 {
01690 ret=kb_stargazer_read_data(&x,&y,&z,&angle,&idnum,&cmode,0);
01691
01692
01693
01694
01695
01696 if (nb_data>MAX_CALIB_DATA)
01697 {
01698 #ifdef DEBUG
01699 fclose(file);
01700 #endif
01701 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
01702 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
01703 return -2;
01704 }
01705
01706 data_x[nb_data]=x;
01707 data_y[nb_data]=y;
01708 angle_t[nb_data]=angle;
01709
01710 #ifdef DEBUG
01711 fprintf(file,"%.2f\t%.2f\t%.2f\t%.2f\t%d\n",x,y,z,angle,idnum);
01712 #endif
01713
01714 nb_data++;
01715
01716 gettimeofday(&endTime, NULL);
01717
01718
01719 if ( ( (endTime.tv_sec - startTime.tv_sec) > 2) && ( abs( (angle<angle_start?360+angle:angle)- angle_start ) < STOP_THRESHOLD_ANGLE ) )
01720 {
01721 stop = 1;
01722 }
01723
01724
01725 if ((endTime.tv_sec - startTime.tv_sec) > CONFIGURE_TIMEOUT)
01726 {
01727 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
01728 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
01729 #ifdef DEBUG
01730 fclose(file);
01731 #endif
01732 return -1;
01733 }
01734
01735 printf("\033[%d`",1);
01736 printf("\033[K");
01737
01738
01739 printf(" start angle: %5.1f current angle: %5.1f done: %4.1f %%",angle_start,angle,((angle<angle_start?360:0)-angle_start+angle)/3.60);
01740 fflush(stdout);
01741
01742 usleep(100000);
01743
01744 }
01745
01746 #ifdef DEBUG
01747 fclose(file);
01748 #endif
01749
01750 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
01751 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
01752
01753
01754 printf("\033[%d`",1);
01755 printf("\033[K");
01756
01757
01758 printf(" start angle: %5.1f current angle: %5.1f\n",angle_start,angle);
01759
01760
01761 if ((ret=compute_ellipse(data_x,data_y,nb_data,¢er_x0,¢er_y0,&angle_rot,&a_axis,&b_axis))!=0)
01762 {
01763 printf("\nERROR: computing ellipse parameters (error : %d)!\n",ret);
01764 return -3;
01765 }
01766
01767
01768
01769
01770
01771
01772 stddevx=0;
01773 stddevy=0;
01774 for (i=0;i<nb_data;i++)
01775 {
01776 xc=-data_x[i]+center_x0+a_axis*cos((angle_t[i]+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*cos(angle_rot*M_PI/180.0)-b_axis*sin((angle_t[i]+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*sin(angle_rot*M_PI/180.0);
01777
01778 stddevx+=xc*xc;
01779
01780 yc=-data_y[i]+center_y0+a_axis*cos((angle_t[i]+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*sin(angle_rot*M_PI/180.0)+b_axis*sin((angle_t[i]+ANGLE_CORRECTION-angle_rot)*M_PI/180.0)*cos(angle_rot*M_PI/180.0);
01781 stddevy+=yc*yc;
01782 }
01783
01784 stddevx=sqrt(stddevx/nb_data);
01785 stddevy=sqrt(stddevy/nb_data);
01786
01787 #ifdef DEBUG
01788 printf("DEBUG: ellipse parameters: centre: (%4.1f,%4.1f) angle: %4.1f major axe: %4.1f minor axe: %4.1f stddevx = %4.2f stddevy = %4.2f\n",center_x0,center_y0,angle_rot,2*a_axis,2*b_axis,stddevx,stddevy);
01789 #endif
01790
01791 *_angle_rot=angle_rot;
01792 *_a_axis=a_axis;
01793 *_b_axis=b_axis;
01794 *_center_x0=center_x0;
01795 *_center_y0=center_y0;
01796 *_stddev_x=stddevx;
01797 *_stddev_y=stddevy;
01798
01799 if ( (stddevx>CALIB_STDEV_MAX) || (stddevy>CALIB_STDEV_MAX) || isnan(stddevx) || isnan(stddevy))
01800 {
01801 return -4;
01802 }
01803
01804
01805 return 0;
01806 }
01807