/** *\brief 通过网络接收数据得接收函数。 *\param fs 定义了套接子得相关属性。 *\param recv_buf 定义了接收消息的缓存。 *\param recv_len 定义了接收消息的长度。 *\param timeout_usec 定义消息接收的超时时间。 *\retval E_OK 表示成功。 */ e_int32 fsocket_recv(fsocket_t *fs, e_uint8 *recv_buf, e_uint32 recv_len, e_uint32 timeout_usec) { e_int32 ret; e_uint8 req_id; e_uint8 s_id; int req_iid = -1, s_iid = -1; e_assert(fs&&fs->state, E_ERROR_INVALID_HANDLER); if (timeout_usec <= 0) //没有设置超时,死等 { ret = wait_for_reply_forever(fs); } else { ret = wait_for_reply(fs, timeout_usec); } if (!e_failed(ret)) { //取出消息,和请求号 recv_len = recv_len >= MSG_MAX_LEN ? MSG_MAX_LEN : recv_len; sscanf(fs->buf, "#%02X%02X%[^@]", &s_iid, &req_iid, recv_buf); s_id = s_iid & 0xFF; req_id = req_iid & 0xFF; //TODO:无视过时消息? if (req_id != fs->rq_id) { DMSG( (STDOUT, "fsocket_recv 取到过时消息:ERROR:request id[%u] != fs->req_id[%u]\n",req_id,fs->rq_id)); } return E_OK; } return ret; }
// open/close/state socket device e_int32 Socket_Open(socket_t **socket_ptr, const char *socket_addr, const e_uint32 port, e_int32 type) { int sockfd; int snd_size = 0; /* 发送缓冲区大小 */ socklen_t optlen; /* 选项值长度 */ socket_t *skt = (socket_t *) malloc(sizeof(socket_t)); e_assert(skt, E_ERROR_BAD_ALLOCATE); memset(skt, 0, sizeof(socket_t)); //保证经过网络初始化 Socket_Init(); /*创建服务器端套接字--IPv4协议*/ switch (type) { case E_SOCKET_TCP: /*面向连接通信,TCP协议*/ sockfd = socket(PF_INET, SOCK_STREAM, 0); break; case E_SOCKET_UDP: /*无连接,UDP协议*/ sockfd = socket(PF_INET, SOCK_DGRAM, 0); break; case E_SOCKET_NAME: sockfd = socket(AF_UNIX, SOCK_STREAM, 0); break; } /*check sockfd*/ if (e_failed(sockfd)) { free(skt); return E_ERROR_IO; } /* * 先读取缓冲区设置的情况 * 获得原始发送缓冲区大小 */ optlen = sizeof(snd_size); if (getsockopt(sockfd, SOL_SOCKET, SO_SNDBUF, &snd_size, &optlen) < 0) { DMSG((STDOUT,"获取发送缓冲区大小错误\n")); skt->send_max_size = ~0; } else { // DMSG((STDOUT,"获取发送缓冲区大小:%d\n",snd_size)); skt->send_max_size = snd_size; } //默认是非阻塞通讯 fcntl(sockfd, F_SETFL, fcntl(sockfd, F_GETFL) | O_NONBLOCK); /*存储附加信息*/ skt->priv = (void *) sockfd; if (socket_addr != NULL) hd_strncpy(skt->ip_address, socket_addr, sizeof(skt->ip_address)); skt->port = port; skt->type = type; skt->state = E_OK; skt->last_error = E_OK; (*socket_ptr) = skt; return E_OK; }
/** *\brief 通过网络成功接收数据的接收函数。 *\param fs 定义了套接子得相关属性。 *\param success_reply 定义了成功接收数据的缓存。 *\param recv_len 定义了接收消息的长度。 *\param timeout_usec 定义消息接收的超时时间。 *\retval E_OK 表示成功。 */ e_int32 fsocket_recv_success(fsocket_t *fs, e_uint8 *success_reply, e_uint32 rlen, e_uint32 timeout_usec) { e_int32 ret; e_uint8 req_id; e_uint8 s_id; e_uint8 buf[MSG_MAX_LEN]; int req_iid = -1, s_iid = -1; e_assert(fs&&fs->state, E_ERROR_INVALID_HANDLER); if (timeout_usec <= 0) //没有设置超时,死等 { ret = wait_for_reply_forever(fs); } else { ret = wait_for_reply(fs, timeout_usec); } if (!e_failed(ret)) { //取出消息,和请求号 rlen = rlen >= MSG_MAX_LEN ? MSG_MAX_LEN : rlen; sscanf(fs->buf, "#%02X%02X%[^@]", &s_iid, &req_iid, buf); s_id = s_iid & 0xFF; req_id = req_iid & 0xFF; //TODO:无视过时消息? if (req_id < fs->rq_id) { DMSG( (STDOUT, "fsocket_recv_success 取到过时消息:ERROR:request id[%u] != fs->req_id[%u]\n",req_id,fs->rq_id)); return E_ERROR; } else if (req_id > fs->rq_id) { DMSG((STDOUT,"出现消息号异常,请检查!\n")); while (1) ; } if (!strncmp(buf, success_reply, rlen)) { return E_OK; } } return E_ERROR; }
data_manager_t* dm_alloc(char **files, int num, int width, int height, int mode) { int i, ret; data_adapter_t* pda; data_manager_t*dm = malloc(sizeof(data_manager_t) + sizeof(data_adapter_t) * num); e_assert(dm, E_ERROR_BAD_ALLOCATE); pda = dm->adapters; dm->num = 0; for (i = 0; i < num; i++) { ret = da_open(pda, files[i], width, height, mode); if (e_failed(ret)) { continue; } dm->num++; pda++; } if (dm->num > 0) dm->state = 1; return dm; }
int on_command(const char *cmd, char *ret_buf, int *ctx, int log_printf(char *buf, int *ctx, int code, char *fmt, ...)) { int ret = 1; //command select signal(SIGINT, sig_handler); if (!strncmp(cmd, "get", 3)) { const char *scmd = get_sub_command(cmd); if (!strcmp(scmd, "author")) { log_printf(ret_buf, ctx, ret, "uplusplus"); } else if (!strcmp(scmd, "version")) { log_printf(ret_buf, ctx, ret, "1.0.0"); } else { log_printf(ret_buf, ctx, 0, "unknown command: <b>%s</b>", cmd); ret = 0; goto end; } } else if (!strncmp(cmd, "connect", sizeof("connect"))) { if (job != NULL) { log_printf(ret_buf, ctx, 0, "Laser already connect.."); return 0; } log_printf(ret_buf, ctx, ret, "Laser try create.."); ret = sj_create(&job, NULL, NULL, NULL, NULL); if (e_failed(ret)) { log_printf(ret_buf, ctx, ret, "Laser scan create failed."); } else { log_printf(ret_buf, ctx, ret, "Laser create successfull."); } } else if (!strncmp(cmd, "disconnect", sizeof("disconnect"))) { if (job == NULL) { log_printf(ret_buf, ctx, 0, "Laser already disconnect.."); return 0; } log_printf(ret_buf, ctx, ret, "Laser try disconnect.."); ret = sj_destroy(job); if (e_failed(ret)) { log_printf(ret_buf, ctx, ret, "Laser scan disconnect failed."); } else { log_printf(ret_buf, ctx, ret, "Laser disconnect successfull."); } job = NULL; } else if (!strncmp(cmd, "config", sizeof("config"))) { log_printf(ret_buf, ctx, ret, "Laser scan try config.."); sj_set_data_dir(job, "/sdcard/ls300/data/point_cloud", "/sdcard/ls300/data/image"); ret = sj_config(job, 100, 0, 360, 5, 0.25, -45, 90); if (e_failed(ret)) { log_printf(ret_buf, ctx, ret, "Laser scan config failed.device busy."); } else { log_printf(ret_buf, ctx, ret, "Laser scan config successfull."); } } else if (!strncmp(cmd, "pointscan", sizeof("pointscan"))) { log_printf(ret_buf, ctx, ret, "Laser scan try start.."); ret = sj_scan_point(job); if (e_failed(ret)) { log_printf(ret_buf, ctx, ret, "Laser scan start scan point cloud failed.device busy."); } } else if (!strncmp(cmd, "photoscan", sizeof("photoscan"))) { log_printf(ret_buf, ctx, ret, "Laser photo scan try start.."); ret = sj_scan_photo(job); if (e_failed(ret)) { log_printf(ret_buf, ctx, ret, "Laser scan start scan photo failed.device busy."); } } else if (!strncmp(cmd, "cancel", sizeof("cancel"))) { log_printf(ret_buf, ctx, ret, "Laser scan try stop.."); ret = sj_cancel(job); if (e_failed(ret)) { log_printf(ret_buf, ctx, ret, "Laser scan stop failed."); } } else { log_printf(ret_buf, ctx, 0, "unknown command: <b>%s</b>", cmd); ret = 0; goto end; } end: return ret; }
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; }
/** *\brief 网络连接发送请求函数。 *\param fs 定义了套接子得相关属性。 *\param msg 定义了发送请求消息。 *\param mlen 定义了发送请求消息长度。 *\param recv_buf 定义了接收缓存。 *\param recv_len 定义了接收消息长度。 *\param timeout_usec 定义了接收消息超时时间。 *\retval E_OK 表示成功。 */ e_int32 fsocket_request(fsocket_t *fs, e_uint8 *msg, e_uint32 mlen, e_uint8 *recv_buf, e_uint32 recv_len, e_uint32 timeout_usec) { e_int32 ret; e_uint8 req_id; e_uint8 s_id; int req_iid = -1, s_iid = -1; /* Timeval structs for handling timeouts */ e_uint32 beg_time, elapsed_time; e_assert(fs&&fs->state, E_ERROR_INVALID_HANDLER); // DMSG((STDOUT, "FAKE SOCKE [%s:%u] try request,current rq_id=%u...\r\n", fs->name, (unsigned int) fs->id,(unsigned int) fs->rq_id)); //请求发送锁 ret = semaphore_timeoutwait(&fs->send_sem, timeout_usec); e_assert(ret, E_ERROR_LOCK_FAILED); /* Acquire the elapsed time since epoch */ beg_time = GetTickCount(); //发送请求 ret = send_one_msg(fs, msg, mlen, timeout_usec); if (e_failed(ret)) goto END; //等待回复 elapsed_time = GetTickCount() - beg_time; while (timeout_usec <= 0 || (elapsed_time = GetTickCount() - beg_time) < timeout_usec) { if (timeout_usec <= 0) //没有设置超时,死等 { ret = wait_for_reply_forever(fs); } else { ret = wait_for_reply(fs, timeout_usec - elapsed_time); } if (!e_failed(ret)) { //取出消息,和请求号 recv_len = recv_len >= MSG_MAX_LEN ? MSG_MAX_LEN : recv_len; sscanf(fs->buf, "#%02X%02X%[^@]", &s_iid, &req_iid, recv_buf); s_id = s_iid & 0xFF; req_id = req_iid & 0xFF; //TODO:无视过时消息? if (req_id < fs->rq_id) { DMSG((STDOUT, "FAKE SOCKE [%s:%u:%u] 取到过时消息:id=%u \n忽略,继续等待下一个消息\n", fs->name, (unsigned int) fs->id, (unsigned int) fs->rq_id, (unsigned int) req_id)); continue; } else if (req_id > fs->rq_id) { // DMSG((STDOUT,"出现消息号异常,请检查!\n")); // while (1) // ; } break; } break; } END: //处理完成,提醒可以发下一个请求了 ret = semaphore_post(&fs->send_sem); e_assert(ret, E_ERROR_TIME_OUT); // DMSG( // (STDOUT, "[%s_%u_%u]FAKE SOCKET release send sem...\r\n", fs->name, (unsigned int)fs->id,(unsigned int)fs->rq_id)); elapsed_time = GetTickCount() - beg_time; if (MSG_LEVEL_VERBOSE) DMSG((STDOUT, "FAKE SOCKET [%s:%u:%u] request done in %u Ms...\r\n", fs->name, (unsigned int) fs->id, req_id, (int) (elapsed_time / 1000))); return E_OK; }