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