void * thread_reading(void *ctmp)
{
set_para(); ////
//Class_GPS *p = (Class_GPS *)ctmp;//Need to get this pointer to use the non-static member
while (1)
{
reading_gps();
// test();
//usleep(p->interval);
//printf("Thread running\n");
usleep(GPS_INTERVAL);
// usleep(400000);
}
return ((void *)0);
}
int camera_init(struct picture_t *out_info)
{
fd_cam = open(CAM_NAME, O_RDWR);
if(fd_cam < 0){
perror("open camera " CAM_NAME);
return 0;
}
if(!set_para())
goto label_close;
//get_std();
//GetStreamParam();
if(!get_format())
goto label_close;
lib = v4lconvert_create(fd_cam);
if(!lib) {
perror("v4lconvert_create");
goto label_close;
}
if(!buf_alloc_mmap())
goto label_free;
YUV420_size = current_pic.width*current_pic.height*3/2;
if(!(current_pic.buffer = malloc(YUV420_size))){
perror("malloc");
goto label_free;
}
*out_info = current_pic;
return 1;
label_free:
free_buf_mmap();
v4lconvert_destroy(lib);
label_close:
close(fd_cam);
return 0;
}
int main(int argc, char **argv)
{
int fd;
int retval;
int ret;
uint8_t tx_buf[1024];
uint8_t rx_buf[1024];
mbus_cmd_info cmd;
mbus_resp_info resp;
if(argc < 2){
printf("%s [PATH]\n", argv[0]);
exit(0);
}
set_para(&cmd);
fd = open(argv[1], O_RDWR);
if(fd < 0){
mb_printf("Open : %s\n", strerror(errno));
return -1;
}
set_termois(fd);
int txlen;
int rlen;
fd_set rfds;
struct timeval tv;
do{
txlen = rtu_build_cmd(tx_buf, 1024, &cmd);
write(fd, tx_buf, txlen);
FD_ZERO(&rfds);
FD_SET(fd, &rfds);
tv.tv_sec = 2;
tv.tv_usec = 0;
rlen = 0;
do{
retval = select(fd+1, &rfds, 0, 0, &tv);
if(retval <= 0){
mb_printf("Select nothing\n");
break;
}
rlen += read(fd, &rx_buf[rlen], sizeof(rx_buf) - rlen);
ret = rtu_get_respinfo(rx_buf, rlen, &resp);
if(ret > 0) break;
if(ret == MBUS_ERROR_DATALEN) continue;
tcflush(fd, TCIOFLUSH);
break;
}while(1);
if(rtu_get_respinfo(rx_buf, rlen, &resp) <= 0){
mb_printf("Failed to get resp info\n");
continue;
}
mb_printf("resp FC = %d\n", resp.info.fc);
if(resp.info.fc & EXCPTIONCODE){
switch(resp.excp.ec){
case EXCPILLGFUNC:
mb_printf("exception: illegal function\n");
break;
case EXCPILLGDATAADDR:
mb_printf("exception: illegal data address\n");
break;
case EXCPILLGDATAVAL:
mb_printf("exception: illegal data value\n");
break;
default:
mb_printf("exception code: %hhx\n", resp.excp.ec);
}
return 0;
}
switch(resp.info.fc){
case READCOILSTATUS:
case READINPUTSTATUS:
case READHOLDINGREGS:
case READINPUTREGS:
for(int i = 0; i < resp.query.bcnt; i++){
printf("[%d] %hhx \n", i, resp.query.data[i]);
}
break;
case PRESETSINGLEREG:
cmd.swrite.data++;
break;
case FORCESINGLECOIL:
cmd.swrite.data = (unsigned short)cmd.swrite.data ? 0x0000:0xff00;
break;
case PRESETMULTIREGS:
case FORCEMULTICOILS:
memset(wdata, ++(wdata[0]), bytecnt);
break;
default:
mb_printf("ERROR: unknown command\n");
return 0;
};
sleep(delay);
}while(1);
if(fd > 0)
close(fd);
return 0;
}