mbedserial.cpp

00001 // mbedserial.cpp
00002 // serial communication : mbed <-> ROS
00003 
00004 #include "mbedserial.h"
00005 
00006 /* init */
00007 void _nullfunc(){ ; };
00008 
00009 Mbedserial::Mbedserial(Serial &pc) : rospc(pc)
00010 {
00011     msg_buf = new char[256];
00012     bufsize = 0;
00013     endmsg = '\n';
00014     floatarraysize = 0;
00015     intarraysize = 0;
00016     chararraysize = 0;
00017     for(int i=0;i<3;i++){
00018         pfunccb[i] = _nullfunc;
00019     }
00020     rospc.attach(callback(this,&Mbedserial::rcv_callback),Serial::RxIrq);
00021 }
00022 
00023 /* receive */
00024 void Mbedserial::rcv_callback()
00025 {
00026     msg_buf[bufsize] = rospc.getc();
00027     if(msg_buf[bufsize] == endmsg){
00028         switch(msg_buf[0]){
00029             case 'f':
00030                 floatarraysize = *(int *)(&msg_buf[1]);
00031                 //memcpy(&floatarraysize,&msg_buf[1],4);
00032                 if(bufsize == floatarraysize * 4 + 5){
00033                     for(int i=0;i<floatarraysize;i++){
00034                         getfloat[i] = *(float *)(&msg_buf[i*4+5]);
00035                         //memcpy(&getfloat[i],&msg_buf[i*4+5]);
00036                     }
00037                     pfunccb[0]();
00038                 }
00039                 break;
00040             case 'i':
00041                 intarraysize = *(int *)(&msg_buf[1]);
00042                 //memcpy(&intarraysize, &msg_buf[1], 4);
00043                 if (bufsize == intarraysize * 4 + 5)
00044                 {
00045                     for (int i = 0; i < intarraysize; i++)
00046                     {
00047                         getint[i] = *(int *)(&msg_buf[i * 4 + 5]);
00048                         //memcpy(&getint[i], &msg_buf[i * 4 + 5], 4);
00049                     }
00050                     pfunccb[1]();
00051                 }
00052                 break;
00053             case 'c':
00054                 chararraysize = *(int *)(&msg_buf[1]);
00055                 //memcpy(&chararraysize, &msg_buf[1], 4);
00056                 if (bufsize == chararraysize + 5)
00057                 {
00058                     memcpy(&getchar[0], &msg_buf[5], chararraysize);
00059                     pfunccb[2]();
00060                 }
00061                 break;
00062         }
00063         delete[] msg_buf;
00064         bufsize = 0;
00065         msg_buf = new char[256];
00066     }else{
00067         bufsize++;
00068     }
00069 }
00070 
00071 /* send */
00072 int pub_wait_time = 8; //ms
00073 
00074 void Mbedserial::float_write(float array[], int arraysize)
00075 {
00076     // send data type
00077     char msg_type = 'f';
00078     rospc.putc(msg_type);
00079 
00080     // send array size
00081     char arraysize_c[4];
00082     *(int *)arraysize_c = arraysize;
00083     //memcpy(arraysize_c, &arraysize, 4);
00084     for (int i = 0; i < 4; i++)
00085     {
00086         rospc.putc(arraysize_c[i]);
00087     }
00088 
00089     // send float data
00090     char array_c[4];
00091     for (int i = 0; i < arraysize; i++)
00092     {
00093         *(float *)array_c = array[i];
00094         //memcpy(array_c, &array[i], 4);
00095         for (int j = 0; j < 4; j++)
00096         {
00097             rospc.putc(array_c[j]);
00098         }
00099     }
00100 
00101     // send end message
00102     rospc.putc(endmsg);
00103     //wait_ms(pub_wait_time);
00104 }
00105 
00106 void Mbedserial::int_write(int *array, int arraysize)
00107 {
00108     // send data type
00109     char msg_type = 'i';
00110     rospc.putc(msg_type);
00111 
00112     // send array size
00113     char arraysize_c[4];
00114    
00115     *(int *)arraysize_c = arraysize;
00116     //memcpy(arraysize_c, &arraysize, 4);
00117     for (int i = 0; i < 4; i++)
00118     {
00119         rospc.putc(arraysize_c[i]);
00120     }
00121 
00122     // send int data
00123     char array_c[4];
00124     for (int i = 0; i < arraysize; i++)
00125     {
00126         *(int *)array_c = array[i];
00127         //memcpy(array_c, &array[i], 4);
00128         for (int j = 0; j < 4; j++)
00129         {
00130             rospc.putc(array_c[j]);
00131         }
00132     }
00133 
00134     // send end message
00135     rospc.putc(endmsg);
00136     //wait_ms(pub_wait_time);
00137 }
00138 
00139 void Mbedserial::char_write(char *array, int arraysize)
00140 {
00141     // send data type
00142     char msg_type = 'c';
00143     rospc.putc(msg_type);
00144 
00145     // send array size
00146     char arraysize_c[4];
00147     *(int *)arraysize_c = arraysize;
00148     //memcpy(arraysize_c, &arraysize, 4);
00149     for (int i = 0; i < 4; i++)
00150     {
00151         rospc.putc(arraysize_c[i]);
00152     }
00153 
00154     // send char data
00155     for (int i = 0; i < arraysize; i++)
00156     {
00157         rospc.putc(array[i]);
00158     }
00159 
00160     // send end message
00161     rospc.putc(endmsg);
00162     //wait_ms(pub_wait_time);
00163 }