int main(int argc, char* argv[])
{
printf("[i] Start... \n");
Serial serial;
#if defined(WIN32)
serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE, true);
#elif defined(LINUX)
serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE);
#endif
if(!serial.connected())
{
printf("[!] Cant open port!\n");
return -1;
}
char c = 'y';
int res = 12;
char buf[BUF_SIZE1];
memset(buf, 0, BUF_SIZE1);
while(true)
{
serial.write( &c, 1 );
#if 0
if(res = serial.available()){
if( res = serial.Read(buf, res) ){
printf("%d %s\n", res, buf);
}
}
#else
if(serial.waitInput(1000)==0)
printf("[i] timeout!\n");
else
{
if(res = serial.available())
{
res = serial.read(buf, res);
printf("%d %s\n", res, buf);
}
}
#endif
}
serial.close();
printf("[i] End.\n");
return 0;
}
TEST(Serial, WriteCOM) {
Serial serial;
serial.open("COM7", 9600);
if(!serial.connected()){
FAIL()<<"cant open serial port!";
}
char buf[1024];
int res = 0;
while(1){
char wbuf[] = {0xff, 0xff, 0x00, 0x00, 0x00, 0x00 ,0xff};
serial.write(wbuf, sizeof(wbuf));
if( serial.waitInput(1000) > 0){
if( (res = serial.available()) > 0 ){
if( res = serial.read(buf, res) ){
printf("[i] read data(%d): \n", res);
for(int i=0; i<res; i++){
printf("%02X ", (unsigned char)buf[i]);
if(i>0 && (i+1)%16 == 0) {
printf("\t");
for(int j=i-15; j<=i; j++){
printf("%c", buf[j]);
}
printf("\n");
}
}
printf("\n");
res = 0;
}
}
}
Sleep(1000);
}
}
void sensors_thread()
{
TBuff<uint8_t> buff;
orcp2::packet pkt;
buff.resize(2048);
pkt.message.resize(256);
if(!buff.data || !pkt.message.data) {
ROS_ERROR("[!] Error: cant allocate memory!\n");
return ;
}
if(!sensors_serial.connected()) {
ROS_WARN("Port not opened!\n");
return;
}
while (!global_stop) {
process_serial(sensors_serial, buff, pkt);
}
}