/************************************************* * Name : BTUartLinuxInit * Parameter : * func : If there is Rx data, callback to stack to handle * Return value : * BT_STATUS_SUCCESS : Success * BT_STATUS_FAILED : Failed *************************************************/ BtStatus BTUartLinuxInit(UartCallback func) { // Checking if the uart is already opened // if com port is opened, let opencomm to close then open again /* if (commPort >= 0) { bt_android_log("[UART][ERR] the uart already opened : %d", commPort); bt_prompt_trace(MOD_BT, "[UART][ERR] the uart already opened : %d", commPort); return BT_STATUS_SUCCESS; } */ // Create this write operation's OVERLAPPED structure's hEvent. BTAdpUartCtx.cb_func = func; //Power on BT before open uart set_bluetooth_power(0); /* Power on controller */ if( set_bluetooth_power(1) < 0) { bt_prompt_trace(MOD_BT, "[ERR] failed to set bt power"); return BT_STATUS_FAILED; } if ( openComm() != 0 ){ set_bluetooth_power(0); return BT_STATUS_FAILED; } else { return BT_STATUS_SUCCESS; } }
IO::IO(){ h = openComm(); if (h == INVALID_HANDLE_VALUE) throw runtime_error("Error Open Comm File"); g_hIOCompletionPort = CreateIoCompletionPort(INVALID_HANDLE_VALUE, NULL, 0, 0); if (INVALID_HANDLE_VALUE == g_hIOCompletionPort) { throw runtime_error("Error occurred while creating IOCP:"); } DWORD nThreadID; for (int i = 0; i < nThreads; i++) { reader_thread[i] = CreateThread(0, 0, WorkerThread, (void *)(this), 0, &nThreadID); } HANDLE hTemp = CreateIoCompletionPort(h, g_hIOCompletionPort, 0, 0); if (INVALID_HANDLE_VALUE == hTemp) { throw runtime_error("Error while bind handle to iocp"); } memset(&ReadOverlapped, 0, sizeof(ReadOverlapped)); if (!ReadFile(h, recv_buffer, sizeof(recv_buffer), NULL, &ReadOverlapped)) { int err = GetLastError(); if (ERROR_IO_PENDING != err) { throw "Read Failed"; } } }
/** * Configure this Transport for communication. * If this Transport is already configured, it will be closed and reconfigured. * The RX buffer and Message queue will be flushed. * Counters will be reset. * @param device The device to communicate over. (Currently, must be serial) * @param retries Number of times to resend an unacknowledged message. * @throws TransportException if configuration fails * @post Transport becomes configured. */ void Transport::configure(const char* device, int retries) { if( configured ) { // Close serial close(); } // Forget old counters resetCounters(); this->retries = retries; if( !openComm(device) ) { configured = true; } else { throw new TransportException("Failed to open serial port", TransportException::CONFIGURE_FAIL); } }
void CtrlComm::run() { openComm(); carSocket_ = new QTcpSocket(); // carSocket_->setSocketOption(QAbstractSocket::SendBufferSizeSocketOption, // 0); connect(carSocket_, SIGNAL(connected()), this, SLOT(connectedSlot()), Qt::DirectConnection); connect(carSocket_, SIGNAL(disconnected()), this, SLOT(disconnectedSlot()), Qt::DirectConnection); connect(carSocket_, SIGNAL(readyRead()), this, SLOT(readyReadSlot()), Qt::DirectConnection); connect(carSocket_, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(errorSlot(QAbstractSocket::SocketError)), Qt::DirectConnection); connect(this, SIGNAL(sendToServerSignal(QByteArray)), this, SLOT(sendToServer(QByteArray)), Qt::DirectConnection); QSettings *configIniRead = new QSettings("xcar.ini", QSettings::IniFormat); QString ipResult = configIniRead->value("/cmd_server/ip").toString(); QString portResult = configIniRead->value("/cmd_server/port").toString(); delete configIniRead; char tmpdata[5] = {(unsigned char)0xff, (unsigned char)0x02, (unsigned char)0x02, (unsigned char)0x80, (unsigned char)0xff}; connectToServer(ipResult.toStdString().c_str(), portResult.toShort()); while (true) { #if 1 if (!serial_->isOpen()) { sleep(1); // emit sendToServerSignal(tmpdata); LOG_ERROR << "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxx"; continue; } unsigned char tmp[5]; memset(&tmp, 0x00, 5); int count = serial_->readData((char *)tmp, 5, timeOut); if (count <= 0) { // emit sendToServerSignal(tmpdata); continue; } // LOG_ERROR<<"1 ===================================== not // find data\n "; // carSocket_->write((const char*)tmp, count); // LOG_ERROR<<"2 ===================================== not // find data\n "; // carSocket_->waitForBytesWritten(timeOut); // LOG_ERROR<<"3 ===================================== not find // data\n "; // continue; QByteArray okData((const char *)tmp); serialArray_.append(okData); // LOG_DEBUG<<"[Serial], recv data: "<<serialArray_.toHex(); // 0xFF 0xFF char spos[2] = {(unsigned char)0xff}; char epos[2] = {(unsigned char)0xff}; int startPos = serialArray_.indexOf(spos, 0); if (startPos == -1) { LOG_ERROR << "1 not find data\n "; serialArray_.clear(); continue; } int endPos = serialArray_.indexOf(epos, startPos + 1); if (endPos == -1) { LOG_ERROR << "2 not find data\n "; continue; } if (endPos - startPos == 1) { serialArray_.clear(); //TODO !!! LOG_ERROR << "3 not find data\n "; continue; } QByteArray serialBuf = serialArray_.mid(startPos, endPos - startPos + 1); if (serialBuf[1] == 0x02 && (serialBuf[3] != 0x80 || serialBuf[3] != 0x7f)) { QByteArray t = serialBuf.toHex(); // LOG_ERROR<<t; } serialArray_.clear(); char light_front[5] = {(unsigned char)0xff, (unsigned char)0x03, (unsigned char)0x03, (unsigned char)0x01}; char light_back[5] = {(unsigned char)0xff, (unsigned char)0x03, (unsigned char)0x03, (unsigned char)0x02}; char power[3] = {(unsigned char)0xff, (unsigned char)0x07}; if (serialBuf.indexOf(light_front, 0) == 0) { emit sendLightMode(1); } else if (serialBuf.indexOf(light_back, 0) == 0) { emit sendLightMode(2); } else if (serialBuf.indexOf(power, 0) == 0) { short a = serialBuf[2]; short b = serialBuf[3]; int value = a << 8 | b; emit showLeftPowerSignal(value); } #endif // sleep(0.1); if (isConnected_) { // f = sendToServer(tmp); // QByteArray serialBuf; // serialBuf.append("test socket\n"); emit sendToServerSignal(serialBuf); // LOG_INFO<<"*****************\n"; // printf("send: %x\n", tmp); } else ; // printf("not connected !\n"); } }
/** * @function main */ int main( int argc, char* argv[] ) { // Open channels with marker-position information if( !openComm() ) { printf("Error opening comm \n"); return 1; } // Marker detector mMd.setMarkerSize(5); // Open device cv::VideoCapture capture( cv::CAP_OPENNI2 ); if( !capture.isOpened() ) { std::cout << "t * Could not open the capture object"<<std::endl; return -1; } printf("\t * Opened device \n"); capture.set( cv::CAP_PROP_OPENNI2_MIRROR, 0.0 ); // off capture.set( cv::CAP_PROP_OPENNI_REGISTRATION, -1.0 ); // on printf("\t * RGB dimensions: (%f,%f), Depth dimensions: (%f,%f) \n", capture.get( cv::CAP_OPENNI_IMAGE_GENERATOR + cv::CAP_PROP_FRAME_WIDTH ), capture.get( cv::CAP_OPENNI_IMAGE_GENERATOR + cv::CAP_PROP_FRAME_HEIGHT ), capture.get( cv::CAP_OPENNI_DEPTH_GENERATOR + cv::CAP_PROP_FRAME_WIDTH ), capture.get( cv::CAP_OPENNI_DEPTH_GENERATOR + cv::CAP_PROP_FRAME_HEIGHT ) ); // Set control panel int value; cv::namedWindow( mWindowName ); // Set mouse callback cv::setMouseCallback( mWindowName, onMouse, 0 ); // Set buttons cv::createTrackbar( "T2_tb", "T2", &value, 255, NULL, NULL ); cv::createButton( "START", start, NULL, cv::QT_PUSH_BUTTON, false ); cv::createButton( "STOP", stop, NULL, cv::QT_PUSH_BUTTON, false ); cv::createTrackbar( "T3_tb", "T3", &value, 255, NULL, NULL ); cv::createButton( "Show points", showPoints, NULL, cv::QT_PUSH_BUTTON, false ); cv::createTrackbar( "T4_tb", "T4", &value, 255, NULL, NULL ); cv::createButton( "Calibrate", calibrate, NULL, cv::QT_PUSH_BUTTON, false ); // Loop for(;;) { if( !capture.grab() ) { std::cout << "\t * [DAMN] Could not grab a frame" << std::endl; return -1; } capture.retrieve( mRgbImg, cv::CAP_OPENNI_BGR_IMAGE ); capture.retrieve( mDepthImg, cv::CAP_OPENNI_POINT_CLOUD_MAP ); update_robotInfo(); mMd.detect( mRgbImg ); // If start if( !stop_flag ) { grabSnapshot( 0, NULL ); storeSnapshot( 0, NULL ); } cv::imshow( mWindowName, mRgbImg ); char k = cv::waitKey(30); if( k == 'q' ) { printf("\t * [PRESSED ESC] Finishing the program \n"); break; } aa_mem_region_local_release(); } // end for return 0; }
/** * @function main */ int main( int argc, char* argv[] ) { // Set kinematics setKinematics(); // Open Communication openComm(); // Marker detector mMd.setMarkerSize(5); // Open device cv::VideoCapture capture( cv::CAP_OPENNI2 ); if( !capture.isOpened() ) { std::cout << "/t * Could not open the capture object"<<std::endl; return -1; } printf("\t * Opened device \n"); capture.set( cv::CAP_PROP_OPENNI2_MIRROR, 0.0 ); // off capture.set( cv::CAP_PROP_OPENNI_REGISTRATION, -1.0 ); // on printf("\t * RGB dimensions: (%f,%f), Depth dimensions: (%f,%f) \n", capture.get( cv::CAP_OPENNI_IMAGE_GENERATOR + cv::CAP_PROP_FRAME_WIDTH ), capture.get( cv::CAP_OPENNI_IMAGE_GENERATOR + cv::CAP_PROP_FRAME_HEIGHT ), capture.get( cv::CAP_OPENNI_DEPTH_GENERATOR + cv::CAP_PROP_FRAME_WIDTH ), capture.get( cv::CAP_OPENNI_DEPTH_GENERATOR + cv::CAP_PROP_FRAME_HEIGHT ) ); // Set control panel int value; cv::namedWindow( mWindowName ); // Set mouse callback cv::setMouseCallback( mWindowName, onMouse, 0 ); // Set buttons cv::createTrackbar( "**********", NULL, &value, 255, NULL ); cv::createButton( "Grab snapshot", grabSnapshot, NULL, cv::QT_PUSH_BUTTON, false ); cv::createButton( "Show snapshot", showSnapshot, NULL, cv::QT_PUSH_BUTTON, false ); cv::createTrackbar( "----------", NULL, &value, 255, NULL ); cv::createButton( "Store snapshot", storeSnapshot, NULL, cv::QT_PUSH_BUTTON, false ); cv::createButton( "Discard snapshot", discardSnapshot, NULL, cv::QT_PUSH_BUTTON, false ); cv::createTrackbar( "xxxxxxxxx", NULL, &value, 255, NULL ); cv::createButton( "Calibrate", calibrate, NULL, cv::QT_PUSH_BUTTON, false ); // Loop for(;;) { if( !capture.grab() ) { std::cout << "\t * [DAMN] Could not grab a frame" << std::endl; return -1; } updateKinematics(); updateTf(); capture.retrieve( mRgbImg, cv::CAP_OPENNI_BGR_IMAGE ); capture.retrieve( mDepthImg, cv::CAP_OPENNI_POINT_CLOUD_MAP ); mMd.detect( mRgbImg ); cv::imshow( mWindowName, mRgbImg ); char k = cv::waitKey(30); if( k == 'q' ) { printf("\t * [PRESSED ESC] Finishing the program \n"); break; } aa_mem_region_local_release(); } // end for return 0; }