void NetworkWidget::checkNetwork() { // check if we have received any packets since last time. if (stats_packets == 0 || stats_packets > stats_packets_old) { // Restart the Timer NetworkTimer->stop(); NetworkTimer->start( networkFailureTime ); stats_packets_old = stats_packets; // Update the value return; } if (stats_packets == stats_packets_old) { // Network timeout has occured. STATE = NET_FAILURE; stats_network_failures++; ui->lblStatus->setText("Network Failure!"); ui->lblNetworkFailures->setText(QString::number(stats_network_failures, 10)); NetworkTimer->stop(); // Dump queue emptyQueue(); if (ui->chkDefaultsOnNetworkFail->isChecked()) { sendDefaults(); } } }
void StreamlineSetup::handleRequest(char* xml) { mxml_node_t *tree, *node; const char * attr = NULL; tree = mxmlLoadString(NULL, xml, MXML_NO_CALLBACK); node = mxmlFindElement(tree, tree, TAG_REQUEST, ATTR_TYPE, NULL, MXML_DESCEND_FIRST); if (node) { attr = mxmlElementGetAttr(node, ATTR_TYPE); } if (attr && strcmp(attr, VALUE_EVENTS) == 0) { sendEvents(); logg.logMessage("Sent events xml response"); } else if (attr && strcmp(attr, VALUE_CONFIGURATION) == 0) { sendConfiguration(); logg.logMessage("Sent configuration xml response"); } else if (attr && strcmp(attr, VALUE_COUNTERS) == 0) { sendCounters(); logg.logMessage("Sent counters xml response"); } else if (attr && strcmp(attr, VALUE_CAPTURED) == 0) { CapturedXML capturedXML; char* capturedText = capturedXML.getXML(false); sendData(capturedText, strlen(capturedText), RESPONSE_XML); free(capturedText); logg.logMessage("Sent captured xml response"); } else if (attr && strcmp(attr, VALUE_DEFAULTS) == 0) { sendDefaults(); logg.logMessage("Sent default configuration xml response"); } else { char error[] = "Unknown request"; sendData(error, strlen(error), RESPONSE_NAK); logg.logMessage("Received unknown request:\n%s", xml); } mxmlDelete(tree); }
/* Get the datagrams and store them in a queue. We have a QTimer that is running and taking them out of the queue. */ void NetworkWidget::processPendingDatagrams() { while (udpSocket->hasPendingDatagrams()) { QByteArray datagram; datagram.resize(udpSocket->pendingDatagramSize()); udpSocket->readDatagram(datagram.data(), datagram.size()); if (STATE == EMERG_STOP || (STATE == NET_FAILURE && ui->chkStopOnNetworkFailure->isChecked())) { // Discard all packets return; } // STOP CONDITION if ( datagram.size() && datagram[0]==(char)0 ) { STATE = EMERG_STOP; // output whatever is need for stop. sendDefaults(); // Stop the network timer NetworkTimer->stop(); emptyQueue(); ui->btnClearEmergencyStop->setEnabled(true); ui->lblEmergencyStatus->setText("EMERGENCY STOP!"); QPalette red_text; red_text.setColor(QPalette::WindowText, Qt::red); ui->lblEmergencyStatus->setPalette(red_text); ui->lblEmergencyStatus->setFrameStyle(QFrame::Box); ui->lblStatus->setText("EMERGENCY STOP!"); return; } // Increment the received packet count stats_packets++; // check if valid packet if (validPacket(datagram.constData())) { queue.enqueue(datagram); } else { stats_invalid++; } } // Update the Statistics ui->lblPackets->setText(QString::number(stats_packets,10)); ui->lblInvalid->setText(QString::number(stats_invalid,10)); // Check if a network failure occured. if (STATE == NET_FAILURE && !ui->chkStopOnNetworkFailure->isChecked()) { STATE = LISTENING; NetworkTimer->start( networkFailureTime ); } ui->lblStatus->setText("Receiving Packets..."); }
// Output data to the ServoBoard void NetworkWidget::ouputServoData() { if (STATE == NET_FAILURE || STATE == EMERG_STOP) { emptyQueue(); if (!ui->chkDefaultsOnNetworkFail->isChecked()) { return; } sendDefaults(); return; } if (STATE == COM_FAILURE || !this->serial || !this->serial->isOpen()){ // communications failure STATE = COM_FAILURE; // Should we try to get the COM port back? return; } if (queue.isEmpty()) return; // handle items that have been received from the network. QByteArray message, data(2,0); unsigned char pairs, current_servo; int error; float p_term, d_term; // Update the statistics on the Max Queue size bool convert; int max = ui->lblQueueMax->text().toInt(&convert, 10); if (convert && max < queue.size()) ui->lblQueueMax->setText(QString::number(queue.size(), 10)); do { message = queue.dequeue(); } while (message.size() < 1); if (ui->chkSendMostRecent->isChecked()) { // dump the queue emptyQueue(); } gain_p = 0.26f; gain_d = 0.04f; gain_i = 0.015f; pairs = (message[0] * 2); unsigned int i = 1; while (i < pairs) { current_servo = message[i] - 1; // Store the current servo number. (range of current_servo: 0-11) data[0] = 144 + current_servo + 1; // Output to Servo Board. (range of data: 1-12) if (ui->chkPIDcontrol->isChecked()) { // Basic PID control error = message[i+1] - servo[current_servo]; // error = new_value - old_value p_term = gain_p * error; servo_integral[current_servo] += gain_i * error; d_term = gain_d * (error - servo_error[current_servo])/(outputTime/100); servo[current_servo] = p_term + d_term + servo_integral[current_servo] + servo[current_servo]; servo_error[current_servo] = error; // Maximum range exceeded? if (servo[current_servo] > 97) servo[current_servo] = 97; if (servo[current_servo] < 1) servo[current_servo] = 1; data[1] = servo[current_servo]; } else { // No control data[1] = servo[current_servo] = message[i+1]; // servo value } // Update the Current value on the form. currentServoValue[current_servo]->setText(QString::number(servo[current_servo],10)); // Send off to the Servo board. if (this->serial->write( data ) == -1) { STATE = COM_FAILURE; return; } i += 2; // Go to the next servo pair } }