Esempio n. 1
0
int Worker::inquiry()
{
    Datagram dg;
    inquiry_result_t * btDev;
    unsigned char btDevNum;

    // wait for inquiry (about 20 seconds)
    // FLEX: returns Bluetooth devices

    sendDatagram(serial, REQUEST, INQUIRY);
    receiveDatagram(serial, &dg);
    if (!(dg.type == RESPONSE && dg.id == INQUIRY))
        return -1;

    btDevNum = parseInquiryDatagram(&btDev, &dg);

    emit bluetoothInquiryCompleted(btDev, btDevNum);

    sync->acquire();

    if (btDeviceIndexChosen == -1)
        return -1;

    sendDatagram(serial, RESPONSE, OK);

    return 0;
}
Esempio n. 2
0
void ConnectionManager::connectionReadyRead()
{
	struct msghdr msg;
	struct iovec iov;
	TTRACE("ConnectionManager::connectionReadyRead");

	/* Response data */
	iov.iov_base = buffer.data();
	iov.iov_len = buffer.length();

	/* compose the message */
	memset(&msg, 0, sizeof(msg));
	msg.msg_iov = &iov;
	msg.msg_iovlen = 1;

	while(connectionFd >= 0) {
		int nread = recvmsg(connectionFd, &msg, MSG_DONTWAIT);
		if (nread == 0) {
			connectionClose();
			return;
		} else if (nread < 0) {
			if (errno != EAGAIN) {
				TWARN("recvmsg error %s", strerror(errno));
				//close(connFd);
				connectionClose();
			}
			return;
		}

		receiveDatagram(buffer.data(), nread);
	}

}
Esempio n. 3
0
int Worker::sendBitmask()
{
    Datagram dg;
    unsigned char * bitmask;

    // FLEX: requests the bitmask

    receiveDatagram(serial, &dg);

    if (dg.type != REQUEST || dg.id != GET_BITMASK) {
        qDebug() << "Expected bitmap request!";
        return -1;
    }

    // PC: sends the bitmask

    bitmask = new unsigned char[((Vehicle *)vehicle)->getBitmaskSize()];
    ((Vehicle *)vehicle)->getBitmask(bitmask);
    constructDatagram(&dg,
                      RESPONSE,
                      GET_BITMASK,
                      ((Vehicle *)vehicle)->getBitmaskSize(),
                      bitmask);

    sendDatagram(serial, &dg);
    destructDatagramData(&dg);

    // switch to data request loop

    return 0;
}
Esempio n. 4
0
void
DatagramTransport<T, U>::handleReceive(const boost::system::error_code& error, size_t nBytesReceived)
{
  receiveDatagram(m_receiveBuffer.data(), nBytesReceived, error);

  if (m_socket.is_open())
    m_socket.async_receive_from(boost::asio::buffer(m_receiveBuffer), m_sender,
                                [this] (auto&&... args) {
                                  this->handleReceive(std::forward<decltype(args)>(args)...);
                                });
}
Esempio n. 5
0
void ping()
{
    Datagram dg;

    receiveDatagram(&serial, &dg);

    if (!(dg.type == REQUEST && dg.id == HELLO)) {
        qDebug() << "\tPing error!";
        exit(1);
    }

    qDebug() << "\tFound PC! Replying";
    sendDatagram(&serial, RESPONSE, HELLO);
}
Esempio n. 6
0
void receiveBitmask()
{
    Datagram dg;

    qDebug() << "Requesting bitmask";
    sendDatagram(&serial, REQUEST, GET_BITMASK);
    qDebug() << "DONE";

    qDebug() << "Receiving bitmask";
    receiveDatagram(&serial, &dg);
    qDebug() << "DONE";

    qDebug() << "Bitmask:";
    bitmaskSize = dg.size;
    bitmask = new unsigned char[dg.size];
    for (unsigned int i=0; i<dg.size; i++) {
        bitmask[i] = dg.data[i];
        qDebug() << "\t" << bitmask[i];
    }

    destructDatagramData(&dg);
}
Esempio n. 7
0
int main()
{
    Datagram dg;
    Serial_t serialConfig;

    t=0;

    serialConfig.baud_rate = 9600;
    serialConfig.bits = 8;
    serialConfig.parity = 0;
    serialConfig.stop_bits = 1;
    serialConfig.device = "ttyUSB0";

    serial.setConfig(serialConfig);
    if (serial.connect() == -1) {
        qDebug() << "Serial connection error!";
        exit(1);
    }
    qDebug() << "Serial connected!";

    qDebug() << "Ping...";
    ping();
    qDebug() << "DONE";

    bool remain = true;
    while (remain) {
        qDebug() << "Inquiry or ok?";
        receiveDatagram(&serial, &dg);
        switch (dg.type) {
        case REQUEST:
            switch (dg.id) {
            case INQUIRY:
                qDebug() << "Inquiry Request...";
                inquiry();
                qDebug() << "DONE";
                break;
            default:
                qDebug() << "Unknown";
                break;
            }
            break;
        case RESPONSE:
            switch (dg.id) {
            case OK:
                qDebug() << "OK";
                remain = false;
                break;
            default:
                qDebug() << "Unknown";
                break;
            }
            break;
        default:
            qDebug() << "Unknown";
            break;
        }
    }

    receiveDatagram(&serial, &dg);
    if (dg.type != REQUEST || dg.id != CONNECT_TO) {
        qDebug() << "Expected connection!";
        qDebug() << "QUIT";
        exit(1);
    }

    qDebug() << "Connecting to: " << *dg.data;
    sendDatagram(&serial, RESPONSE, SUCCESS);
    qDebug() << "DONE";

    loop();
    return 0;
}
Esempio n. 8
0
void PolycrankPanelDlg::edpClicked()
{
	if (!edpCheckBox->isChecked())
	{	//edpCheckBox is NOT active (Graspit is NOT connecting with MRROC++)

		IPLineEdit->setEnabled(true);
		PortLineEdit->setEnabled(true);

		slider->setEnabled(true);//user can change frequency
		spinBox->setEnabled(true);//user can change frequency

		//Reset Timer for send & receive
		timerInterval.stop();

		saveCheckBox->setChecked(false); //set button in state :: off

		//saveCheckBox->setChecked(true); //set button in state :: on
		saveCheckBox->setEnabled(false); //user can NOT save trajectory, because connection failed

		udpSocket.close();

		Q0DegreesLineEdit->clear();
		Q1DegreesLineEdit->clear();
		Q2DegreesLineEdit->clear();
		Q3DegreesLineEdit->clear();
		Q4DegreesLineEdit->clear();
		Q5DegreesLineEdit->clear();
		Q6DegreesLineEdit->clear();

		Q0RadianLineEdit->clear();
		Q1RadianLineEdit->clear();
		Q2RadianLineEdit->clear();
		Q3RadianLineEdit->clear();
		Q4RadianLineEdit->clear();
		Q5RadianLineEdit->clear();
		Q6RadianLineEdit->clear();

		progressBar->setRange( 0, 1 );

	
		file.close();//very important, when user suddenly cancel EDP Polycrank
					 //in first step app should close socket
					 //in second step app should close file (descriptor)
					 //when user want to save trajectory again trajectory will be saved in new file
					 //NOT the same file 
	}
	else
	{   //edpCheckBox is active (Graspit is connecting with MRROC++)
/*
		Q0DegreesLineEdit->clear();
		Q1DegreesLineEdit->clear();
		Q2DegreesLineEdit->clear();
		Q3DegreesLineEdit->clear();
		Q4DegreesLineEdit->clear();
		Q5DegreesLineEdit->clear();
		Q6DegreesLineEdit->clear();

		Q0RadianLineEdit->clear();
		Q1RadianLineEdit->clear();
		Q2RadianLineEdit->clear();
		Q3RadianLineEdit->clear();
		Q4RadianLineEdit->clear();
		Q5RadianLineEdit->clear();
		Q6RadianLineEdit->clear();
*/
		Q0DegreesLineEdit->setText("PLEASE WAIT");
		Q1DegreesLineEdit->setText("PLEASE WAIT");
		Q2DegreesLineEdit->setText("PLEASE WAIT");
		Q3DegreesLineEdit->setText("PLEASE WAIT");
		Q4DegreesLineEdit->setText("PLEASE WAIT");
		Q5DegreesLineEdit->setText("PLEASE WAIT");
		Q6DegreesLineEdit->setText("PLEASE WAIT");
	
		Q0RadianLineEdit->setText("PLEASE WAIT");
		Q1RadianLineEdit->setText("PLEASE WAIT");
		Q2RadianLineEdit->setText("PLEASE WAIT");
		Q3RadianLineEdit->setText("PLEASE WAIT");
		Q4RadianLineEdit->setText("PLEASE WAIT");
		Q5RadianLineEdit->setText("PLEASE WAIT");
		Q6RadianLineEdit->setText("PLEASE WAIT");

		progressBar->setRange( 0, 1 );

		not_synchronised_counter=0;
		failed_counter=0;

		IPLineEdit->setEnabled(false);
		PortLineEdit->setEnabled(false);

		slider->setEnabled(false);//user can NOT change frequency during the connection
		spinBox->setEnabled(false);//user can NOT change frequency during the connection
		
		saveCheckBox->setEnabled(true);//user can save trajectory

		//udpSocket.bind(5000);
		udpSocket.bind(PortLineEdit->text().toLong());
		
		connect(&timerInterval, SIGNAL(timeout()), this, SLOT(sendDatagram()));
		//set interval in (ms) between sending datagrams
		timerInterval.start(1000.0/ spinBox->text().toInt());

		connect(&timerInterval, SIGNAL(timeout()), this, SLOT(receiveDatagram()));
		//set interval in (ms) between receiving datagrams
		timerInterval.start(1000.0/ spinBox->text().toInt());
	}
}