ARControlConnection::ARControlConnection(ARController *controller)
: QObject(controller), d_ptr(new ARControlConnectionPrivate(controller, this))
{
Q_D(ARControlConnection);
ARDiscoveryDevice *device = d->controller->discoveryDevice();
DEBUG_T("Loading command dictionary data...");
d->commands->import(":/ARSDK/packages/libARCommands/Xml/ARDrone3_commands.xml");
d->commands->import(":/ARSDK/packages/libARCommands/Xml/common_commands.xml");
d->commands->import(":/ARSDK/packages/libARCommands/Xml/common_debug.xml");
d->commands->import(":/ARSDK/packages/libARCommands/Xml/SkyController_commands.xml");
DEBUG_T("Creating D2C UDP communications socket...");
// Setup UDP port for D2C comms.
d->d2c = new QUdpSocket(this);
d->d2c->bind(QHostAddress(d->controller->controllerAddress()),
d->controller->controllerPort());
QObject::connect(d->d2c, SIGNAL(readyRead()), this, SLOT(onReadyRead()));
DEBUG_T("Creating C2D UDP communications socket...");
// Setup UDP port for C2D comms.
d->c2d = new QUdpSocket(this);
d->c2d->connectToHost(device->address(),
device->parameters().value(ARDISCOVERY_KEY_C2DPORT).toInt());
}
int errorXMLF(const char * errorStr)
{
char s[128];
int el = strlen(errorStr);
if (el > 100)
el = 100;
sprintf(s," Error XML %.*s!\n",el,errorStr);
DEBUG_T(0,s);
return -1;
}
NODE* CParseXml::mainXML(const char *szFileName)
{
int buffLen;
if (bufFile)
delete bufFile;
// bufFile is always terminated with 'nul' bytes
bufFile = loadFile(szFileName, buffLen);
if (bufFile == NULL) {
DEBUG_T(0,"xml file not found");
return NULL;
}
return parseXml(bufFile, buffLen);
}
// short* : Used for systems (Windows) that use wide-chars in filenames
NODE* CParseXml::mainXML(const short *szFileName)
{
int buffLen;
if(bufFile)
delete bufFile;
bufFile =loadFileW(szFileName, buffLen);
if (bufFile == NULL) {
DEBUG_T(0,"xml file not found");
return NULL;
}
return parseXml(bufFile, buffLen);
}
bool ARControlConnection::sendFrame(quint8 type, quint8 id, quint8 seq, const char *data, quint32 dataSize)
{
Q_D(ARControlConnection);
// Check we're actually able to send.
if(d->c2d == NULL || !d->c2d->isWritable())
{
d->errorString = "Control connection not establised.";
emit error();
return false;
}
QByteArray datagram(ARNETWORK_FRAME_HEADER_SIZE + dataSize, 0x00);
QDataStream stream(&datagram, QIODevice::WriteOnly);
stream.setByteOrder(QDataStream::LittleEndian);
// Write frame header to buffer.
stream << type
<< id
<< seq
<< (quint32)datagram.size();
// Write frame payload to buffer.
if(dataSize > 0) stream.writeRawData(data, dataSize);
qint64 result = d->c2d->write(datagram.constData(), datagram.size());
if(result != datagram.size())
{
//TODO: Maybe retry here ..
WARNING_T("Failed to send complete message");
return false;
}
if(type != ARControlConnection::LowLatencyData)
{
DEBUG_T(QString(">> %1:%2 [%3]")
.arg(d->c2d->peerAddress().toString())
.arg(d->c2d->peerPort())
.arg(QString(datagram.toHex())));
}
return true;
}
void my_task_1s(void)
{
static u8 tCnt=0;
#ifdef debug_task_1s
DEBUG_T();
#endif
tCnt++;
my_flow_counter();
my_flow_meter();
my_flow_meterA();
sprintf(mPrintBuf," TM %02X FRA %05d A %02X %04X %04d %04d",tCnt,mFR,mFlowCounter.sensor[0],mFlowCounterA[0].fr_cnt,mFlowRate[0].rate,mFlowRateA[0].rate[4]);
LCD_ShowLineEx(LN_IS+0, (u8 *)mPrintBuf);
sprintf(mPrintBuf," ELT %05d B %02X %04X %04d %04d",mFlowCounter.elapsedtime,mFlowCounter.sensor[1],mFlowCounterA[1].fr_cnt,mFlowRate[1].rate,mFlowRateA[1].rate[4]);
LCD_ShowLineEx(LN_IS+1, (u8 *)mPrintBuf);
sprintf(mPrintBuf," ST %d C %02X %04X %04d %04d",mFlowCounter.state,mFlowCounter.sensor[2],mFlowCounterA[2].fr_cnt,mFlowRate[2].rate,mFlowRateA[2].rate[4]);
LCD_ShowLineEx(LN_IS+2, (u8 *)mPrintBuf);
}
void ARControlConnection::onNavdata(const ARControlFrame &frame)
{
Q_D(ARControlConnection);
// TODO: Should we do this, or should we allow application to decide
// when/if an acknowledge occurs?
// Construct acknowledge frame, if this incoming frame requires it.
if(frame.type == ARControlConnection::AcknowledgeData)
{
QByteArray payload(1, 0x00);
QDataStream datastream(&payload, QIODevice::WriteOnly);
datastream.setByteOrder(QDataStream::LittleEndian);
datastream << (quint8)frame.seq;
sendFrame(ARControlConnection::Acknowledge, ARNET_C2D_NAVDATA_ACK_ID, payload.constData(), payload.size());
}
// Decode command header.
quint8 project = static_cast<quint8>(frame.payload[0]);
quint8 klass = static_cast<quint8>(frame.payload[1]);
quint16 id = qFromLittleEndian(static_cast<quint16>(frame.payload[2]));
const char *data = frame.payload.data() + 4;
// Resolve command meta-type information.
ARCommandInfo *command = d->commands->find(project, klass, id);
if(command == NULL)
{
WARNING_T(QString("Unrecognised command: %1 %2 %3")
.arg(project)
.arg(klass)
.arg(id));
return;
}
// Use command codec to decode command parameters.
QVariantMap params = d->codec->decode(command, data);
DEBUG_T(QString("Decoded Command %1 %2 %3").arg(command->klass->project).arg(command->klass->name).arg(command->name));
d->controller->onCommandReceived(*command, params);
}
void ARControlConnection::onVideoData(const ARControlFrame &frame)
{
TRACE
Q_D(ARControlConnection);
quint16 frameNumber = qFromLittleEndian(*((quint16*)(frame.payload.constData())));
quint8 frameFlags = static_cast<quint8>(frame.payload[2]);
quint8 fragmentNumber = static_cast<quint8>(frame.payload[3]);
quint8 fragsPerFrame = static_cast<quint8>(frame.payload[4]);
if(frameNumber != d->frameNumber)
{
if(frameFlags == 0x01)
{
DEBUG_T(QString("Video Key Frame Header [%1] (%2 %3 %4)")
.arg(QString(QByteArray().append(frame.payload.constData(), 5).toHex()))
.arg(frameNumber)
.arg(fragmentNumber)
.arg(fragsPerFrame));
}
else
{
DEBUG_T(QString("Video Frame Header (%2 %3 %4)")
.arg(frameNumber)
.arg(fragmentNumber)
.arg(fragsPerFrame));
}
if(fragsPerFrame < 64)
{
d->hiAck = 0xffffffffffffffff;
d->loAck = 0xffffffffffffffff << fragsPerFrame;
}
else if(fragsPerFrame < 128)
{
d->hiAck = 0xffffffffffffffff << (fragsPerFrame - 64);
d->loAck = 0ll;
}
else
{
d->hiAck = 0ll;
d->loAck = 0ll;
}
d->frameNumber = frameNumber;
}
if(fragmentNumber < 64)
{
d->loAck |= (1ll << fragmentNumber);
}
else if(fragmentNumber < 128)
{
d->hiAck |= (1ll << (fragmentNumber - 64));
}
// Construct reply.
QByteArray payload(2 + 8 + 8, 0x00);
QDataStream datastream(&payload, QIODevice::WriteOnly);
if(!d->sequenceIds.contains(ARNET_C2D_VIDEO_ACK_ID))
d->sequenceIds.insert(ARNET_C2D_VIDEO_ACK_ID, 0x00);
datastream.setByteOrder(QDataStream::LittleEndian);
datastream
<< (quint16)frameNumber
<< (quint64)d->hiAck
<< (quint64)d->loAck;
sendFrame(ARControlConnection::LowLatencyData, ARNET_C2D_VIDEO_ACK_ID, payload.constData(), payload.size());
}
void ARControlConnection::onReadyRead()
{
Q_D(ARControlConnection);
while(d->d2c->hasPendingDatagrams())
{
QHostAddress remoteAddr;
quint16 remotePort;
// If we've not received enough data, then return and wait for more.
if(d->d2c->pendingDatagramSize() < ARNETWORK_FRAME_HEADER_SIZE) break;
QByteArray datagram(d->d2c->pendingDatagramSize(), 0x00);
quint32 offset = 0;
d->d2c->readDatagram(datagram.data(), datagram.size(), &remoteAddr, &remotePort);
while(offset < datagram.size())
{
ARControlFrame frame;
frame.type = static_cast<quint8>(datagram[offset + 0]);
frame.id = static_cast<quint8>(datagram[offset + 1]);
frame.seq = static_cast<quint8>(datagram[offset + 2]);
frame.size = qFromLittleEndian(*((quint32*)(datagram.constData() + offset + 3)));
// Copy datagram data to frame if the frame size is larger than just the header.
if(frame.size > ARNETWORK_FRAME_HEADER_SIZE)
{
const char *data = datagram.constData();
frame.payload.insert(0, data + offset + ARNETWORK_FRAME_HEADER_SIZE, frame.size - ARNETWORK_FRAME_HEADER_SIZE);
}
// Output comms debug info (if it's not a video data frame).
if(frame.id != ARNET_D2C_VIDEO_DATA_ID)
{
DEBUG_T(QString("<< %1:%2 [%3]")
.arg(remoteAddr.toString())
.arg(remotePort)
.arg(QString(datagram.toHex())));
}
// Process frame depending on buffer id.
if(frame.id == ARNET_D2C_PING_ID)
{
onPing(frame);
}
else if(frame.id == ARNET_D2C_EVENT_ID || frame.id == ARNET_D2C_NAVDATA_ID)
{
onNavdata(frame);
}
else if(frame.id == ARNET_D2C_VIDEO_DATA_ID)
{
onVideoData(frame);
}
else
{
WARNING_T(QString("Unhandled frame id: %1").arg(frame.id));
}
// Increment datagram offset to continue processing next frame.
offset += frame.size;
}
}
}
// Call @ every 200ms
void my_task_sync(void)
{
u8 t;
static u8 tTMR=0;
static u8 tLED=0;
static u8 tTMR_S=0;
#ifdef debug_task_200ms
DEBUG_T();
#endif
tTMR ++;
if (tTMR >=1)
{
tTMR=0;
tLED ++;
if (tLED>=32)
tLED=0;
if (tLED>16)
mLED[0] = (tLED ^ 0x1f);
else
mLED[0] = tLED ;
}
tTMR_S ++;
if (tTMR_S >=5)
{
// 1Second Task
tTMR_S = 0;
my_task_1s();
}
// Scan Input
t=KEY_Scan();//得到键值
if(t)
{
switch(t)
{
case 1:
mFR += 0x200;
mFR &= 0x0fff;
// LED0=!LED0;
LCD_ShowLineEx(LN_KEY, " KEY : 1");
break;
case 2:
mFR += 0x10;
mFR &= 0x0fff;
// LED1=!LED1;
LCD_ShowLineEx(LN_KEY, " KEY : 2");
break;
case 3:
// LED0=!LED0;
// LED1=!LED1;
LCD_ShowLineEx(LN_KEY, " KEY : 3");
mFlowCounter.state ++;
if (mFlowCounter.state>=3)
mFlowCounter.state=0;
break;
}
}
}
void debugMessage(const QString &message)
{
DEBUG_T(QString("OfonoVoiceCallProvider(") + ofonoModem->modemPath() + "): " + message);
}
int CMakeSip::addAuth(char *un, char *pwd, SIP_MSG *psMsg)
{
HASHHEX HA1;
HASHHEX HA2 = "";
HASHHEX respHex32;
HDR_AUT * hdrAuth=&psMsg->hdrProxyAuthen;
if (psMsg==NULL || un==NULL || pwd==NULL)
return -1;
DEBUG_T(0,"add proxy auth");
if(psMsg->sipHdr.dstrStatusCode.uiVal==401)
{
hdrAuth=&sMsg->hdrWWWAuth;
}
char *pCNonce=hdrAuth->dstrQOP.strVal?(char *)"6ad34fc1":NULL;
char *pNC=(char *)"00000001";
if (false==DigestCalcHA1(hdrAuth,(unsigned char *)un,(unsigned char *)pwd, HA1))
return -2;
if (false==DigestCalcResponse(HA1, hdrAuth,psMsg->hdrCSeq.uiMethodID,pCNonce,pNC, &strDstAddr, HA2, respHex32))
return -2;
if(psMsg->sipHdr.dstrStatusCode.uiVal==407)
{
ADD_STR(buf,uiLen,"Proxy-Authorization: ");
}
else
{
ADD_STR(buf,uiLen,"Authorization: ");
}
//"
//TODO add MD5 or MD5 sess
uiLen+= (unsigned int)sprintf(buf+uiLen,
"Digest username=\"%s\", realm=\"%.*s\", nonce=\"%.*s\", uri=\"%.*s\", response=\"%.*s\""
,un
,D_STR(hdrAuth->dstrRealm)
,D_STR(hdrAuth->dstrNonce)
,strDstAddr.len,strDstAddr.s
,32,respHex32
);
if(hdrAuth->iFlag & 4)
{
uiLen+= (unsigned int)sprintf(buf+uiLen,", algorithm=%.*s",D_STR(hdrAuth->dstrAlgo));
}
if(hdrAuth->iFlag & 128)
{
uiLen+= (unsigned int)sprintf(buf+uiLen,", opaque=\"%.*s\"",D_STR(hdrAuth->dstrOpaque));
}
if(hdrAuth->iFlag & 32)
{
uiLen+= (unsigned int)sprintf(buf+uiLen,", qop=\"%.*s\"",D_STR(hdrAuth->dstrQOP));
if(pNC)
{
uiLen+= (unsigned int)sprintf(buf+uiLen,", nc=%s",pNC);
}
if(pCNonce)
{
uiLen+= (unsigned int)sprintf(buf+uiLen,", cnonce=%s",pCNonce);
}
}
ADD_CRLF(buf,uiLen);
return 0;
}
NODE* CParseXml::parseXml(char * inbuf, int iBufLen)
{
char * buf = inbuf;
flag = 0;
curFlag = 0;
nextFlag = F_FIRST;
NODE *root = NULL;
memset(&nextStr, 0, sizeof(nextStr));
memset(&curStr, 0, sizeof(curStr));
// Buf (from inbuf) is always terminated with 'nul' bytes
if(iBufLen == 0)
iBufLen = strlen(buf);
end = buf + iBufLen;
cur = buf;
flag = 0; //F_NODE_NOPEN;
// skip leeading whitespace chars
while(*cur == ' ' || *cur == '\t' || *cur == '\n' || *cur == '\r')
cur++;
if (cur[0] != '<') {
errorXML("invalid start of document");
return root;
}
//skip <? ?>
if (cur[1] == '?') {
// TODO _TEST_XML;
cur += 2;
while (cur[0] != '?' && cur[1] != '>') {
if (*cur < ' ') {
errorXML("<? ?>");
return root;
}
cur++;
}
cur += 2;
}
while (*cur == ' ' || *cur == '\t' || *cur == '\n' || *cur == '\r')
cur++;
if (cur[0] == '<' && cur[1] == '!') {
cur += 2;
while(*cur >= ' ' && *cur != '>')
cur++;
cur++;
}
while (*cur == ' ' || *cur == '\t' || *cur == '\n' || *cur == '\r')
cur++;
if (rootNode)
freeNode(rootNode);
root = node();
rootNode = root;
if (!root) {
DEBUG_T(0,"root is null");
return root;
}
reverse(&root, NULL);
terminateStrings(root);
rootNode = root;
return rootNode;
}
