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;
}
