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;
}
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);
}
}
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;
}
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)...);
});
}
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);
}
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);
}
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;
}
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());
}
}