/**********************************************************************
* @return None
* @remark Steps into the set station state machine.
**********************************************************************/
static void doSetStationSM() {
/* Decide whether to save the current postion as a station, or
a given position. */
if (wantSaveStation) {
// Save current position as station
Navigation_getLocalPosition(&nedStation);
handleAcknowledgement();
haveStation = TRUE;
event.flags.setStationDone = TRUE;
DBPRINT("Saved new station: N=%.2f, E=%.2f, D=%.2f\n",
nedStation.north, nedStation.east, nedStation.down);
}
else {
// Set the given coordinate as station
nedStation.north = Mavlink_newMessage.gpsLocalData.north;
nedStation.east = Mavlink_newMessage.gpsLocalData.east;
nedStation.down = Mavlink_newMessage.gpsLocalData.down;
handleAcknowledgement();
haveStation = TRUE;
event.flags.setStationDone = TRUE;
DBPRINT("Set new station: N=%.2f, E=%.2f, D=%.2f.\n",
nedStation.north, nedStation.east, nedStation.down);
}
}
/**********************************************************************
* Function: doSetOriginSM
* @return None
* @remark Steps into the set origin state machine.
**********************************************************************/
static void doSetOriginSM() {
// Waiting for origin message from command center
if (event.flags.haveSetOriginMessage) {
GeocentricCoordinate ecefOrigin;
ecefOrigin.x = Mavlink_newMessage.gpsGeocentricData.x;
ecefOrigin.y = Mavlink_newMessage.gpsGeocentricData.y;
ecefOrigin.z = Mavlink_newMessage.gpsGeocentricData.z;
Navigation_setOrigin(&ecefOrigin);
handleAcknowledgement();
haveOrigin = TRUE;
event.flags.setOriginDone = TRUE;
DBPRINT("Set new origin: X=%.1f, Y=%.1f, Z=%.1f\n",
ecefOrigin.x, ecefOrigin.y, ecefOrigin.z);
}
else {
// Resend request if timer expires
if (Timer_isExpired(TIMER_SETORIGIN)) {
// Resend request origin if timed out
if (resendMessageCount >= RESEND_MESSAGE_LIMIT) {
// Sent too many times
setError(ERROR_NO_ORIGIN);
return;
}
else {
DBPRINTF("Resending origin request.\n");
Mavlink_sendRequestOrigin(NO_ACK); // just want message
Timer_new(TIMER_SETORIGIN, RESEND_MESSAGE_DELAY);
resendMessageCount++;
}
} // timer expired
}
}
void QXmppStream::_q_socketReadyRead()
{
d->dataBuffer.append(d->socket->readAll());
// handle whitespace pings
if (!d->dataBuffer.isEmpty() && d->dataBuffer.trimmed().isEmpty()) {
d->dataBuffer.clear();
handleStanza(QDomElement());
}
// FIXME : maybe these QRegExps could be static?
QRegExp startStreamRegex("^(<\\?xml.*\\?>)?\\s*<stream:stream.*>");
startStreamRegex.setMinimal(true);
QRegExp endStreamRegex("</stream:stream>$");
endStreamRegex.setMinimal(true);
// check whether we need to add stream start / end elements
//
// NOTE: as we may only have partial XML content, do not alter the stream's
// state until we have a valid XML document!
QByteArray completeXml = d->dataBuffer;
const QString strData = QString::fromUtf8(d->dataBuffer);
bool streamStart = false;
if (d->streamStart.isEmpty() && strData.contains(startStreamRegex))
streamStart = true;
else
completeXml.prepend(d->streamStart);
bool streamEnd = false;
if (strData.contains(endStreamRegex))
streamEnd = true;
else
completeXml.append(streamRootElementEnd);
// check whether we have a valid XML document
QDomDocument doc;
if (!doc.setContent(completeXml, true))
return;
// remove data from buffer
logReceived(strData);
d->dataBuffer.clear();
// process stream start
if (streamStart) {
d->streamStart = startStreamRegex.cap(0).toUtf8();
handleStream(doc.documentElement());
}
// process stanzas
QDomElement nodeRecv = doc.documentElement().firstChildElement();
while (!nodeRecv.isNull()) {
if (QXmppStreamManagementAck::isStreamManagementAck(nodeRecv))
handleAcknowledgement(nodeRecv);
else if (QXmppStreamManagementReq::isStreamManagementReq(nodeRecv))
sendAcknowledgement();
else {
handleStanza(nodeRecv);
if(nodeRecv.tagName() == QLatin1String("message") ||
nodeRecv.tagName() == QLatin1String("presence") ||
nodeRecv.tagName() == QLatin1String("iq"))
++d->lastIncomingSequenceNumber;
}
nodeRecv = nodeRecv.nextSiblingElement();
}
// process stream end
if (streamEnd)
disconnectFromHost();
}
static size_t
getPacket (BrailleDisplay *brl, FS_Packet *packet) {
while (1) {
size_t count = readPacket(brl, packet);
if (count > 0) {
switch (packet->header.type) {
{
int ok;
case FS_PKT_NAK:
cancelMissingAcknowledgementAlarm(brl);
logNegativeAcknowledgement(packet);
if (!brl->data->acknowledgementHandler) {
logMessage(LOG_WARNING, "unexpected NAK");
continue;
}
switch (packet->header.arg1) {
case FS_ERR_TIMEOUT: {
int originalLimit = brl->data->outputPayloadLimit;
if (brl->data->outputPayloadLimit > brl->data->model->cellCount)
brl->data->outputPayloadLimit = brl->data->model->cellCount;
if (brl->data->outputPayloadLimit > 1)
brl->data->outputPayloadLimit -= 1;
if (brl->data->outputPayloadLimit != originalLimit) {
logMessage(LOG_WARNING, "maximum payload length reduced from %d to %d",
originalLimit, brl->data->outputPayloadLimit);
}
break;
}
}
ok = 0;
goto doAcknowledgement;
case FS_PKT_ACK:
cancelMissingAcknowledgementAlarm(brl);
if (!brl->data->acknowledgementHandler) {
logMessage(LOG_WARNING, "unexpected ACK");
continue;
}
ok = 1;
goto doAcknowledgement;
doAcknowledgement:
if (handleAcknowledgement(brl, ok)) continue;
count = 0;
break;
}
default:
break;
}
}
return count;
}
}
