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() << "Cant open any: 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 //#else
 431         // Qt 4.8
 432         if(!navSock->joinMulticastGroup(QHostAddress("224.1.1.1")))
 433             qDebug() << "can't join multicast Group 224.1.1.1" << navSock->errorString();
 434 //#endif
 435         cmdSock=new QUdpSocket();
 436         cmdSock->bind(QHostAddress::Any,5556);
 437         droneHost=host;
 438         recordLog=false;
 439         logFile=NULL;
 440         logFileName="";
 441         seq=1;
 442         m_pitch=0;
 443         m_roll=0;
 444         m_yaw=0;
 445         m_vv=0;
 446         m_fgain=1.0;
 447         m_bgain=1.0;
 448         m_rgain=1.0;
 449         m_lgain=1.0;
 450         noreply=0;
 451         start();
 452
 453 };
 454
 455 void DroneThread::setFly(bool fly)
 456 {
 457     if(state==ready) {
 458         m_fly=fly;
 459         state=flying;
 460         stateTimer->setInterval(50); // More frequent updates
 461         sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
 462
 463     }
 464      if(state==flying) {
 465          if(!fly) {
 466              stateTimer->setInterval(200); // Less frequent updates
 467              state=ready;
 468          }
 469          m_fly=fly;
 470      }
 471      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)));
 472 };
 473
 474 void DroneThread::setRecordLog(bool log)
 475 {
 476     qDebug() << "DroneThread::setRecordLog" <<  log;
 477     QDateTime fileDate=QDateTime::currentDateTime();
 478     if(!recordLog && log) {
 479         if(logFileName.isEmpty())  logFileName="dronerec"+fileDate.toString("yyyyMMddhhmm");
 480         qDebug() << "logFileName=" <<  logFileName;
 481         //emit frameSeqChanged();
 482     }
 483     recordLog=log;
 484     if(!recordLog && logFile) { // Stop recording
 485         logFile->close();
 486         logFile->~QFile();
 487         logFile=NULL;
 488         logFileName="";
 489     }
 490
 491
 492 }
 493
 494 bool DroneThread::getRecordLog()
 495 {
 496     return recordLog;
 497 }
 498
 499 QString DroneThread::getLogFileName()
 500 {
 501     return logFileName;
 502 }
 503
 504 void DroneThread::setLogFileName(QString name)
 505 {
 506     logFileName=name;
 507 }
 508
 509 unsigned int DroneThread::getLogSeq()
 510 {
 511     return logSeq;
 512 }
 513
 514 void DroneThread::setLogSeq(unsigned int val)
 515 {
 516     logSeq=val;
 517 }
 518
 519 void DroneThread::setEmergency(bool emg)
 520 {
 521     m_emergency=emg;
 522     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)));
 523     // if(m_emergency==1)
 524         state=ready;
 525 }   ;
 526 void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
 527 {
 528     m_pitch=pitch*m_fgain;
 529     m_roll=roll*m_rgain;
 530     m_yaw=yaw;
 531     m_vv=vv;
 532 //    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);
 533 };
 534
 535 void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
 536 {
 537     m_fgain=fgain;
 538     m_bgain=bgain;
 539     m_rgain=rgain;
 540     m_lgain=lgain;
 541 };
 542
 543 void DroneThread::sendCmd(QString cmd)
 544 {
 545     QByteArray dgram;
 546     QString seqCmd=cmd;
 547     noreply++;
 548     if(cmd.contains("%")) seqCmd=cmd.arg(seq++);
 549     dgram=seqCmd.toLatin1();
 550     cmdSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5556);
 551     seqCmd.chop(1); // Remove training cr
 552  //   qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
 553 }
 554
 555 void DroneThread::navDataReady()
 556 {
 557    qint64 l;
 558    char buf[2048];
 559    QHostAddress host;
 560    quint16 port;
 561    QString cmd;
 562    bool ack;
 563    noreply=0; // We got a reply
 564    cmd="";ack=false;
 565    while(navSock->hasPendingDatagrams()) l=navSock->readDatagram(buf,sizeof(buf),&host,&port);
 566 //   qDebug() << "DroneThread::navDataReady state=" << state <<" l=" << l << "read=" << buf << "from"  << host ;
 567    nd.parseRawNavData((char *)&buf,l);
 568    if(nd.state& (ARDRONE_NAVDATA_BOOTSTRAP)) state=notInitialized;
 569    if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
 570        cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 571    if((nd.state& (ARDRONE_COMMAND_MASK ))) {
 572        cmd+=QString("AT*CTRL=%1,%2,0\r").arg(seq++).arg(ACK_CONTROL_MODE);
 573        ack=true;
 574    };
 575     switch(state) {
 576     case notInitialized:
 577       cmd+=QString("AT*CONFIG=%1,\"general:navdata_demo\",\"TRUE\"\r").arg(seq++);
 578   //    cmd+=QString("AT*CONFIG=%1,\"general:navdata_options\",\"%3\"\r").arg(seq++).arg(NAVDATA_OPTION_FULL_MASK);
 579   //    cmd+=QString("AT*CONFIG=%1,\"video:codec\",\"%2\"\r").arg(seq++).arg(0x20);
 580         state=bootstrap;
 581         retry=10;
 582         qDebug() << "Connected to drone" << seq << navData()->decodedState;
 583         parent->setConnected(TRUE); // We are connected to drone
 584         break;
 585     case bootstrap:
 586         if(ack) state=initialized;
 587         else
 588           if(--retry<=0) state=notInitialized;
 589         break;
 590     case initialized:
 591         cmd+=QString("AT*CTRL=%1,0\r").arg(seq++);
 592         cmd+=QString("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r").arg(seq++);
 593         state=ready;
 594         break;
 595     case ready:
 596         break;
 597     }
 598     if(!cmd.isEmpty())sendCmd(cmd); // don't send null cmd
 599 }
 600
 601 void DroneThread::sendNav(QString cmd)
 602 {
 603     QByteArray dgram;
 604     dgram=cmd.toLatin1();
 605 //    qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
 606     navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
 607 }
 608
 609
 610
 611 void DroneThread::run()
 612 {
 613   qDebug() << "DroneThread::DroneThread";
 614     stateTimer=new QTimer(this);
 615     connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
 616     connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
 617     stateTimer->start(100);
 618     while(!stopped) {
 619         exec();
 620     }
 621
 622 }
 623
 624 void DroneThread::timer()
 625 {
 626    QString cmd;
 627   //  qDebug() << "thread Timer";
 628    noreply++;
 629     if(noreply>50 )  {
 630         cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 631         navData()->setState("**TimeOut**");
 632    //     qDebug("Timeout");
 633         noreply=0;
 634     };
 635     switch(state) {
 636         case notInitialized:
 637             sendNav("AT\r");
 638          //    cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 639             break;
 640     case bootstrap:
 641     case initialized:
 642         cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 643        break;
 644     case ready:
 645             cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 646             break;
 647     case flying: {
 648             float_or_int_t _pitch,_roll,_yaw,_vv;
 649             int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
 650
 651
 652             _pitch.f=m_pitch;
 653             _roll.f=m_roll;
 654             _yaw.f=m_yaw;
 655             _vv.f=m_vv;
 656 //            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);
 657 //            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);
 658             cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 659             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);
 660
 661
 662             break;
 663         }
 664 }
 665     if(!cmd.isEmpty())sendCmd(cmd);
 666 }
 667
 668
 669