git.maemo.org Git - mardrone/blob - mardrone/dronelib/dronecontrol.cpp

   1 /*==================================================================
   2   !
   3   !  mardrone application AR-Drone for MeeGo
   4
   5   ! Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
   6   ! All rights reserved.
   7   !
   8   !  Author:Kate Alhola  kate.alhola@nokia.com
   9   !
  10   ! GNU Lesser General Public License Usage
  11   ! This file may be used under the terms of the GNU Lesser
  12   ! General Public License version 2.1 as published by the Free Software
  13   ! Foundation and appearing in the file LICENSE.LGPL included in the
  14   ! packaging of this file.  Please review the following information to
  15   ! ensure the GNU Lesser General Public License version 2.1 requirements
  16   ! will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
  17   !
  18   !
  19   !
  20   *===================================================================*/
  21 #include <stdio.h>
  22 #include <sys/types.h>
  23 #include <sys/socket.h>
  24 #include <netinet/in.h>
  25 #include <arpa/inet.h>
  26 #include "dronecontrol.h"
  27 #include <QDebug>
  28 #include <QNetworkConfigurationManager>
  29 #include <QNetworkSession>
  30 #include <QList>
  31 #include <QDateTime>
  32 #include "navdata.h"
  33 #include "ardrone_api.h"
  34
  35 DroneControl::DroneControl():QObject()
  36 {
  37     qDebug() << "DroneControl::DroneControl";
  38
  39
  40
  41     droneSettings=new QSettings("katix.org","mardrone");
  42     m_ctrlActive=false;
  43     m_pitch=0;
  44     m_roll=0;
  45     m_yaw=0;
  46     m_vv=0;
  47     m_connected=FALSE;
  48     m_useAccel=droneSettings->value("useAccel",false).toBool();
  49     m_useJoyStick=droneSettings->value("useJoyStick",false).toBool();
  50     m_ctrlTrsh=droneSettings->value("ctrlTreshold",10.0).toFloat();
  51     droneHost.setAddress(confDroneIp());
  52     droneThread=new DroneThread(this,droneHost);
  53     connect(droneThread->navData(),SIGNAL(navDataUpdated()),this,SLOT(navDataUpdated()));
  54     connect(droneThread->navData(),SIGNAL(stateUpdated()),this,SLOT(statusUpdated()));
  55 #ifdef QT_SENSORS
  56     rSensor=new QRotationSensor();
  57     rSensor->start();
  58     connect(rSensor,SIGNAL(readingChanged()),this,SLOT(rotationReadingsChanged()));
  59 #endif
  60
  61     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
  62
  63 }
  64
  65 void DroneControl::axisValueChanged(int axis, int value)
  66 {
  67 #ifdef HAVE_SDL
  68     qDebug()<< "DroneControl::axisValueChanged(" << axis << value << ")";
  69 #endif
  70
  71 };
  72 void DroneControl::rotationReadingsChanged()
  73 {
  74 #ifdef QT_SENSORS
  75     if(m_useAccel && m_ctrlActive) {
  76         m_pitch=rSensor->reading()->x()-m_rotRefX;
  77         m_roll =rSensor->reading()->y()-m_rotRefY;
  78         m_pitch=(fabs(m_pitch)<m_ctrlTrsh ) ? 0:(m_pitch>0 ? m_pitch-m_ctrlTrsh:m_pitch+m_ctrlTrsh);
  79         m_roll =(fabs(m_roll )<m_ctrlTrsh ) ? 0:(m_roll>0  ? m_roll- m_ctrlTrsh:m_roll+ m_ctrlTrsh);
  80         droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
  81         emit pitchRollChanged();
  82         //m_rotRefZ=rSensor->reading()->z();
  83         //qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
  84     }
  85 #endif
  86 }
  87
  88 void DroneControl::setConnected(bool val_)
  89 {
  90     m_connected=val_;
  91     emit connectedChanged();
  92 }
  93 void DroneControl::navDataUpdated()
  94 {
  95     emit navDataChanged();
  96
  97 }
  98 void DroneControl::statusUpdated()
  99 {
 100     emit statusChanged();
 101
 102 }
 103
 104 void DroneControl::setPitch(float val_)
 105 {
 106     m_pitch=val_;
 107 //    qDebug() << "setPitch=" << val_;
 108     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 109     emit pitchRollChanged();
 110 };
 111 float DroneControl::pitch() {return m_pitch;};
 112
 113 void DroneControl::setRoll(float val_)
 114 {
 115     m_roll=val_;
 116 //    qDebug() << "setRoll=" << val_;
 117     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 118         emit pitchRollChanged();
 119 };
 120 float DroneControl::roll() {return m_roll;};
 121 void DroneControl::setYaw(float val) {
 122     m_yaw=val;
 123  //   qDebug() << "setYaw=" << val;
 124     if(m_useAccel && m_ctrlActive) m_yaw=(fabs(m_yaw)<(m_ctrlTrsh*2) ) ? 0:(m_yaw>0 ? m_yaw-(m_ctrlTrsh*2):m_yaw+(m_ctrlTrsh*2));
 125     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 126         emit pitchRollChanged();
 127 };
 128 float DroneControl::yaw() {return m_yaw;};
 129 void DroneControl::setVVelocity(float val) {
 130     m_vv=val;
 131     if(m_useAccel && m_ctrlActive) m_vv=(fabs(m_vv)<(m_ctrlTrsh*2) ) ? 0:(m_vv>0? m_vv-(m_ctrlTrsh*2):m_vv+(m_ctrlTrsh*2));
 132 //    qDebug() << "setVv=" << val_;
 133     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 134     emit pitchRollChanged();
 135 };
 136 float DroneControl::vVelocity()             {return m_vv;};
 137 void  DroneControl::setAltitude(float val_) {m_altitude=val_;};
 138 float DroneControl::altitude()              {return m_altitude;};
 139 void  DroneControl::setEnabled(int val_)    { m_enabled=val_;};
 140 int   DroneControl::enabled()               {return m_enabled;};
 141 bool  DroneControl::ctrlActive()            { return m_ctrlActive;};
 142 bool  DroneControl::emergency()             {return m_emergency;};
 143
 144 void  DroneControl::setCtrlActive(bool val_)
 145 {
 146 #if QT_SENSORS
 147     if(val_ && !m_ctrlActive) { // Ctrl switched to active
 148         m_rotRefX=rSensor->reading()->x();
 149         m_rotRefY=rSensor->reading()->y();
 150         m_rotRefZ=rSensor->reading()->z();
 151         qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
 152     }
 153 #endif
 154     m_ctrlActive=val_;
 155     if(!m_ctrlActive) { m_pitch=0;m_roll=0; };
 156     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 157     emit pitchRollChanged();
 158
 159 };
 160
 161
 162 void DroneControl::setFly(bool val_)
 163 {
 164     m_fly=val_;
 165     qDebug() << "setFly=" << val_;
 166     droneThread->setFly(m_fly);
 167 };
 168 bool DroneControl::fly() {return m_fly;};
 169 void DroneControl::setEmergency(bool val_)
 170 {
 171     m_emergency=val_;
 172     qDebug() << "setEmergency=" << val_;
 173     droneThread->setEmergency(m_emergency);
 174 };
 175
 176 bool  DroneControl::recordLog()
 177 {
 178     return droneThread? droneThread->getRecordLog():false;
 179 };
 180
 181 void DroneControl::setRecordLog(bool val_)
 182 {
 183    if(droneThread) droneThread->setRecordLog(val_);
 184 };
 185 QString DroneControl::logFileName()
 186 {
 187     return droneThread? droneThread->getLogFileName():"";
 188 };
 189 void DroneControl::setLogFileName(QString val_)
 190 {
 191    if(droneThread) droneThread->setLogFileName(val_);
 192 };
 193 unsigned int   DroneControl::logSeq()
 194 {
 195     return droneThread? droneThread->getLogSeq():0;
 196 };
 197
 198 void DroneControl::setLogSeq(int val_)
 199 {
 200     if(droneThread) droneThread->setLogSeq(val_);
 201 };
 202
 203
 204
 205 // Getters to drone actual valyes sent by drone
 206 float DroneControl::droneAltitude() { return droneThread->navData()->altitude; };
 207 float DroneControl::dronePitch()    { return droneThread->navData()->pitch; };
 208 float DroneControl::droneRoll()     { return droneThread->navData()->roll; };
 209 float DroneControl::droneYaw()      { return droneThread->navData()->yaw; };
 210 float DroneControl::droneSpeed()    { return droneThread->navData()->vx;};
 211 float DroneControl::droneVBat()     { return droneThread->navData()->vbat;};
 212 QString DroneControl::decodedStatus() { return droneThread->navData()->decodedState; };
 213 int   DroneControl::pwm_motor1()    { return droneThread->navData()->pwm_motor1; };
 214 int   DroneControl::pwm_motor2()    { return droneThread->navData()->pwm_motor2; };
 215 int   DroneControl::pwm_motor3()    { return droneThread->navData()->pwm_motor3;};
 216 int   DroneControl::pwm_motor4()      { return droneThread->navData()->pwm_motor4;};
 217
 218
 219 /*
 220
 221   Setters and Getters for Config variables
 222
 223   */
 224
 225
 226
 227 QString DroneControl::confDroneIp()
 228 {
 229     qDebug() << "confDroneIp:" << droneSettings->value("droneIp","192.168.1.1").toString();
 230     return droneSettings->value("droneIp","192.168.1.1").toString();
 231 };
 232 void DroneControl::setConfDroneIp(QString ip)
 233 {
 234     qDebug() << "setConfDroneIp:" << ip;
 235   droneSettings->setValue("droneIp",ip);
 236   emit configChanged();
 237 };
 238 QString DroneControl::confActiveUI()
 239 {
 240     qDebug() << "confActiveUI:" << droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
 241     return droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
 242 };
 243 void DroneControl::setConfActiveUI(QString ui)
 244 {
 245     qDebug() << "setConfActiveUI:" << ui;
 246   droneSettings->setValue("activeUI",ui);
 247   emit configChanged();
 248 };
 249 bool DroneControl::confShowDebug()
 250 {
 251 return droneSettings->value("showDebug",true).toBool();
 252 };
 253 void DroneControl::setConfShowDebug(bool val)
 254 {
 255     droneSettings->setValue("showDebug",val);
 256       emit configChanged();
 257 };
 258 bool DroneControl::confShowJSIndicators()
 259 {
 260 return droneSettings->value("showJSIndicators",true).toBool();
 261 };
 262 void DroneControl::setConfShowJSIndicators(bool val)
 263 {
 264     droneSettings->setValue("showJSIndicators",val);
 265       emit configChanged();
 266 };
 267 bool DroneControl::confShowHorizon()
 268 {
 269     return droneSettings->value("showHorizon",true).toBool();
 270 };
 271 void DroneControl::setConfShowHorizon(bool val)
 272 {
 273     droneSettings->setValue("showHorizon",val);
 274     emit configChanged();
 275 };
 276 bool DroneControl::confShowGauges()
 277 {
 278     return droneSettings->value("showGuges",true).toBool();
 279 };
 280 void DroneControl::setConfShowGauges(bool val)
 281 {
 282     droneSettings->setValue("showGauges",val);
 283     emit configChanged();
 284 };
 285
 286 bool DroneControl::confUseAccel()
 287 {
 288     //return droneSettings->value("useAccel",false).toBool();
 289     return m_useAccel; // return cached value
 290 };
 291 void DroneControl::setConfUseAccel(bool val)
 292 {
 293     droneSettings->setValue("useAccel",val);
 294     m_useAccel=val;
 295     emit configChanged();
 296 };
 297 bool DroneControl::confUseJoyStick()
 298 {
 299     return droneSettings->value("useJoyStick",false).toBool();
 300 };
 301 void DroneControl::setConfUseJoyStick(bool val)
 302 {
 303     droneSettings->setValue("useJoyStick",val);
 304     m_useJoyStick=val;
 305     emit configChanged();
 306 };
 307
 308 int DroneControl::confSimuMode()
 309 {
 310     return droneSettings->value("SimuMode",true).toInt();
 311 };
 312 void DroneControl::setConfSimuMode(int val)
 313 {
 314     droneSettings->setValue("SimuMode",val);
 315     emit configChanged();
 316 };
 317
 318 bool DroneControl::confFullScreen()
 319 {
 320     return droneSettings->value("fullScreen",true).toBool();
 321 };
 322 void DroneControl::setConfFullScreen(bool val)
 323 {
 324     droneSettings->setValue("fullScreen",val);
 325     emit configChanged();
 326 };
 327
 328
 329 float DroneControl::confForwardGain()
 330 {
 331     return droneSettings->value("forwardGain",1.0).toFloat();
 332 };
 333 void DroneControl::setConfForwardGain(float val)
 334 {
 335     droneSettings->setValue("forwardGain",val);
 336     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 337     emit configChanged();
 338 };
 339 float DroneControl::confCtrlTrsh()
 340 {
 341     return m_ctrlTrsh;
 342 };
 343 void DroneControl::setConfCtrlTrsh(float val)
 344 {
 345     droneSettings->setValue("ctrlTreshold",val);
 346     m_ctrlTrsh=val;
 347     emit configChanged();
 348 };
 349
 350 float DroneControl::confBackwardGain()
 351 {
 352     return droneSettings->value("backwardGain",1.0).toFloat();
 353 };
 354 void DroneControl::setConfBackwardGain(float val)
 355 {
 356     droneSettings->setValue("backwardGain",val);
 357     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 358     emit configChanged();
 359 };
 360
 361 float DroneControl::confLeftGain()
 362 {
 363     return droneSettings->value("leftGain",1.0).toFloat();
 364 };
 365 void DroneControl::setConfLeftGain(float val)
 366 {
 367     droneSettings->setValue("leftGain",val);
 368     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 369     emit configChanged();
 370 };
 371 float DroneControl::confRightGain()
 372 {
 373     return droneSettings->value("rightGain",1.0).toFloat();
 374 };
 375 void DroneControl::setConfRightGain(float val)
 376 {
 377     droneSettings->setValue("rightGain",val);
 378     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 379     emit configChanged();
 380 };
 381
 382 QString DroneControl::errorString()
 383 {
 384     return m_errorString;
 385 };
 386 void DroneControl::setErrorString(QString val_)
 387 {
 388     m_errorString=val_;
 389 };
 390
 391 /*=================================================
 392
 393  DroneThread class starts here
 394
 395 ==================================================*/
 396
 397 DroneThread::DroneThread(DroneControl *parentp,QHostAddress host)
 398 {
 399
 400     struct ip_mreq imreq;
 401     QString my_ip;
 402     qDebug() << "DroneThread::DroneThread";
 403     my_ip="192.168.1.2";
 404         stopped=false;
 405         state=notInitialized;
 406         parent=parentp;
 407         navSock=new QUdpSocket();
 408         QList<QNetworkConfiguration> netconfs=QNetworkConfigurationManager().allConfigurations();
 409         foreach (QNetworkConfiguration np,netconfs) {
 410             qDebug() << "network Confifuration name " << np.name() << np.bearerName() << np.bearerTypeName();
 411             QNetworkSession ns(np);
 412             if(ns.interface().addressEntries().empty())
 413                 qDebug() << "network session " << ns.interface().humanReadableName() << "**NotConfig**";
 414             else {
 415                 qDebug() << "network session " << ns.interface().humanReadableName() <<   ns.interface().addressEntries().first().ip();
 416                 // Ubuntu may give wlan0 as "Ethernet"
 417                 if((np.bearerName()==QString("WLAN")) || (ns.interface().humanReadableName()==QString("wlan0")) ) {
 418                    my_ip=ns.interface().addressEntries().first().ip().toString();
 419                    qDebug() << "My IP is " << my_ip;
 420             }
 421             }
 422         }
 423         if(!navSock->bind(QHostAddress::Any,5554))
 424             qDebug() << "DroneThread::DroneThread Can't bind to port: 5554" << navSock->errorString() ;
 425 //#if 0
 426         // Qt 4.7.x
 427         imreq.imr_multiaddr.s_addr=inet_addr("224.1.1.1");
 428         imreq.imr_interface.s_addr=inet_addr(my_ip.toAscii());
 429         setsockopt(navSock->socketDescriptor(),IPPROTO_IP,IP_ADD_MEMBERSHIP,&imreq,sizeof(imreq));
 430
 431 #if QT_VERSION >= 0x040800
 432         // Qt 4.8
 433         if(!navSock->joinMulticastGroup(QHostAddress("224.1.1.1")))
 434             qDebug() << "can't join multicast Group 224.1.1.1" << navSock->errorString();
 435 #endif
 436         cmdSock=new QUdpSocket();
 437         cmdSock->bind(QHostAddress::Any,5556);
 438         droneHost=host;
 439         recordLog=false;
 440         logFile=NULL;
 441         logFileName="";
 442         seq=1;
 443         m_pitch=0;
 444         m_roll=0;
 445         m_yaw=0;
 446         m_vv=0;
 447         m_fgain=1.0;
 448         m_bgain=1.0;
 449         m_rgain=1.0;
 450         m_lgain=1.0;
 451         noreply=0;
 452         start();
 453
 454 };
 455
 456 void DroneThread::setFly(bool fly)
 457 {
 458     if(state==ready) {
 459         m_fly=fly;
 460         state=flying;
 461         stateTimer->setInterval(50); // More frequent updates
 462         sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
 463
 464     }
 465      if(state==flying) {
 466          if(!fly) {
 467              stateTimer->setInterval(200); // Less frequent updates
 468              state=ready;
 469          }
 470          m_fly=fly;
 471      }
 472      sendCmd(QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28) +(state==flying ? 1<<9:0)));
 473 };
 474
 475 void DroneThread::setRecordLog(bool log)
 476 {
 477     qDebug() << "DroneThread::setRecordLog" <<  log;
 478     QDateTime fileDate=QDateTime::currentDateTime();
 479     if(!recordLog && log) {
 480         if(logFileName.isEmpty())  logFileName="dronerec"+fileDate.toString("yyyyMMddhhmm");
 481         qDebug() << "logFileName=" <<  logFileName;
 482         //emit frameSeqChanged();
 483     }
 484     recordLog=log;
 485     if(!recordLog && logFile) { // Stop recording
 486         logFile->close();
 487         logFile->~QFile();
 488         logFile=NULL;
 489         logFileName="";
 490     }
 491
 492
 493 }
 494
 495 bool DroneThread::getRecordLog()
 496 {
 497     return recordLog;
 498 }
 499
 500 QString DroneThread::getLogFileName()
 501 {
 502     return logFileName;
 503 }
 504
 505 void DroneThread::setLogFileName(QString name)
 506 {
 507     logFileName=name;
 508 }
 509
 510 unsigned int DroneThread::getLogSeq()
 511 {
 512     return logSeq;
 513 }
 514
 515 void DroneThread::setLogSeq(unsigned int val)
 516 {
 517     logSeq=val;
 518 }
 519
 520 void DroneThread::setEmergency(bool emg)
 521 {
 522     m_emergency=emg;
 523     sendCmd(QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28) + (m_emergency ? 1<<8:0)));
 524     // if(m_emergency==1)
 525         state=ready;
 526 }   ;
 527 void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
 528 {
 529     m_pitch=pitch*m_fgain;
 530     m_roll=roll*m_rgain;
 531     m_yaw=yaw;
 532     m_vv=vv;
 533 //    qDebug()  << QString("pitch=%1 roll=%2 yaw=%3 vv=%4").arg(m_pitch,3,'F',2).arg(m_roll,3,'F',2).arg(m_yaw,3,'F',2).arg(m_vv,3,'F',2);
 534 };
 535
 536 void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
 537 {
 538     m_fgain=fgain;
 539     m_bgain=bgain;
 540     m_rgain=rgain;
 541     m_lgain=lgain;
 542 };
 543
 544 void DroneThread::sendCmd(QString cmd)
 545 {
 546     QByteArray dgram;
 547     QString seqCmd=cmd;
 548     noreply++;
 549     if(cmd.contains("%")) seqCmd=cmd.arg(seq++);
 550     dgram=seqCmd.toLatin1();
 551     cmdSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5556);
 552     seqCmd.chop(1); // Remove training cr
 553  //   qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
 554 }
 555
 556 void DroneThread::navDataReady()
 557 {
 558    qint64 l;
 559    char buf[2048];
 560    QHostAddress host;
 561    quint16 port;
 562    QString cmd;
 563    bool ack;
 564    noreply=0; // We got a reply
 565    cmd="";ack=false;
 566    while(navSock->hasPendingDatagrams()) l=navSock->readDatagram(buf,sizeof(buf),&host,&port);
 567 //   qDebug() << "DroneThread::navDataReady state=" << state <<" l=" << l << "read=" << buf << "from"  << host ;
 568    nd.parseRawNavData((char *)&buf,l);
 569    if(nd.state& (ARDRONE_NAVDATA_BOOTSTRAP)) state=notInitialized;
 570    if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
 571        cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 572    if((nd.state& (ARDRONE_COMMAND_MASK ))) {
 573        cmd+=QString("AT*CTRL=%1,%2,0\r").arg(seq++).arg(ACK_CONTROL_MODE);
 574        ack=true;
 575    };
 576     switch(state) {
 577     case notInitialized:
 578       cmd+=QString("AT*CONFIG=%1,\"general:navdata_demo\",\"TRUE\"\r").arg(seq++);
 579   //    cmd+=QString("AT*CONFIG=%1,\"general:navdata_options\",\"%3\"\r").arg(seq++).arg(NAVDATA_OPTION_FULL_MASK);
 580   //    cmd+=QString("AT*CONFIG=%1,\"video:codec\",\"%2\"\r").arg(seq++).arg(0x20);
 581         state=bootstrap;
 582         retry=10;
 583         qDebug() << "Connected to drone" << seq << navData()->decodedState;
 584         parent->setConnected(TRUE); // We are connected to drone
 585         break;
 586     case bootstrap:
 587         if(ack) state=initialized;
 588         else
 589           if(--retry<=0) state=notInitialized;
 590         break;
 591     case initialized:
 592         cmd+=QString("AT*CTRL=%1,0\r").arg(seq++);
 593         cmd+=QString("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r").arg(seq++);
 594         state=ready;
 595         break;
 596     case ready:
 597         break;
 598     }
 599     if(!cmd.isEmpty())sendCmd(cmd); // don't send null cmd
 600 }
 601
 602 void DroneThread::sendNav(QString cmd)
 603 {
 604     QByteArray dgram;
 605     dgram=cmd.toLatin1();
 606 //    qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
 607     navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
 608 }
 609
 610
 611
 612 void DroneThread::run()
 613 {
 614   qDebug() << "DroneThread::DroneThread";
 615     stateTimer=new QTimer(this);
 616     connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
 617     connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
 618     stateTimer->start(100);
 619     while(!stopped) {
 620         exec();
 621     }
 622
 623 }
 624
 625 void DroneThread::timer()
 626 {
 627    QString cmd;
 628   //  qDebug() << "thread Timer";
 629    noreply++;
 630     if(noreply>50 )  {
 631         cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 632         navData()->setState("**TimeOut**");
 633    //     qDebug("Timeout");
 634         noreply=0;
 635     };
 636     switch(state) {
 637         case notInitialized:
 638             sendNav("AT\r");
 639          //    cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 640             break;
 641     case bootstrap:
 642     case initialized:
 643         cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 644        break;
 645     case ready:
 646             cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 647             break;
 648     case flying: {
 649             float_or_int_t _pitch,_roll,_yaw,_vv;
 650             int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
 651
 652
 653             _pitch.f=m_pitch;
 654             _roll.f=m_roll;
 655             _yaw.f=m_yaw;
 656             _vv.f=m_vv;
 657 //            qDebug()  << QString("AT*PCMD=%1,0,%2,%3,%4,%5\r").arg(seq).arg(_roll.f,3,'F',2).arg(_pitch.f,3,'F',2).arg(_vv.f,3,'F',2).arg(_yaw.f,3,'F',2);
 658 //            qDebug() << QString("AT*PCMD=%1,0,%2,%3,%4,%5\r").arg(seq).arg(_roll.i,8,16).arg(_pitch.i,8,16).arg(_vv.i,8,16).arg(_yaw.i,8,16);
 659             cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 660             cmd+=QString("AT*PCMD=%1,%2,%3,%4,%5,%6\r").arg(seq++).arg(r).arg(_roll.i).arg(_pitch.i).arg(_vv.i).arg(_yaw.i);
 661
 662
 663             break;
 664         }
 665 }
 666     if(!cmd.isEmpty())sendCmd(cmd);
 667 }
 668
 669
 670