static e_int32 inter_ls_scan(laser_sick_t *ls) { e_int32 ret; //提交配置到控制板 // ret = hl_turntable_config(ls->control, ls->speed_h, ls->start_angle_h, // ls->end_angle_h + ls->pre_scan_angle); ret = lm_turntable_config( r2f(ls->speed_h), ls->start_angle_h, ls->end_angle_h + ls->pre_scan_angle); e_assert(ret>0, ret); //开始真正数据采集前的准备工作,快速转动转台到0点 // ret = lm_search_zero(ls->control); // e_assert(ret>0 && should_continue, ret); // Delay(100); //开始真正数据采集前的准备工作,快速转动转台到需要的启始位置 ret = lm_turntable_prepare(ls->pre_scan_angle); e_assert(ret>0 && should_continue, ret); //初始化sick扫描设备 ret = sld_initialize(ls->sick); e_assert(ret>0 && should_continue, ret); ret = sld_set_global_params_and_scan_areas_interlace(ls->sick, ls->speed_v, ls->resolution_v, ls->interlace_v, ls->start_angle_v, ls->end_angle_v, ls->active_sectors.left + ls->active_sectors.right); e_assert(ret>0 && should_continue, ret); /*打印当前扫描参数配置(经优化后的,生成的有效配置)*/ ret = sld_print_sector_config(ls->sick); e_assert(ret>0 && should_continue, ret); //让转台开始正常转动 ret = lm_turntable_start(); e_assert(ret>0 && should_continue, ret); DMSG((STDOUT, "scan job routine starting write routine...\r\n")); //启动sick数据读取线程 ret = createthread("hd_scan_job_write", (thread_func) &write_pool_data_routine, ls, NULL, &ls->thread_write); e_assert(ret>0 && should_continue, ret); ret = resumethread(ls->thread_write); e_assert(ret>0 && should_continue, ret); DMSG((STDOUT, "scan job routine starting read routine...\r\n")); //启动数据回写线程 ret = createthread("hd_scan_job_read", (thread_func) &read_pool_data_routine, ls, NULL, &ls->thread_read); e_assert(ret>0 && should_continue, ret); ret = resumethread(ls->thread_read); e_assert(ret>0 && should_continue, ret); DMSG((STDOUT, "scan job routine start done.\r\n")); return ret; }
int main(int argc, char *argv[]) { e_int32 ret,i; pool_init(&pool); ethread_t* threads[2]; DMSG((STDOUT, "scan job routine starting write routine...\r\n")); ret = createthread("hd_scan_job_write", (thread_func) &write_pool_data_routine, &pool, NULL, &threads[0]); if (ret <= 0) { DMSG((STDOUT, "createhread failed!\r\n")); return E_ERROR; } ret = resumethread(threads[0]); if (ret <= 0) { DMSG((STDOUT, "write resumethread failed!\r\n")); return E_ERROR; } DMSG((STDOUT, "scan job routine starting read routine...\r\n")); ret = createthread("hd_scan_job_read", (thread_func) &read_pool_data_routine, &pool,NULL, &threads[1]); if (ret <= 0) { DMSG((STDOUT, "createhread failed!\r\n")); return E_ERROR; } ret = resumethread(threads[1]); if (ret <= 0) { DMSG((STDOUT, "read resumethread failed!\r\n")); return E_ERROR; } while(count<2) { //DMSG((STDOUT,"COUNT = %d",count)); sleep(1); } for (i=0;i<2;i++) { killthread( threads[i]); } pool_destroy(&pool); return 0; }
int main(int argc, char *argv[]) { e_int32 ret,i; hd_connect_t connect; msg_monitor_t monitor = { 0 }; ethread_t* threads[MAX_CLIENT_SIZE]; e_uint8 name[128]={'t','e','s','t',0}; ret = sc_open_socket(&connect, "127.0.0.1", 6666); e_assert(ret>0, ret); ret = sc_connect(&connect); e_assert(ret>0, ret); ret = mm_init(&monitor, &connect); e_assert(ret>0, ret); mm_start(&monitor); #if 0 fsocket_t* fd = mm_create_socket(&monitor, name); if (!fd) { DMSG((STDOUT,"ERROR CREATE FAKE SOCKET\r\n")); } else{ request_thread(fd); } #else while (count--) { sprintf(name,"Thread[%d]",count); fsocket_t* fd = mm_create_socket(&monitor,name); DMSG((STDOUT,"create request thread %d\r\n",count)); ret = createthread("request", (thread_func) &request_thread, (void*) fd, NULL, &threads[count]); if (ret <= 0) { DMSG((STDOUT,"createhread failed!\r\n")); return E_ERROR; } ret = resumethread(threads[count]); if (ret <= 0) { DMSG((STDOUT,"resumethread failed!\r\n")); return E_ERROR; } } #endif while(count<MAX_CLIENT_SIZE-1) { //DMSG((STDOUT,"COUNT = %d",count)); sleep(1); } for (i=0;i<MAX_CLIENT_SIZE;i++) { killthread( threads[i]); } mm_stop(&monitor); sc_close(&connect); return 0; }
e_int32 lm_init(laser_control_t *lc) { int ret; e_assert(!lm->state, E_ERROR_INVALID_STATUS); lm->state = STATE_PAUSE; lm->is_paused = 0; semaphore_init(&lm->wakeup, 0); lm->lc = lc; DMSG((STDOUT, "laser machine starting main routine...\r\n")); //启动sick数据读取线程 ret = createthread("laser_machine", (thread_func) &main_loop, lm, NULL, &lm->main_thread); e_assert(ret>0, ret); ret = resumethread(lm->main_thread); e_assert(ret>0, ret); return E_OK; }
e_int32 ls_scan(laser_sick_t *ls, char* ptDir, char *grayDir, e_uint32 speed_h_delay, const e_float64 start_angle_h, const e_float64 end_angle_h, e_uint32 speed_v_hz, e_float64 resolution_v, const e_uint32 interlace_v, const e_float64 start_angle_v, const e_float64 end_angle_v) { e_int32 ret; e_assert(ls && ls->state == STATE_IDLE, E_ERROR_INVALID_STATUS); ls->state = STATE_WORK; ls->on_status_change(ls->ctx, ls->state); ret = ls_phrase_config(ls, speed_h_delay, start_angle_h, end_angle_h, speed_v_hz, resolution_v, interlace_v, start_angle_v, end_angle_v); if (ret <= 0) { DMSG((STDOUT, "ls_phrase_config failed!\r\n")); ls->state = STATE_IDLE; return E_ERROR; } //创建输出端子 ls->writer = dm_alloc(ptDir, grayDir, ls->width, ls->height, ls->h_w, ls->active_sectors.right && ls->active_sectors.left ? E_DWRITE : E_WRITE); if (ls->writer == 0) { DMSG((STDOUT, "dm_alloc failed!\r\n")); ls->state = STATE_IDLE; return E_ERROR; } ret = dm_alloc_buffer(ls->writer, DATA_BLOCK_TYPE_COLUMN, &ls->points_polar, &ls->points_gray); if (e_failed( ret )) { DMSG((STDOUT, "dm_alloc_buffer failed!\r\n")); ls->state = STATE_IDLE; return E_ERROR; } ls->slip_idx = 0; ls->slip_tick = 0; DMSG((STDOUT, "scan job routine starting read routine...\r\n")); //check priv if (ls->thread_work) killthread(ls->thread_work); ret = createthread("point scan scan thread", (thread_func) &thread_scan_func, ls, NULL, &ls->thread_work); if (ret <= 0) { DMSG((STDOUT, "create point scan thread failed!\r\n")); ls->state = STATE_IDLE; return E_ERROR; } ret = resumethread(ls->thread_work); if (ret <= 0) { DMSG((STDOUT, "resume point scan thread failed!\r\n")); if (ls->thread_work) killthread(ls->thread_work); ls->state = STATE_IDLE; return E_ERROR; } DMSG((STDOUT, "point scan job routine start successful.\r\n")); return E_OK; }