int Java_org_yabause_android_YabauseRunnable_initViewport()
{
int error;
char * buf;
g_Display = eglGetCurrentDisplay();
g_Surface = eglGetCurrentSurface(EGL_READ);
g_Context = eglGetCurrentContext();
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrthof(0, 320, 224, 0, 1, 0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glMatrixMode(GL_TEXTURE);
glLoadIdentity();
glDisable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
yprintf(glGetString(GL_VENDOR));
yprintf(glGetString(GL_RENDERER));
yprintf(glGetString(GL_VERSION));
yprintf(glGetString(GL_EXTENSIONS));
yprintf(eglQueryString(g_Display,EGL_EXTENSIONS));
eglSwapInterval(g_Display,0);
eglMakeCurrent(g_Display,EGL_NO_SURFACE,EGL_NO_SURFACE,EGL_NO_CONTEXT);
return 0;
}
void FrequencyManager_ControlCurrentFrequency () {
static uint32_t lastFrequency = 0;
uint32_t frequency = s_bandsData[s_selectedBand].CurrentFrequency;
// Output control
if(!Si570_isEnabled()) {
if (frequency != lastFrequency) {
debug (CONTROL, "Serial -> *F%d\n", frequency);
debug (CONTROL, "Serial -> *OF2\n");
yprintf ("*F%d\n", frequency);
yprintf ("*OF2\n"); // turn on output
lastFrequency = frequency;
}
}
}
//
// Yoics Printf, we do not protect against buffer overflows, do not
// print strings longer than ~400 bytes
//
void
yprintf(const char *fmt, ...)
{
int ret;
//int idlen=0;
char *buff;
va_list args;
struct sockaddr_in client; /* Information about the client */
// calculate idlen
if(0!=logport)
{
if( strncmp(log_id,printf_buffer,strlen(log_id)) )
yprintf("corrupted log_id\n");
buff=strchr(printf_buffer,' ');
buff++;
}
else
buff=printf_buffer;
va_start(args, fmt);
vsprintf(buff, fmt, args);
va_end(args);
// Check if logging is enabled
if(0==logport)
{
if(LOG_PRINTF&lmask)
{
printlog(LOG_PRINTF, "%s", printf_buffer);
}
#if !defined(SUPRESS_PRINTFS)
printf("%s", printf_buffer);
#endif
}
else
{
// clear out socket structure
memset((void *)&client, '\0', sizeof(struct sockaddr_in));
//
// Send message to localhost on log_port
//
client.sin_family = AF_INET;
client.sin_addr.s_addr = 0x0100007f;
client.sin_port = htons(logport); // Destination port
//
//printf("send pkt at %x len %d\n",(int)pkt,len);
ret=sendto(logsocket , (char *)printf_buffer, strlen(printf_buffer), 0, (struct sockaddr *)&client, sizeof(struct sockaddr));
//printf("end\n");
if(ret<0)
log_lasterror((U8 *) "yprintf error");
}
}
void escape_road( void )
{
#if defined(__MINGW32__)
if( !SetConsoleCtrlHandler( win32_ctrlc_handler, TRUE ) )
{
yprintf( OUTPUT_LV_ERROR, "Error: Win32 Ctrl+C handler registration failed.\n" );
}
#else
signal( SIGINT, emergency );
#endif
}
int initEGLFunc()
{
void * handle;
char *error;
handle = dlopen("libEGL.so",RTLD_LAZY);
if( handle == NULL )
{
yprintf(dlerror());
return -1;
}
eglGetCurrentDisplay = dlsym(handle, "eglGetCurrentDisplay");
if( eglGetCurrentDisplay == NULL){ yprintf(dlerror()); return -1; }
eglGetCurrentSurface = dlsym(handle, "eglGetCurrentSurface");
if( eglGetCurrentSurface == NULL){ yprintf(dlerror()); return -1; }
eglGetCurrentContext = dlsym(handle, "eglGetCurrentContext");
if( eglGetCurrentContext == NULL){ yprintf(dlerror()); return -1; }
eglQuerySurface = dlsym(handle, "eglQuerySurface");
if( eglQuerySurface == NULL){ yprintf(dlerror()); return -1; }
eglSwapInterval = dlsym(handle, "eglSwapInterval");
if( eglSwapInterval == NULL){ yprintf(dlerror()); return -1; }
eglMakeCurrent = dlsym(handle, "eglMakeCurrent");
if( eglMakeCurrent == NULL){ yprintf(dlerror()); return -1; }
eglSwapBuffers = dlsym(handle, "eglSwapBuffers");
if( eglSwapBuffers == NULL){ yprintf(dlerror()); return -1; }
eglQueryString = dlsym(handle, "eglQueryString");
if( eglQueryString == NULL){ yprintf(dlerror()); return -1; }
eglGetError = dlsym(handle, "eglGetError");
if( eglGetError == NULL){ yprintf(dlerror()); return -1; }
return 0;
}
void YuiSwapBuffers(void)
{
int buf_width, buf_height;
int error;
pthread_mutex_lock(&g_mtxGlLock);
if( g_Display == EGL_NO_DISPLAY )
{
pthread_mutex_unlock(&g_mtxGlLock);
return;
}
if( eglMakeCurrent(g_Display,g_Surface,g_Surface,g_Context) == EGL_FALSE )
{
yprintf( "eglMakeCurrent fail %04x",eglGetError());
pthread_mutex_unlock(&g_mtxGlLock);
return;
}
glClearColor( 0.0f,0.0f,0.0f,1.0f);
glClear(GL_COLOR_BUFFER_BIT);
if( g_FrameBuffer == 0 )
{
glEnable(GL_TEXTURE_2D);
glGenTextures(1,&g_FrameBuffer);
glBindTexture(GL_TEXTURE_2D, g_FrameBuffer);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, 1024, 1024, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
error = glGetError();
if( error != GL_NO_ERROR )
{
yprintf("gl error %d", error );
return;
}
}else{
glBindTexture(GL_TEXTURE_2D, g_FrameBuffer);
}
VIDCore->GetGlSize(&buf_width, &buf_height);
glTexSubImage2D(GL_TEXTURE_2D, 0,0,0,buf_width,buf_height,GL_RGBA,GL_UNSIGNED_BYTE,dispbuffer);
if( g_VertexBuffer == 0 )
{
glGenBuffers(1, &g_VertexBuffer);
glBindBuffer(GL_ARRAY_BUFFER, g_VertexBuffer);
glBufferData(GL_ARRAY_BUFFER, sizeof(vertices),vertices,GL_STATIC_DRAW);
error = glGetError();
if( error != GL_NO_ERROR )
{
yprintf("gl error %d", error );
return;
}
}else{
glBindBuffer(GL_ARRAY_BUFFER, g_VertexBuffer);
}
if( buf_width != g_buf_width || buf_height != g_buf_height )
{
vertices[6]=vertices[10]=(float)buf_width/1024.f;
vertices[11]=vertices[15]=(float)buf_height/1024.f;
glBufferData(GL_ARRAY_BUFFER, sizeof(vertices),vertices,GL_STATIC_DRAW);
glVertexPointer(2, GL_FLOAT, sizeof(float)*4, 0);
glTexCoordPointer(2, GL_FLOAT, sizeof(float)*4, (void*)(sizeof(float)*2));
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
g_buf_width = buf_width;
g_buf_height = buf_height;
}
glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
eglSwapBuffers(g_Display,g_Surface);
pthread_mutex_unlock(&g_mtxGlLock);
}
// process characters at a time for commands
char
command(void)
{
S16 ret=0;
IPADDR t;
U8 *u;
readln_from_a_file((FILE*)stdin, command_buffer, CMD_MAX_SIZE);
yprintf("command in = %s\n",command_buffer);
if(command_buffer[0]=='?')
{
// Attach to service
printf("Supported Commands:\n");
printf(" r - restart\n");
printf(" e - drop next tunnel rx packet\n");
printf(" m - check status of memory allocator\n");
printf(" p - change port\n");
printf(" s - status\n");
printf(" y - startup proxy\n");
printf(" c - connect\n");
printf(" w - send wakup command (ping server and clients)\n");
printf(" i - send direct message (specify uid and msg)\n");
#if defined(WEB_SSL)
printf(" ssl on - enable ssl\n");
printf(" ssl off - disable ssl\n");
#endif
printf(" f - files from share target (fl - directory list...)\n");
printf(" d - Get device List (only works if yoicsid client.)\n");
printf("\n q to quit bcaster\n");
printf("\n x to exit right now without cleanup.\n");
}
else
if(command_buffer[0]=='r')
{
// Attach to service
printf("Reconnect to Weaved Service...\n");
Yoics_reconnect();
//printf("current state %s\n",server_state_string[state]);
}
else if(command_buffer[0]=='p')
{
printf("Swapping local port to %d\n",Yoics_Change_Port(0));
}
else if(command_buffer[0]=='w')
{
printf("Wakeup Command\n");
send_ping(PING_STANDARD);
Yoics_Session_Force_Ping();
#if defined(TRACE_LOG)
traceDump(); // Tracedump just for test
#endif
}
else if (command_buffer[0]=='c')
{
// connect
printf("sending connect request for ");
print_uid(Remote_Address);
printf("\n");
//Session_create(&Remote_Address, 0); ////+++ remote address right?
#if defined(BCASTER)
ret=Yoics_Session_create(Remote_Address);
if(-1== ret )
{
printf("Initiate session create faild\n");
}
else if(-2==ret)
{
printf("Initiate session create faild, no slots left\n");
}
else
printf("OK ret= %d\n",ret);
#endif
#if defined(BUILD_SHARE)
new_share=Share_Connect(Remote_Address,&share_app_callback);
if(0== new_share )
{
printf("Initiate session create faild\n");
}
else
printf("OK ret= %d\n",ret);
#endif
//yoics_connect(Remote_Address);
}
else if (command_buffer[0]=='m')
{
#if ALLOC_DEBUG
alloc_check();
#else
alloc_stat();
#endif
alloc_pool_check(pool);
}
else if (command_buffer[0]=='s')
{
#if defined(WEB_SSL)
if (!strcmp(command_buffer,"ssl on")) {
printf("Turn on SSL\n");
//
// Set the web port to ssl port
//
webPort = WEB_PORT_SSL;
//
// Enable SSL module
Yoics_Set_SSL(SSL_ENABLE);
}
else if (!strcmp (command_buffer, "ssl off")) {
printf("Turn off SSL\n");
//
// Set the web port to non SSL port
//
webPort = WEB_PORT;
//
// Disable SSL module
//
Yoics_Set_SSL (SSL_DISABLE);
}
else
#endif
{
t=Yoics_Get_Current_Server();
u=Yoics_Current_UID();
printf("server state is %d -- 0)Uconn 1)rstart 2)NATchk 3)ReqAuth 4)ID 5)Conn\n",Yoics_Server_Connection_State());
printf(" server is %d.%d.%d.%d:%d\n",t.ipb1,t.ipb2,t.ipb3,t.ipb4,(U16)Yoics_Get_Current_Server_Port());
printf(" key hash is %x:%x\n",secret[0],secret[1]);
printf(" server spi is 0x%X\n",(unsigned int)Yoics_Return_Server_SPI());
printf("Our UID is %.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
u[0],u[1],u[2],u[3],u[4],u[5],u[6],u[7]);
printf("Our Serial Number is %.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
serial_num[0],serial_num[1],serial_num[2],serial_num[3],serial_num[4],serial_num[5],
serial_num[6],serial_num[7]);
t=Yoics_Get_Our_Bound_IP();
printf("Our bound ip:port is %d.%d.%d.%d:%d\n",t.ipb1,t.ipb2,t.ipb3,t.ipb4,Yoics_Get_Our_Bound_Port());
t=Yoics_Get_Our_Mapped_IP();
printf("Our mapped ip:port is %d.%d.%d.%d:%d - UPNP/NAT-PMP mapped port is %d\n",t.ipb1,t.ipb2,t.ipb3,t.ipb4,Yoics_Get_Our_Mapped_Port(),parameters.upnp_port);
printf("Last server hearbeat %u seconds ago\n",(U32)((U32)second_count()-(U32)server_hearbeat));
printf("Our NAT type is %d\n",Yoics_Return_NAT_Type());
printf("Forward Port is %d\n\n",parameters.forward_port);
alloc_stat();
printf("\nActive Sessions %d\n",active_sessions);
printf("\nSession List\n");
Yoics_session_list();
printf("\nProxy Status\n");
proxy_status();
}
}
else if (command_buffer[0]=='e')
{
//printf("Drop next tun data packet.\n");
//tunnel_drop(tun_num);
}
else if (command_buffer[0]=='y')
{
if(sindex)
{
if(proxy_local_port==0)
proxy_local_port=5959;
printf("Starting Proxy on port %d.\n",proxy_local_port);
if(proxy_client_start(proxy_ip,proxy_local_port, restrict_connect_ip, sindex))
//if(proxy_start(proxy_ip,proxy_local_port))
printf("Proxy started.\n");
else
printf("proxy startup failed.\n");
}
else
{
printf("must be connected to peer first\n");
}
}
else if (command_buffer[0]=='d')
{
printf("Device List:\n");
#if !defined(BCASTER)
if(YOICS_SERVER_CONNECTED==Yoics_Server_Connection_State())
{
DEVICE devlist[10];
char server_name[32];
int tt;
IPADDR answer;
// Get current server IP and make it into a string
answer=Yoics_Get_Current_Server();
sprintf(server_name,"%d.%d.%d.%d",answer.ipb1,answer.ipb2,answer.ipb3,answer.ipb4);
tt=Yoics_Get_Device_List(server_name, webPort, 5000, devlist, 10, 1);
printf("found %d entries\n",tt);
}
else
printf("Must be connected to service to issue this command\n");
#endif
}
else if (command_buffer[0]=='z')
{
U8* crash;
yprintf("Force SIG 11:\n");
crash=0;
*crash="c"; // This is correct, it forces a crash! Ignore this warning in compile
//session_list();
}
else if (command_buffer[0]=='q')
{
ret=EXIT;
}
else if (command_buffer[0]=='x')
{
DEBUG4("Free Pool x\n");
free_pool();
#if defined(TRACE_LOG)
traceCleanup();
#endif
exit(0);
ret=EXIT;
}
else if (command_buffer[0]=='i')
{
int len;
U8* tptr;
char tpack[255];
U8 tuid[32];
YOICSPACKET *pkt;
pkt=(YOICSPACKET*)tpack;
//
// Create a map peer
//
pkt->packet_type = htons(PING_MSG);
pkt->source = htons(TARGET_FORWARD); // not done yet
len = sizeof(YOICSPACKET);
pkt->salt = 0;
pkt->spi = 0;
tptr=pkt->data;
//
// Add our UID
//
tptr=yoics_add_data(tptr,CLIENTUID,my_uid,UID_SIZE);
len+=UID_SIZE+sizeof(DATATYPE);
//
// Add Null Type to finish the message
//
yoics_add_data(tptr,NULLTYPE,0,0);
len+=sizeof(DATATYPE);
//
// send direct message using packet forwarder
//
UID_Extract(&tuid[0],"80:00:00:05:46:00:26:e7");
send_packet_forward(tuid, (U8*)tpack ,len);
}
else
{
yprintf("unknown command = %s\n",command_buffer);
}
return((S8)ret);
}
void
print_uid(U8* uid)
{
yprintf("%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x",uid[0],uid[1],uid[2],uid[3],uid[4],uid[5],uid[6],uid[7]);
}
/* main */
int main( int argc, char *argv[] )
{
pthread_t command_thread;
pthread_t control_thread;
pthread_t update_thread;
int command_thread_en;
int control_thread_en;
int update_thread_en;
Ver_t version;
Param_t driver_param;
int i, ret;
ParametersPtr param;
char paramfile[512];
int quit;
hook_pre_global();
ret = arg_analyze( argc, argv );
if( option( OPTION_DAEMON ) )
{
#if HAVE_FORK
pid_t pid;
pid = fork( );
if( pid < 0 )
{
return -1;
}
else if( pid != 0 )
{
return 0;
}
setsid( );
if( chdir( "/" ) < 0 )
{
yprintf( OUTPUT_LV_ERROR, "Failed to chdir.\n" );
return EXIT_FAILURE;
}
close( STDIN_FILENO );
close( STDOUT_FILENO );
close( STDERR_FILENO );
#else
yprintf( OUTPUT_LV_ERROR, "Daemon mode is not supported in your system.\n" );
return EXIT_FAILURE;
#endif
}
if( option( OPTION_SHOW_HELP ) )
{
arg_help( argc, argv );
return EXIT_SUCCESS;
}
if( option( OPTION_SHOW_LONGHELP ) )
{
arg_longhelp( argc, argv );
return EXIT_SUCCESS;
}
if( option( OPTION_SHOW_PARAMHELP ) )
{
param_help( );
return EXIT_SUCCESS;
}
if( option( OPTION_VERSION ) )
{
fprintf( stderr, "YamabicoProject-Spur\n" );
fprintf( stderr, " Ver. %s\n", PACKAGE_VERSION );
return EXIT_SUCCESS;
}
if( !ret ) /* オプション解析に失敗したとき */
return EXIT_FAILURE;
yprintf( OUTPUT_LV_PROCESS, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
yprintf( OUTPUT_LV_PROCESS, "YamabicoProject-Spur\n" );
yprintf( OUTPUT_LV_PROCESS, " Ver. %s\n", PACKAGE_VERSION );
yprintf( OUTPUT_LV_PROCESS, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
/* Ctrl-C割り込みハンドラーの登録 */
escape_road( );
g_emergency = 0;
/* パラメータを読み込み、セットする */
param = get_param_ptr( );
#ifdef HAVE_SSM
/* SSM初期化 */
if( !option( OPTION_WITHOUT_SSM ) )
init_ypspurSSM( param->ssm_id );
#endif
/* 座標系の初期化、コマンド処理系の初期化 */
init_coordinate_systems( );
init_odometry( );
init_spur_command( );
fflush( stderr );
command_thread_en = 0;
command_thread_en = 0;
do
{
FILE *temp_paramfile = NULL;
quit = 0;
yprintf( OUTPUT_LV_PROCESS, "Device Information\n" );
yprintf( OUTPUT_LV_PROCESS, " Port : %s \n", param->device_name );
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
if( !serial_connect( param->device_name ) )
{
// quit=0;でbreakしたら異常終了と判断される
break;
}
if( !( option( OPTION_DO_NOT_USE_YP ) ) )
{
int current, age;
sscanf( YP_PROTOCOL_NAME, "YPP:%d:%d", ¤t, &age );
yprintf( OUTPUT_LV_PROCESS, " Checking device information...\r" );
for ( i = 0; i < 3; i++ )
{
int device_current, device_age;
// プロトコルがYPであることを確認
if( get_version( &version ) == -1 )
{
continue;
}
if( strstr( version.protocol, "YPP:" ) != version.protocol )
{
continue;
}
sscanf( version.protocol, "YPP:%d:%d", &device_current, &device_age );
if( device_current - device_age > current || device_current < current )
{
continue;
}
break;
}
yprintf( OUTPUT_LV_PARAM, " Vender : %s\033[K\n", version.vender );
yprintf( OUTPUT_LV_PARAM, " Product : %s\n", version.product );
yprintf( OUTPUT_LV_PARAM, " Firmware: %s\n", version.firmware );
yprintf( OUTPUT_LV_PARAM, " Protcol : %s\n", version.protocol );
yprintf( OUTPUT_LV_PARAM, " Serialno: %s\n", version.serialno );
yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
if( i == 3 )
{
yprintf( OUTPUT_LV_ERROR, "Error: Device doesn't have available YP protocol version.\n" );
if( option( OPTION_RECONNECT ) && g_emergency == 0 )
{
yp_usleep( 500000 );
continue;
}
break; // quit=0でbreakしたら異常終了と判断
}
}
fflush( stderr );
if( get_parameter( &driver_param ) == -1 )
{
continue;
}
yprintf( OUTPUT_LV_PARAM, "Driver depending parameters\n" );
yprintf( OUTPUT_LV_PARAM, " Name : %s\n", driver_param.robot_name );
yprintf( OUTPUT_LV_PARAM, " PWM resolution: %s\n", driver_param.pwm_resolution );
yprintf( OUTPUT_LV_PARAM, " Motor number : %s\n", driver_param.motor_num );
yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
if( strlen( driver_param.pwm_resolution ) <= 0 ||
strlen( driver_param.motor_num ) <= 0 )
{
yprintf( OUTPUT_LV_ERROR, "Error: Failed to load driver parameters.\n" );
if( option( OPTION_RECONNECT ) && g_emergency == 0 )
{
yp_usleep( 500000 );
continue;
}
break;
}
}
if( !( option( OPTION_PARAM_FILE ) ) )
{
// パラメータファイルが指定されておらず、ドライバにパラメータが内蔵されている場合
if( strcmp( driver_param.robot_name, "embedded" ) == 0 )
{
char param[2048];
yprintf( OUTPUT_LV_MODULE, "Reading device embedded parameter.\n" );
temp_paramfile = tmpfile( );
if( !temp_paramfile )
{
yprintf( OUTPUT_LV_ERROR, "Error: Failed to create temporary file.\n" );
return 0;
}
if( !get_embedded_param( param ) )
{
yprintf( OUTPUT_LV_ERROR, "Error: Failed to read embedded parameters.\n" );
if( option( OPTION_RECONNECT ) && g_emergency == 0 )
{
yp_usleep( 500000 );
continue;
}
break;
}
fprintf( temp_paramfile, "%s", param );
fseek( temp_paramfile, 0L, SEEK_SET );
}
// パラメータファイルが指定されておらず、ドライバにロボット名が登録されている場合
else if( strlen( driver_param.robot_name ) > 0 && strcmp( driver_param.robot_name, "unknown" ) != 0 )
{
strcpy( param->parameter_filename, driver_param.robot_name );
strcat( param->parameter_filename, ".param" );
}
}
if( temp_paramfile )
{
yprintf( OUTPUT_LV_PARAM, "Embedded parameter file\n" );
if( !set_paramptr( temp_paramfile ) )
{
yprintf( OUTPUT_LV_ERROR, "Error: Cannot use embedded parameter.\n" );
return 0;
}
}
else
{
yprintf( OUTPUT_LV_PARAM, "Parameter file: %s\n", param->parameter_filename );
if( !set_param( param->parameter_filename, paramfile ) )
{
yprintf( OUTPUT_LV_ERROR, "Error: Cannot find parameter file.\n" );
return 0;
}
}
{
int i;
for ( i = 0; i < YP_PARAM_MOTOR_NUM; i++ )
{
*pp( YP_PARAM_PWM_MAX, i ) = atoi( driver_param.pwm_resolution );
}
}
yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n\n" );
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
// ボーレートの設定
if( param->speed )
{
yprintf( OUTPUT_LV_MODULE, "Setting baudrate to %d baud.\n", param->speed );
}
else {
// 指定されてない場合デフォルトの値
param->speed = DEFAULT_BAUDRATE;
}
ret = set_baudrate( param->speed );
if( ret == 0 )
{
// 設定失敗
yprintf( OUTPUT_LV_WARNING, "Error: Failed to change baudrate.\n" );
serial_close( );
quit = 0;
break; // quit=0でbreakしたら異常終了と判断
}
if (ret == 4)
{
// ボーレートの設定未対応
yprintf( OUTPUT_LV_WARNING, "Warn: Baudrate setting is not supported on this device.\n" );
}
else
{
// 設定成功
// 正常ならば何もしない
}
if( param->admask )
{
yprintf( OUTPUT_LV_MODULE, "Setting admask to %x.\n", param->admask );
set_admask( param->admask );
}
if( option( OPTION_ENABLE_GET_DIGITAL_IO ) )
{
yprintf( OUTPUT_LV_MODULE, "Enabling digital io input.\n" );
set_diomask( 1 );
}
if( !( option( OPTION_PARAM_CONTROL ) ) )
{
apply_robot_params( );
}
/* サーボをかける */
if( state( YP_STATE_MOTOR ) && state( YP_STATE_VELOCITY ) )
{
motor_servo( );
}
}
yprintf( OUTPUT_LV_MODULE, "YP-Spur coordinator started.\n" );
/* スレッド初期化 */
init_command_thread( &command_thread );
pthread_detach( command_thread );
command_thread_en = 1;
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
init_control_thread( &control_thread );
pthread_detach( control_thread );
control_thread_en = 1;
}
if( option( OPTION_UPDATE_PARAM ) )
{
init_param_update_thread( &update_thread, paramfile );
pthread_detach( update_thread );
update_thread_en = 1;
}
// オドメトリ受信ループ
#if HAVE_SIGLONGJMP
if( sigsetjmp( ctrlc_capture, 1 ) != 0 )
{
quit = 1;
}
else
#elif HAVE_LONGJMP
if( setjmp( ctrlc_capture ) != 0 )
{
quit = 1;
}
else
#endif
{
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
odometry_receive_loop( );
}
else
{
while( 1 )
yp_usleep( 1000000 );
}
yprintf( OUTPUT_LV_MODULE, "Connection to %s was closed.\n", param->device_name );
}
/* 終了処理 */
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
serial_close( );
}
if( update_thread_en )
{
pthread_cancel( update_thread );
pthread_join( update_thread, NULL );
update_thread_en = 0;
}
if( control_thread_en )
{
pthread_cancel( control_thread );
pthread_join( control_thread, NULL );
control_thread_en = 0;
}
if( command_thread_en )
{
pthread_cancel( command_thread );
pthread_join( command_thread, NULL );
command_thread_en = 0;
}
if( option( OPTION_RECONNECT ) && quit == 0 )
{
init_spur_command( );
yp_usleep( 500000 );
if( !( option( OPTION_WITHOUT_DEVICE ) ) )
{
while( !serial_tryconnect( param->device_name ) )
{
yp_usleep( 200000 );
}
}
yprintf( OUTPUT_LV_MODULE, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
yp_usleep( 500000 );
continue;
}
break;
}
while( 1 );
#ifdef HAVE_SSM
/* SSM終了処理 */
if( !option( OPTION_WITHOUT_SSM ) )
end_ypspurSSM( );
#endif
yp_usleep( 200000 );
fflush( stderr );
return ( quit ? EXIT_SUCCESS : EXIT_FAILURE );
}