int RMessagePassing::begin(RMP_Node &n, RMP_Node &nm) { int ret; // set node to normal node m_isMaster = 0; // insert to node map RMP_Node *pn = new RMP_Node, *pnm = new RMP_Node; *pn = n; *pnm = nm; pn->setNodeInfo(); pn->mpMe = this; pn->mpObj = this; pnm->mpMe = this; pnm->isMaster = 1; m_nodeMe = pn; m_nodeMaster = pnm; m_nodeMe->print(); m_nodeMaster->print(); m_nodeMap.setMP(this); m_nodeMap.insert(pn, 0); m_nodeMap.insert(pnm, 0); // start server ret = m_socket.startServer(pn->inet4_port); if( ret != 0 ) { dbg_pe("Can not start socket_server: %s (%s)\n", n.nodeName.c_str(), n.hostName.c_str()); return -1; } // start receiving thread start(); // send my node information to master node RMessage msg; msg.msgType = 0xFFFFFFF0; pn->toStream(msg.data); if( pnm->sendMsg(&msg) < 0 ) { dbg_pe("Please start master node first!\n"); return -2; } return 0; }
int UART::write(void *d, int len, int master) { UART_inner_data *pd; BOOL bRes; unsigned long iByteWritten; // handle structure pd = (UART_inner_data *) data; if( pd->m_hCom != NULL ) { // wirte data bRes = WriteFile( pd->m_hCom, /* Handle to file */ d, /* Data buffer */ len, /* Number of bytes to write */ &iByteWritten, /* Number of bytes written */ NULL /* overlapped buffer */ ); if( !bRes ) { dbg_pe("UART::write Send data error, errorcode: %d", GetLastError()); return 2; } if( iByteWritten < len ) { dbg_pw("UART::write Send data byte leng is not correct!"); return 3; } } else { return 1; } return 0; }
int UART::read(void *d, int len, int master) { UART_inner_data *pd; int r; pd = (UART_inner_data *) data; if( pd->fd < 0 ) { dbg_pe("UART port not opened yet!\n"); return -1; } if( m_options & UART_BUFFER ) { r = pd->getReadBuff((uint8_t*)d, len); if( r < 1 ) { int l; uint8_t buf[m_bufSize]; l = _read(buf, m_bufSize); pd->putReadBuf(buf, l); } } else { r = _read(d, len); } return r; }
/******************************************************************************\ * 1-D Spline interpolation * * Prameters: * xa [in] x coordinate * ya [in] y values * y2a [in] second derivative * n [in] data lenght * x [in] sampling point x coordinate * y [out] output value at x \******************************************************************************/ void splint_s(double *_xa, double *_ya, double *_y2a, int n, double x, double *y) { double *xa, *ya, *y2a; int klo, khi, k; double h, b, a; xa = _xa - 1; ya = _ya - 1; y2a = _y2a - 1; klo=1; khi=n; while (khi-klo > 1) { k=(khi+klo) >> 1; if (xa[k] > x) khi=k; else klo=k; } h=xa[khi]-xa[klo]; if (h == 0.0) { dbg_pe("Bad xa input to routine splint"); exit(1); } a=(xa[khi]-x)/h; b=(x-xa[klo])/h; *y= a*ya[klo] + b*ya[khi] + ((a*a*a-a)*y2a[klo] + (b*b*b-b)*y2a[khi])*(h*h)/6.0; }
int RDateTime::fromStream(RDataStream &s, int isSingleDS) { ri32 dt; ru32 d_magic, d_ver; if( isSingleDS ) { s.seek(0); // get magic number & version s.getHeader(d_magic, d_ver); if( d_magic != 0xEABF ) { dbg_pe("Magic number error! %x\n", d_magic); return -1; } } // read date/time if( 0 != s.read(year) ) return -2; if( 0 != s.read(month) ) return -2; if( 0 != s.read(day) ) return -2; if( 0 != s.read(hour) ) return -2; if( 0 != s.read(min) ) return -2; if( 0 != s.read(sec) ) return -2; if( 0 != s.read(nano_sec) ) return -2; if( 0 != s.read(timeZone) ) return -2; // read date/time type if( 0 != s.read(dt) ) return -2; dt_type = (DateTime_Type) dt; return 0; }
int RGBDVideoData::fromStream(pi::RDataStream &ds) { ru32 d_magic, d_ver; int w, h, c, n; ru8 *pix = NULL; // rewind to begining ds.rewind(); // get magic & version number ds.getHeader(d_magic, d_ver); if( d_magic != 0x83F8 ) { dbg_pe("Input data magic number error! %x\n", d_magic); return -1; } // read data if( 0 != ds.read(w) ) return -2; if( 0 != ds.read(h) ) return -2; if( 0 != ds.read(c) ) return -2; if( 0 != ds.read(timestamp) ) return -2; if( 0 != ds.read((ru8*)&asso,sizeof(AssociateData))) return -2; n = w*h*c; if ( n == 0 ) return -3; if( c == 1 ) { img.create(h, w, CV_8UC1); } else if( c == 3 ) { img.create(h, w, CV_8UC3); } else { dbg_pe("image channel not support! c:%d\n", c); return -4; } pix = img.data; if( 0 != ds.read(pix, n) ) return -2; return 0; }
int UART::_write(void *d, int len) { UART_inner_data *pd; int r; pd = (UART_inner_data *) data; if( pd->fd >= 0 ) { r = ::write(pd->fd, d, len); return r; } else { dbg_pe("UART port not opened yet!\n"); return -1; } }
UART::~UART() { UART_inner_data *pd; int r; pd = (UART_inner_data *) data; if( pd->m_hCom != NULL ) { r = CloseHandle(pd->m_hCom); if( !r ) dbg_pe("UART::~UART Close port error!"); pd->m_hCom = NULL; } delete pd; data = NULL; }
UART::~UART() { UART_inner_data *pd; int r; pd = (UART_inner_data *) data; if( pd->fd >= 0 ) { r = ::close(pd->fd); if( r == -1 ) { dbg_pe("close port error!\n"); } } delete pd; data = NULL; }
int UART::close(void) { UART_inner_data *pd; int r; pd = (UART_inner_data *) data; if( pd->fd >= 0 ) { r = ::close(pd->fd); if( r == -1 ) { dbg_pe("close port error!\n"); } pd->fd = -1; } return r; }
int RMessagePassing::sendString(std::string nn, std::string msg, bool isCommand) { RMP_Node *n; n = m_nodeMap.get(nn); if( NULL == n ) { dbg_pe("Can not find node: %s\n", nn.c_str()); return -1; } RMessage rmsg; if( isCommand ) rmsg.msgType = 0xFFFFFFF2; else rmsg.msgType = 1; rmsg.msgID = 1; rmsg.data.write(msg); return n->sendMsg(&rmsg); }
int UART::close(void) { UART_inner_data *pd; int iRes; // handle structure pd = (UART_inner_data *) data; if( pd->m_hCom != NULL ) { iRes = CloseHandle(pd->m_hCom); if( !iRes ) { dbg_pe("UART::close Close COM port error!"); return 2; } pd->m_hCom = NULL; } else { return 1; } return 0; }
int RMessagePassing::begin(RMP_Node &n) { int ret; // set self node to master node m_isMaster = 1; RMP_Node *pn = new RMP_Node; *pn = n; pn->isMaster = 1; pn->setNodeInfo(); pn->mpObj = this; pn->mpMe = this; m_nodeMe = pn; m_nodeMaster = pn; pn->print(); // insert to node map m_nodeMap.setMP(this); m_nodeMap.insert(pn, 0); // start server ret = m_socket.startServer(pn->inet4_port); if( ret != 0 ) { dbg_pe("Can not start socket_server: %s (%s)\n", n.nodeName.c_str(), n.hostName.c_str()); return -1; } // start receiving thread start(); return 0; }
int UART::read(void *d, int len, int master) { UART_inner_data *pd; BOOL bRes; int iByteRead = len; unsigned long byte_read_act; // handle structure pd = (UART_inner_data *) data; // read data if( pd->m_hCom != NULL ) { bRes = ReadFile( pd->m_hCom, /* Handle to file */ d, /* Data buffer */ iByteRead, /* Number of byte to read */ &byte_read_act, /* Number of bytes readed */ NULL ); if( !bRes ) { dbg_pe("UART::read Read data error, errorcode: %d", GetLastError()); return 2; } len = byte_read_act; if( byte_read_act < iByteRead ) { dbg_pw("UART::read Read data byte leng is not correct!"); return 3; } } else { return 1; } return 0; }
int RMessagePassing::thread_func(void *arg) { // accept connection forever while( getAlive() ) { RSocket *new_socket = new RSocket; // accept new connection if( 0 != m_socket.accept(*new_socket) ) { dbg_pe("Accept new connection failed!"); delete new_socket; break; } // create new receiving thread RMP_SocketThread *new_thread; new_thread = new RMP_SocketThread(new_socket, this); new_thread->setNodeMe(m_nodeMe); // start receiving thread new_thread->start(); } return 0; }
int RMessagePassing::begin(string name) { string node_ip = "127.0.0.1"; int node_port; string nodeMaster_ip = "127.0.0.1"; int nodeMaster_port = 30000; int ret; if( "Master" == name ) { m_isMaster = 1; node_port = 30000; } else { m_isMaster = 0; node_port = 30001; } // load self information (IP address, port) node_ip = svar.GetString(name+".ip", node_ip); node_port = svar.GetInt(name+".port", node_port); // start message passing system if( m_isMaster ) { RMP_Node *pn = new RMP_Node; pn->set("Master", node_ip, node_port); pn->isMaster = 1; pn->setNodeInfo(); pn->mpObj = this; pn->mpMe = this; m_nodeMe = pn; m_nodeMaster = pn; pn->print(); // insert to node map m_nodeMap.setMP(this); m_nodeMap.insert(pn, 0); // start server ret = m_socket.startServer(pn->inet4_port); if( ret != 0 ) { dbg_pe("Can not start socket_server: %s (%s)\n", pn->nodeName.c_str(), pn->hostName.c_str()); return -1; } // start receiving thread start(); } else { // insert to node map RMP_Node *pn = new RMP_Node, *pnm = new RMP_Node; nodeMaster_ip = svar.GetString("Master.ip", nodeMaster_ip); nodeMaster_port = svar.GetInt("Master.port", 30000); // set self/master node information pn->set(name, node_ip, node_port); pnm->set("Master", nodeMaster_ip, nodeMaster_port); pn->setNodeInfo(); pn->mpMe = this; pn->mpObj = this; pnm->mpMe = this; pnm->isMaster = 1; m_nodeMe = pn; m_nodeMaster = pnm; m_nodeMe->print(); m_nodeMaster->print(); m_nodeMap.setMP(this); m_nodeMap.insert(pn, 0); m_nodeMap.insert(pnm, 0); // start server ret = m_socket.startServer(pn->inet4_port); if( ret != 0 ) { dbg_pe("Can not start socket_server: %s (%s)\n", pn->nodeName.c_str(), pn->hostName.c_str()); return -1; } // start receiving thread start(); // send my node information to master node RMessage msg; msg.msgType = 0xFFFFFFF0; pn->toStream(msg.data); if( pnm->sendMsg(&msg) < 0 ) { dbg_pe("Please start master node first!\n"); return -2; } } return 0; }
// Reference: // http://stackoverflow.com/questions/6947413/how-to-open-read-and-write-from-serial-port-in-c // http://stackoverflow.com/questions/18108932/linux-c-serial-port-reading-writing int UART::open(const std::string &portName, int baudRate) { UART_inner_data *pd; int r = 0; pd = (UART_inner_data *) data; if( portName.size() ) port_name = portName; if( baudRate ) baud_rate = baudRate; //pd->fd = ::open(port_name, O_RDWR|O_NOCTTY|O_SYNC); //pd->fd = ::open(port_name, O_RDWR| O_NONBLOCK | O_NDELAY); if( m_options & UART_NONBLOCK ) pd->fd = open_c(port_name.c_str(), O_RDWR|O_NOCTTY|O_SYNC|O_NONBLOCK); else pd->fd = open_c(port_name.c_str(), O_RDWR|O_NOCTTY|O_SYNC); if( pd->fd < 0 ) { dbg_pe("open port error! (%s)\n", port_name.c_str()); return -1; } // set port baud rate speed_t b_speed; switch(baud_rate) { case 110: b_speed = B110; break; case 300: b_speed = B300; break; case 600: b_speed = B600; break; case 1200: b_speed = B1200; break; case 2400: b_speed = B2400; break; case 4800: b_speed = B4800; break; case 9600: b_speed = B9600; break; case 19200: b_speed = B19200; break; case 38400: b_speed = B38400; break; case 57600: b_speed = B57600; break; case 115200: b_speed = B115200; break; case 230400: b_speed = B230400; break; case 460800: b_speed = B460800; break; case 500000: b_speed = B500000; break; case 576000: b_speed = B576000; break; case 921600: b_speed = B921600; break; case 1000000: b_speed = B1000000; break; default: dbg_pe("unsupported baud rate: %d\n", baud_rate); r = -2; goto UART_OPEN_ERR; } struct termios tty; memset(&tty, 0, sizeof(tty)); if( tcgetattr(pd->fd, &tty) != 0 ) { dbg_pe("error %d from tcgetattr\n", errno); r = -3; goto UART_OPEN_ERR; } cfsetispeed(&tty, b_speed); cfsetospeed(&tty, b_speed); /* Setting other Port Stuff */ tty.c_cflag &= ~PARENB; // Make 8n1 tty.c_cflag &= ~CSTOPB; tty.c_cflag &= ~CSIZE; tty.c_cflag |= CS8; tty.c_cflag &= ~CRTSCTS; // no flow control tty.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines if( m_options & UART_NONBLOCK ) { tty.c_cc[VMIN] = 0; // read doesn't block tty.c_cc[VTIME] = 0; // 0.5 seconds read timeout } else { tty.c_cc[VMIN] = 1; // read doesn't block tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout } /* Make raw */ cfmakeraw(&tty); tcflush( pd->fd, TCIFLUSH ); if (tcsetattr (pd->fd, TCSANOW, &tty) != 0) { dbg_pe("error %d from tcsetattr\n", errno); r = -4; goto UART_OPEN_ERR; } goto UART_OPEN_RET; UART_OPEN_ERR: ::close(pd->fd); pd->fd = -1; UART_OPEN_RET: return r; }
/******************************************************************************\ * 1-D Spline interpolation * * Prameters: * xa [in] x coordinate * ya [in] y values * xia [in] interpolated x array * yia [out] interpolated y array * n [in] data length * ni [in] interpolated length \******************************************************************************/ void splint(double *_xa, double *_ya, double *_xia, double *_yia, int n, int ni, double yp1, double ypn) { double *xa, *ya; double *xia, *yia; double *y2a, *_y2a; int klo, khi, k, j; double h, b, a; double x; // calculate second derivtive _y2a = (double*) malloc(sizeof(double)*n); y2a = _y2a - 1; spline(_xa, _ya, n, yp1, ypn, _y2a); // interpolate each point xa = _xa - 1; ya = _ya - 1; xia = _xia - 1; yia = _yia - 1; klo = 1; khi = klo + 1; for(j=1; j<=ni; j++) { x = xia[j]; #if 1 // update klo while( klo+1<=n && x>xa[klo+1]) { klo ++; } // update khi while( x>xa[khi] ) { khi ++; } #else klo=1; khi=n; while (khi-klo > 1) { k=(khi+klo) >> 1; if (xa[k] > x) khi=k; else klo=k; } #endif h=xa[khi]-xa[klo]; if (h == 0.0) { dbg_pe("Bad xa input to routine splint"); exit(1); } a = (xa[khi]-x)/h; b = (x-xa[klo])/h; yia[j] = a*ya[klo] + b*ya[khi] + ((a*a*a-a)*y2a[klo] + (b*b*b-b)*y2a[khi])*(h*h)/6.0; } free(_y2a); }
int UART::open(const std::string &portName, int baudRate) { UART_inner_data *pd; COMMTIMEOUTS oComTimeout; BOOL fSuccess; int ret_code = 0; // handle structure pd = (UART_inner_data *) data; if( portName.size() ) port_name = portName; if( baudRate ) baud_rate = baudRate; // open port pd->m_hCom = CreateFile( port_name.c_str(), // COM port Name GENERIC_READ | GENERIC_WRITE, // read/write flags 0, // comm devices must be opened w/exclusive-access NULL, // no security attributes OPEN_EXISTING, // COM devices must use OPEN_EXISTING 0, // no overlapped I/O NULL // hTemplate must be NULL for COM devices ); if (pd->m_hCom == INVALID_HANDLE_VALUE) { // Handle the error. dbg_pe("UART::open CreateFile failed with wrror %d.", GetLastError()); return 1; } // We will build on the current configuration, and skip setting the size // of the input and output buffers with SetupComm. fSuccess = GetCommState(pd->m_hCom, &(pd->m_oDCB)); if ( !fSuccess ) { dbg_pe("UART::open GetComState failed with error %d.", GetLastError()); ret_code = 2; goto UART_OPEN_ERR; } // Fill in the DCB switch(baud_rate) { case 110: pd->m_oDCB.BaudRate = CBR_110; break; case 300: pd->m_oDCB.BaudRate = CBR_300; break; case 600: pd->m_oDCB.BaudRate = CBR_600; break; case 1200: pd->m_oDCB.BaudRate = CBR_1200; break; case 2400: pd->m_oDCB.BaudRate = CBR_2400; break; case 4800: pd->m_oDCB.BaudRate = CBR_4800; break; case 9600: pd->m_oDCB.BaudRate = CBR_9600; break; case 14400: pd->m_oDCB.BaudRate = CBR_14400; break; case 19200: pd->m_oDCB.BaudRate = CBR_19200; break; case 38400: pd->m_oDCB.BaudRate = CBR_38400; break; case 56000: pd->m_oDCB.BaudRate = CBR_56000; break; case 57600: pd->m_oDCB.BaudRate = CBR_57600; break; case 115200: pd->m_oDCB.BaudRate = CBR_115200; break; case 128000: pd->m_oDCB.BaudRate = CBR_128000; break; case 2560000: pd->m_oDCB.BaudRate = CBR_256000; break; default: dbg_pe("UART::open Unsupport baud rate %d.", baud_rate);; ret_code = 3; goto UART_OPEN_ERR; } if( DTR_sw ) pd->m_oDCB.fDtrControl = DTR_CONTROL_DISABLE; if( RTS_sw ) pd->m_oDCB.fRtsControl = RTS_CONTROL_DISABLE; pd->m_oDCB.ByteSize = byte_size; pd->m_oDCB.Parity = parity_sw; switch( stop_bits ) { case 1: pd->m_oDCB.StopBits = ONESTOPBIT; break; case 15: pd->m_oDCB.StopBits = ONE5STOPBITS; break; case 2: pd->m_oDCB.StopBits = TWOSTOPBITS; break; } fSuccess = SetCommState(pd->m_hCom, &(pd->m_oDCB)); if (!fSuccess) { // Handle the error. dbg_pe("UART::open SetComState failed with error %d.", GetLastError()); ret_code = 4; goto UART_OPEN_ERR; } // timout options if( timeout_sw ) { fSuccess = GetCommTimeouts( pd->m_hCom, /* Handle to comm device */ &oComTimeout /* time-out values */ ); if( !fSuccess ) { dbg_pe("UART::open GetCommTimeouts failed with error %d.", GetLastError()); ret_code = 5; goto UART_OPEN_ERR; } oComTimeout.ReadIntervalTimeout = DEFAULT_READINTERVALTIMEOUT; oComTimeout.ReadTotalTimeoutMultiplier = DEFAULT_READMULTIPLIER; oComTimeout.ReadTotalTimeoutConstant = DEFAULT_READTOTALTIMEOUT; oComTimeout.WriteTotalTimeoutMultiplier = DEFAULT_WRITEMULTIPLIER; oComTimeout.WriteTotalTimeoutConstant = DEFAULT_WRITETOTALTIMEOUT; fSuccess = SetCommTimeouts( pd->m_hCom, &oComTimeout ); if( !fSuccess ) { dbg_pe("UART::open SetCommTimeouts failed with error %d.", GetLastError()); ret_code = 6; goto UART_OPEN_ERR; } } // port state goto UART_OPEN_RET; UART_OPEN_ERR: CloseHandle(pd->m_hCom); pd->m_hCom = NULL; UART_OPEN_RET: return ret_code; }
int RMP_Node::sendMsg(RMessage *m) { int ret; ru8 *buf = NULL; // same process if( isSameProcess && mpObj != NULL ) { mpObj->recvMessageSlot(m); return 0; } // connect to node if( socketSend == NULL ) { socketSend = new RSocket; // create connections ret = socketSend->startClient(inet4_addr, inet4_port); if( ret != 0 ) { dbg_pe("Can not connect to node: %s (%s), addr = %d, port = %d\n", nodeName.c_str(), hostName.c_str(), inet4_addr, inet4_port); ret = -1; goto SEND_MSG_ERR; } // send my node name to pairwise node RDataStream ds; ds.write(mpMe->getNodeMe()->nodeName); ret = socketSend->send(ds); //dbg_pt("send my node to other: %s\n", nodeName.c_str()); if( ret < ds.size() ) { dbg_pe("Can not connect to node: %s (%s), addr = %d, port =%d\n", nodeName.c_str(), hostName.c_str(), inet4_addr, inet4_port); ret = -1; goto SEND_MSG_ERR; } } // send message to node ru32 header_len; ru32 msg_magic, msg_ver, msg_header; ru32 data_len; header_len = 4*sizeof(ru32); buf = new ru8[header_len+4]; msg_magic = RTK_MAGIC_MESSAGE; msg_ver = 1; msg_header = (msg_magic << 16) | (msg_ver & 0xFFFF); data_len = m->data.size(); memcpy(buf + 0*sizeof(ru32), &msg_header, sizeof(ru32)); memcpy(buf + 1*sizeof(ru32), &(m->msgType), sizeof(ru32)); memcpy(buf + 2*sizeof(ru32), &(m->msgID), sizeof(ru32)); memcpy(buf + 3*sizeof(ru32), &data_len, sizeof(ru32)); // send header ret = socketSend->send(buf, header_len); if( ret < 0 ) goto SEND_MSG_ERR; // send data ret = socketSend->send(m->data.data(), m->data.size()); if( ret < 0 ) goto SEND_MSG_ERR; goto SEND_MSG_RETURN; SEND_MSG_ERR: ret = -2; dbg_pw("Send message error to node: %s (%s), errcode = %d\n", nodeName.c_str(), hostName.c_str(), ret); socketSend->close(); socketSend = NULL; SEND_MSG_RETURN: if( buf != NULL ) delete [] buf; return ret; }