/**
*\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;
}