/*void CUartThread::ReadUart()
{
int nLen = 0;
char szBuf[RECV_DATA_LEN] = {0};
int time = 1000;
nLen = readport(m_nFD, szBuf, RECV_CMD_LEN);
if(nLen <= 0)
{
printf("recv data return \r\n");
return;
}
if((szBuf[0] == 'O') && (szBuf[1] == 'K'))
{
int nDatalen = 0;
if(nLen >= RECV_CMD_LEN)
{
nDatalen = szBuf[3]*256 + szBuf[4] - 5 + 2;
//printf("nDatalen=%d \r\n", nDatalen);
}
nLen = readport(m_nFD, szBuf, nDatalen);
// printf("recv data len=%d \r\n", nLen);
if(nLen <= 0)
{
return;
}
printf("remit sSerialData(szBuf) \r\n");
// put a signals that recv data from serial port
emit sSerialData(szBuf);
}
if((szBuf[0] == 'E') && (szBuf[1] == 'R'))
{
nLen = readport(m_nFD, szBuf, 2);
printf("recv data error \r\n");
}
}*/
void CUartThread::ReadUart()
{
printf("readuart");
int nLen = 0;
char szBuf[RECV_DATA_LEN] = {0};
nLen = readport(m_nFD, szBuf, 5);
if(nLen < 0)
{
printf("readuart < 0");
CLOG::Log("Read sensor cmd error .");
return;
}
if((szBuf[0] == 'O') && (szBuf[1] == 'K'))
{
printf("readuart ok");
int nDatalen = 0;
if(nLen >= RECV_CMD_LEN)
{
nDatalen = szBuf[3]*256 + szBuf[4] - 5 + 2;
}
nLen = readport(m_nFD, szBuf, nDatalen);
if(nLen <= 0)
{
printf("nLen <= 0,Read sensor data error .\r\n");
CLOG::Log("Read sensor data error .");
return;
}
// for(int i = 0; i < nLen; i++)
// {
// printf("%x\r\n", *(szBuf + i));
// }
printf("Read uart data success \r\n");
CLOG::Log("Read uart data success .");
//put a signals that recv data from serial port
emit sSerialData(szBuf);
}
if((szBuf[0] == 'E') && (szBuf[1] == 'R'))
{
nLen = readport(m_nFD, szBuf, 2);
//printf("recv data error \r\n");
}
}
//-------------------------------------------------------------------------
// read a byte from the embedded controller (EC) via port io
//-------------------------------------------------------------------------
int ReadByteFromEC(int offset, byte *pdata)
{
int ok;
// wait for IBF and OBF to clear
ok = waitportstatus(EC_STAT_IBF|EC_STAT_OBF, 0);
if (!ok) return false;
// tell 'em we want to "READ"
ok = writeport(EC_CTRLPORT, EC_CTRLPORT_READ);
if (!ok) return false;
// wait for IBF to clear (command byte removed from EC's input queue)
ok = waitportstatus(EC_STAT_IBF, 0);
if (!ok) return false;
// tell 'em where we want to read from
ok = writeport(EC_DATAPORT, offset);
if (!ok) return false;
// wait for IBF to clear (address byte removed from EC's input queue)
// Note: Techically we should waitportstatus(IBF|OBF,OBF) here. (a byte
// being in the EC's output buffer being ready to read). For some reason
// this never seems to happen
ok = waitportstatus(EC_STAT_IBF, 0);
if (!ok) return false;
// read result (EC byte at offset)
byte data;
ok = readport(EC_DATAPORT, &data);
if (ok) *pdata= data;
return ok;
}
int volume()
{
int ret, vol;
char cmd[MAXLEN];
char buf[MAXLEN]="";
vol=getvol();
if(vol==-1) return -1;
sprintf(cmd, "AT*EAID=4,1,\"Volume\",10,%d\r", (int) ((vol/10.0)+.5));
writeport(cmd, strlen(cmd));
if(waitfor("OK", 1, TIMEOUT) != 1) return -1;
while(1)
{
while((ret = readport(buf, MAXLEN, TIMEOUT)) == -EINTR);
if(ret <= 0) return -1;
if(strncmp(buf, "*EAII: 15,",10)==0) setvol(atoi(buf+10));
if(strncmp(buf, "*EAII: 0", 8)==0 || strncmp(buf, "*EAAI", 5)==0)
{
setvol(vol);
return 0;
}
if(strncmp(buf, "*EAII: 4,",9)==0)
{
setvol(atoi(buf+9));
return 1;
}
}
}
int submenu(int def)
{
int i, ret;
char cmd[MAXLEN] = "AT*EASM=\"PC Remote\",1,";
char buf[MAXLEN]="";
if(def<=0 || def > conf->menucnt) def=1;
sprintf(buf,"%d,%d", def, conf->menucnt);
strcat(cmd, buf);
for(i=0; i<conf->menucnt; i++)
{
strcat(cmd,",\"");
strcat(cmd,conf->menu[i].text);
strcat(cmd,"\"");
}
strcat(cmd,"\r");
writeport(cmd, strlen(cmd));
if(waitfor("OK", 1, TIMEOUT) != 1) return -1;
while((ret = readport(buf, MAXLEN, TIMEOUT)) == -EINTR);
if(ret <= 0) return -1;
if(strncmp(buf, "*EAMI: ", 7)!=0) return -1;
return(atoi(buf+7));
}
int keyboard()
{
int ret=-1, go=1;
char cmd1[]= "AT*EAID=1,1,\"Press the keys to simulate key presses on the computer\"\r";
char cmd2[]= "AT+CMER=3,2,,,\r";
char cmd3[]= "AT+CMER=0,0,,,\r";
char buf[MAXLEN]="", *key;
int status, keycnt;
Display *dpy = XOpenDisplay (NULL);
if(dpy==NULL) return -1;
writeport(cmd1, strlen(cmd1));
if(waitfor("OK", 1, TIMEOUT) != 1) go = 0;
writeport(cmd2, strlen(cmd2));
if(waitfor("OK", 1, TIMEOUT) != 1) go = 0;
while(go)
{
buf[0]=0;
ret = readport(buf, MAXLEN, TIMEOUT);
if( (ret <= 0 && ret != -EINTR) || strncmp(buf, "ERROR", 5)==0 )
{
ret=-1;
break;
}
if(strncmp(buf, "*EAII: ", 7)==0 || strncmp(buf, "*EAAI", 5)==0)
{
ret=1;
break;
}
if(strncmp(buf, "+CKEV: ", 7)==0)
{
status=strchr(buf,',')[1]-'0';
strchr(buf,',')[0]=0;
key=buf+7;
/* Correct for bug in T610 */
if(key[0]==0x1B && key[1]==0x14)
{
key[0]='^';
key[1]=0;
}
keycnt=0;
while(keycnt < conf->keymapcnt && strcmp(conf->keymap[keycnt].name,key)) keycnt++;
if(keycnt < conf->keymapcnt)
key_down_up(dpy, conf->keymap[keycnt].val, status);
}
}
writeport(cmd3, strlen(cmd3));
if(waitfor("OK", 5, TIMEOUT) != 1) ret = -1;
XCloseDisplay (dpy);
return ret;
}
static long
rtcread(Chan *c, void *buf, long n, vlong off)
{
ulong offset = off;
ulong t;
char *b;
if(c->qid.type & QTDIR)
return devdirread(c, buf, n, rtcdir, nrtc, devgen);
switch((ulong)c->qid.path){
case Qrtc:
t = m->iomem->rtc;
n = readnum(offset, buf, n, t, 12);
return n;
case Qswitch:
return readnum(offset, buf, n, archoptionsw(), 12);
case Qintstat:
b = malloc(2048);
if(waserror()){
free(b);
nexterror();
}
intrstats(b, 2048);
t = readstr(offset, buf, n, b);
poperror();
free(b);
return t;
case Qporta:
case Qportb:
case Qportc:
return readport(c->qid.path, offset, buf, n);
case Qnvram:
if(offset < 0 || offset >= conf.nvramsize)
return 0;
if(offset + n > conf.nvramsize)
n = conf.nvramsize - offset;
memmove(buf, (char*)conf.nvrambase+offset, n);
return n;
}
error(Egreg);
return 0; /* not reached */
}
void WriteThread::ReadUart()
{
int wLen = 0;
char szBuf[8] = {0};
wLen = readport(wFD, szBuf, 8);
if(wLen < 0)
{
CLOG::Log("Read write_serial cmd error .");
return;
}
else if((szBuf[0] == 'O') && (szBuf[1] == 'K'))
{
//设置写串口数据标志位为true
writeFlag = true;
printf("Read OK \r\n");
CLOG::Log("Read OK .");
}
}
void* tf_read(void *args) {
int oldstate;
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, &oldstate);
setup_termination();
void *load;
int nRead;
uint8_ buf[3];
uint8_ *pkg;
int len;
int iid;
int id;
int sender;
struct payload_pkg_rob_command *pkg_rob_comm;
struct payload_pkg_trajectory *pach;
xbee_packet_received = 0;
xbee_ack_received = 0;
while (1) {
printf("------------\n");
iid = -1;
/* Look for a command */
/* Lock the mutex on the job queue. */
nRead = readport(xbee_fd_r, buf, 3);
if (nRead == 3) {
if (buf[0] == 0x7e) {
printf("%02x\t%02x\n", buf[1], buf[2]);
len = MSB_LSB_2_INT(buf + 1, 2);
if (len > 0) {
printf("Len pkg: %d\t", len);
pkg = (uint8_ *) malloc(sizeof (uint8_)*(len + 1));
nRead = readport(xbee_fd_r, pkg, len + 1);
sender = pkg[2];
printf("Sender: %2x\t%d\n", pkg[2], sender);
++xbee_packet_neighborhood[sender];
printf("Read pkg: %d\n", nRead);
id = pkg_dispatcher(pkg, nRead, &iid, &load);
if (id == rx_pack_16 && iid == pkg_1) {
printf("---PARSER----\n");
printf("Long: %ld\n", ((struct payload_pkg1 *) load)->i);
printf("Float: %f\n", ((struct payload_pkg1 *) load)->j);
}
xbee_packet_received++;
switch (iid) {
case pkg_state:
//printf("pkg_state\n");
break;
case pkg_trajectory:
pach=(struct payload_pkg_trajectory*)load;
v_xbee=pach->v;
w_xbee=pach->w;
fprintf(stderr,"VELOCITA' RICEVUTE DA MATLAB: line=%f ang=%f\n",pach->v,pach->w);
break;
case pkg_webcam_data:
printf("pkg_webcam_data\n");
break;
case pkg_rob_command:
printf("pkg_rob_command\n");
pkg_rob_comm = (struct payload_pkg_rob_command *) load;
console_current_command = pkg_rob_comm->command[0];
console_current_value1 = pkg_rob_comm->value;
switch (console_current_command) {
case 'v':
printf("Command v\tValue: %d\n", console_current_value1);
set_vel_2_array(pic_buffer[pic_last_vel_2_send], console_current_value1, console_current_value1);
pic_last_vel_2_send %= LEN_PIC_BUFFER;
break;
case 'p':
printf("Command p\tValue: %d\n", console_current_value1);
break;
case 'd':
printf("Command d\tValue: %d\n", console_current_value1);
break;
case 't':
printf("Command d\tValue: %d\n", console_current_value1);
break;
case 's':
printf("Command s\tValue: %d\n", console_current_value1);
break;
default:
break;
}
break;
default:
break;
}
free(pkg);
}
printf("Len: %d\n", len);
}
#ifdef VERBOSE_XBEE
printf("nRead(in): %d\t", nRead);
print_pkg(buf, nRead);
#endif
}
#ifdef VERBOSE_XBEE
printf("nRead(out): %d\t", nRead);
print_pkg(buf, nRead);
#endif
}
pthread_exit(NULL);
}
int Manual(int argc, unsigned char **argv)
{
/* Serial port initialization */
fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1) {
perror("open_port: Unable to open /dev/ttyS0 - ");
return 1;
} else {
fcntl(fd, F_SETFL, 0);
}
getbaud(fd);
initport(fd);
int PVi, k;
K= uatof(Gain);
/* Things will be drawn more quickly if you always acquire the screen before
trying to draw onto it. */
acquire_screen();
Graphics(); /* Call to graphics from a different function to avoid messy code */
RLE_SPRITE *rle;
sCmd[0]= 'C';
writeport(fd, sCmd);
PVi= 0; k= 100; M=0;
while (!key[KEY_ESC]) /* Only pressing ESC can Exit */
{
char ValveState;
if(ValveState=='O')
{
sCmd[0]= 'A';
writeport(fd, sCmd);
}
if(ValveState=='C')
{
sCmd[0]= 'C';
writeport(fd, sCmd);
}
M++;
/* Check if user needs some help */
if(key[KEY_F1])
Help();
k++;
i= 897;
Captura = create_sub_bitmap(screen, 72, 350, 898, 368);
rle = get_rle_sprite(Captura);
destroy_bitmap(Captura);
draw_rle_sprite(screen, rle, 71, 350);
destroy_rle_sprite(rle);
/* This line reads data from the interfase which is
the process variable(measured variable) of the system */
fcntl(fd, F_SETFL, FNDELAY); // don't block serial read
readport(fd,sResult);
PVi= (int) *sResult;
PV= PVi;
/*
if(PVi<=40)
{
PV= 51;
system("festival --tts Messages/Disconnected&");
blit(Disconnected, screen, 0, 0, 70, 290, 887, 52);
}
*/
if(PV<=48)
PVi= 51;
PV= 1.794117647*(PVi-51);
SP= PV;
if(key[KEY_RIGHT])
{
//fd = close("/dev/parport0");
Simulator();
}
if(key[KEY_PGUP])
OP= (OP+46);
if(key[KEY_PGDN])
OP= (OP-46);
if(key[KEY_UP])
{
OP=162; //(OP+3.66);
sCmd[0]= 'A';
writeport(fd, sCmd);
ValveState='O';
}
if(key[KEY_DOWN])
{
if(OP>=1)
OP= 0;//(OP-3.66);
sCmd[0]= 'C';
writeport(fd, sCmd);
ValveState='C';
}
if(key[KEY_PRTSCR])
{
Captura = create_sub_bitmap(screen, 0, 0, 1024, 768);
save_bitmap("images/User/Captura.pcx", Captura, pal);
destroy_bitmap(Captura);
}
Timer++;
if(OP<=0)
OP= 0;
// if (PV>=40)
// {
textprintf_ex(screen, font, 230, 297, BLACK, WHITE, "%3.1f", (PV/368)*100); // Medición
textprintf_ex(screen, font, 450, 297, BLACK, WHITE, "%3.1f", (SP/368)*100); // SP
textprintf_ex(screen, font, 710, 297, BLACK, WHITE, "%3.1f", (OP/368)*100); // Controlador
// }
if(k>=100)
{
k= 0;
vline(screen, 967, 351, 717, GRAY);
blit(Clean, screen, 0, 0, 968, 350, 2, 368);
}
int Recorder;
Recorder++;
if(Recorder>=900)
{
Recorder= 0;
Captura = create_sub_bitmap(screen, 258, 350, 715, 368);
}
Captura = create_sub_bitmap(screen, 248, 350, 705, 368);
if(PV>=362) PV= 365;
if(OP>=367) OP= 365;
if(PV<=0) PV= 0;
if(OP<=0)
OP= 1;
/*
OPi= fixtoi(itofix((OP*0.69234783)/255*100));
sCmd[0]= (unsigned char)OPi+50;
sCmd[0]= sCmd[0]+0.00;
writeport(fd, sCmd);
*/
// textprintf_ex(screen, font, 30, 297, BLACK, WHITE, "%i - %s - %3.1f", PVi, sResult, PV); // Medición
/* Draw the behaviour of the PV, SP and OP over time */
line(screen, 71+i, 717-PV, 71+i, 717-PVj, RED);
PVj= PV; /* Flag for line y2 as a precedent state */
line(screen, 71+i, 717-OP, 71+i, 717-OPj, BLUE);
OPj= OP; /* Flag for line y2 as a precedent state */
fprintf(outfile,"%i\t%f\t %f\t %f\n", M, ((PV/368)*100), ((SP/368)*100), ((OP/368)*100));
rest(Delay);
}
int ScreenWide;
RLE_SPRITE *rle0, *rle1;
BITMAP *Screen3;
Screen3 = load_bitmap("images/Close/Base.pcx", pal);
system("mp3blaster /home/mentesuprema/Residencia/sounds/swing2.wav &");
rest(5);
rle0= get_rle_sprite(Screen2);
rle1= get_rle_sprite(Screen3);
for(ScreenWide=0;ScreenWide<=768;ScreenWide=ScreenWide+5)
{
draw_rle_sprite(screen, rle0, 0, 768);
draw_rle_sprite(screen, rle1, 0, -768+ScreenWide);
}
destroy_rle_sprite(rle0);
destroy_rle_sprite(rle1);
destroy_bitmap(Screen2);
destroy_bitmap(Screen3);
destroy_bitmap(Clean);
destroy_bitmap(Disconnected);
release_screen();
Close();
exit(0);
}
int mouse()
{
int ret=-1, go=1;
char cmd1[]= "AT*EAID=1,1,\"Use joystick to move mouse & keypad for clicks\"\r";
char cmd2[]= "AT+CMER=3,2,,,\r";
char cmd3[]= "AT+CMER=0,0,,,\r";
char buf[MAXLEN]="";
char key;
int status, mdelta=15, mwait=MOUSE_DELAY;
int mup=0, mdown=0, mleft=0, mright=0;
int mbleft=0, mbright=0, mbmiddle=0, mtoggle=0;
Display *dpy = XOpenDisplay (NULL);
if(dpy==NULL) return -1;
writeport(cmd1, strlen(cmd1));
if(waitfor("OK", 1, TIMEOUT) != 1) go = 0;
writeport(cmd2, strlen(cmd2));
if(waitfor("OK", 1, TIMEOUT) != 1) go = 0;
while(go)
{
buf[0]=0;
ret = readport(buf, MAXLEN, mwait);
if( (ret <= 0 && ret != -EINTR) || strncmp(buf, "ERROR", 5)==0 )
{
ret=-1;
break;
}
if(strncmp(buf, "*EAII: ", 7)==0 || strncmp(buf, "*EAAI", 5)==0)
{
ret=1;
break;
}
if(strncmp(buf, "+CKEV: ", 7)==0)
{
key=buf[7];
status=strchr(buf,',')[1]-'0';
switch(key)
{
//Simply assume it's joy up coz T610 key report is buggy
//case 27:
// K700i is not buggy!
case '^':
case '5':
mup=status;
mwait=MOUSE_DELAY;
break;
case 'v':
case '8':
mdown=status;
mwait=MOUSE_DELAY;
break;
case '<':
case '4':
case '7':
mleft=status;
mwait=MOUSE_DELAY;
break;
case '>':
case '6':
case '9':
mright=status;
mwait=MOUSE_DELAY;
break;
case '1':
if(mtoggle && status)
{
mbleft = !mbleft;
mouse_down_up(dpy, 1, mbleft);
}
if(!mtoggle) mouse_down_up(dpy, 1, status);
break;
case '2':
if(mtoggle && status)
{
mbmiddle = !mbmiddle;
mouse_down_up(dpy, 2, mbmiddle);
}
if(!mtoggle) mouse_down_up(dpy, 2, status);
break;
case '3':
if(mtoggle && status)
{
mbright = !mbright;
mouse_down_up(dpy, 3, mbright);
}
if(!mtoggle) mouse_down_up(dpy, 3, status);
break;
case '*':
if(status && !mtoggle) mouse_double_click(dpy);
break;
case '#':
if(status)
{
mtoggle = !mtoggle;
mbleft = mbright = mbmiddle = 0;
}
break;
}
if((key=='1' || key=='2' || key =='3') && !status
&& mtoggle && !mbleft && !mbmiddle && !mbright) mtoggle=0;
}
if(ret == -EINTR) mwait=MOUSE_RRATE;
if(mup) mouse_rel_move(dpy, 0, -mdelta);
if(mdown) mouse_rel_move(dpy, 0, mdelta);
if(mright) mouse_rel_move(dpy, mdelta, 0);
if(mleft) mouse_rel_move(dpy, -mdelta, 0);
}
writeport(cmd3, strlen(cmd3));
if(waitfor("OK", 5, TIMEOUT) != 1) ret = -1;
XCloseDisplay (dpy);
return ret;
}
