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 "navdata.h"
  32 #include "ardrone_api.h"
  33
  34 DroneControl::DroneControl():QObject()
  35 {
  36     qDebug() << "DroneControl::DroneControl";
  37
  38
  39
  40     droneSettings=new QSettings("katix.org","mardrone");
  41     m_ctrlActive=false;
  42     m_pitch=0;
  43     m_roll=0;
  44     m_yaw=0;
  45     m_vv=0;
  46     m_connected=FALSE;
  47     m_useAccel=droneSettings->value("useAccel",false).toBool();
  48     m_useJoyStick=droneSettings->value("useJoyStick",false).toBool();
  49     m_ctrlTrsh=droneSettings->value("ctrlTreshold",10.0).toFloat();
  50     droneHost.setAddress(confDroneIp());
  51     droneThread=new DroneThread(this,droneHost);
  52     connect(droneThread->navData(),SIGNAL(navDataUpdated()),this,SLOT(navDataUpdated()));
  53     connect(droneThread->navData(),SIGNAL(stateUpdated()),this,SLOT(statusUpdated()));
  54 #ifdef QT_SENSORS
  55     rSensor=new QRotationSensor();
  56     rSensor->start();
  57     connect(rSensor,SIGNAL(readingChanged()),this,SLOT(rotationReadingsChanged()));
  58 #endif
  59
  60     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
  61
  62 }
  63
  64 void DroneControl::axisValueChanged(int axis, int value)
  65 {
  66 #ifdef HAVE_SDL
  67     qDebug()<< "DroneControl::axisValueChanged(" << axis << value << ")";
  68 #endif
  69
  70 };
  71 void DroneControl::rotationReadingsChanged()
  72 {
  73 #ifdef QT_SENSORS
  74     if(m_useAccel && m_ctrlActive) {
  75         m_pitch=rSensor->reading()->x()-m_rotRefX;
  76         m_roll =rSensor->reading()->y()-m_rotRefY;
  77         m_pitch=(fabs(m_pitch)<m_ctrlTrsh ) ? 0:(m_pitch>0 ? m_pitch-m_ctrlTrsh:m_pitch+m_ctrlTrsh);
  78         m_roll =(fabs(m_roll )<m_ctrlTrsh ) ? 0:(m_roll>0  ? m_roll- m_ctrlTrsh:m_roll+ m_ctrlTrsh);
  79         droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
  80         emit pitchRollChanged();
  81         //m_rotRefZ=rSensor->reading()->z();
  82         //qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
  83     }
  84 #endif
  85 }
  86
  87 void DroneControl::setConnected(bool val_)
  88 {
  89     m_connected=val_;
  90     emit connectedChanged();
  91 }
  92 void DroneControl::navDataUpdated()
  93 {
  94     emit navDataChanged();
  95
  96 }
  97 void DroneControl::statusUpdated()
  98 {
  99     emit statusChanged();
 100
 101 }
 102
 103 void DroneControl::setPitch(float val_)
 104 {
 105     m_pitch=val_;
 106 //    qDebug() << "setPitch=" << val_;
 107     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 108     emit pitchRollChanged();
 109 };
 110 float DroneControl::pitch() {return m_pitch;};
 111
 112 void DroneControl::setRoll(float val_)
 113 {
 114     m_roll=val_;
 115 //    qDebug() << "setRoll=" << val_;
 116     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 117         emit pitchRollChanged();
 118 };
 119 float DroneControl::roll() {return m_roll;};
 120 void DroneControl::setYaw(float val) {
 121     m_yaw=val;
 122  //   qDebug() << "setYaw=" << val;
 123     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));
 124     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 125         emit pitchRollChanged();
 126 };
 127 float DroneControl::yaw() {return m_yaw;};
 128 void DroneControl::setVVelocity(float val) {
 129     m_vv=val;
 130     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));
 131 //    qDebug() << "setVv=" << val_;
 132     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 133     emit pitchRollChanged();
 134 };
 135 float DroneControl::vVelocity()             {return m_vv;};
 136 void  DroneControl::setAltitude(float val_) {m_altitude=val_;};
 137 float DroneControl::altitude()              {return m_altitude;};
 138 void  DroneControl::setEnabled(int val_)    { m_enabled=val_;};
 139 int   DroneControl::enabled()               {return m_enabled;};
 140 bool  DroneControl::ctrlActive()            { return m_ctrlActive;};
 141 bool  DroneControl::emergency()             {return m_emergency;};
 142
 143 void  DroneControl::setCtrlActive(bool val_)
 144 {
 145 #if QT_SENSORS
 146     if(val_ && !m_ctrlActive) { // Ctrl switched to active
 147         m_rotRefX=rSensor->reading()->x();
 148         m_rotRefY=rSensor->reading()->y();
 149         m_rotRefZ=rSensor->reading()->z();
 150         qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
 151     }
 152 #endif
 153     m_ctrlActive=val_;
 154     if(!m_ctrlActive) { m_pitch=0;m_roll=0; };
 155     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
 156     emit pitchRollChanged();
 157
 158 };
 159
 160
 161 void DroneControl::setFly(bool val_)
 162 {
 163     m_fly=val_;
 164     qDebug() << "setFly=" << val_;
 165     droneThread->setFly(m_fly);
 166 };
 167 bool DroneControl::fly() {return m_fly;};
 168 void DroneControl::setEmergency(bool val_)
 169 {
 170     m_emergency=val_;
 171     qDebug() << "setEmergency=" << val_;
 172     droneThread->setEmergency(m_emergency);
 173 };
 174
 175
 176
 177 // Getters to drone actual valyes sent by drone
 178 float DroneControl::droneAltitude() { return droneThread->navData()->altitude; };
 179 float DroneControl::dronePitch()    { return droneThread->navData()->pitch; };
 180 float DroneControl::droneRoll()     { return droneThread->navData()->roll; };
 181 float DroneControl::droneYaw()      { return droneThread->navData()->yaw; };
 182 float DroneControl::droneSpeed()    { return droneThread->navData()->vx;};
 183 float DroneControl::droneVBat()     { return droneThread->navData()->vbat;};
 184 QString DroneControl::decodedStatus() { return droneThread->navData()->decodedState; };
 185 int   DroneControl::pwm_motor1()    { return droneThread->navData()->pwm_motor1; };
 186 int   DroneControl::pwm_motor2()    { return droneThread->navData()->pwm_motor2; };
 187 int   DroneControl::pwm_motor3()    { return droneThread->navData()->pwm_motor3;};
 188 int   DroneControl::pwm_motor4()      { return droneThread->navData()->pwm_motor4;};
 189
 190 QString DroneControl::confDroneIp()
 191 {
 192     qDebug() << "confDroneIp:" << droneSettings->value("droneIp","192.168.1.1").toString();
 193     return droneSettings->value("droneIp","192.168.1.1").toString();
 194 };
 195 void DroneControl::setConfDroneIp(QString ip)
 196 {
 197     qDebug() << "setConfDroneIp:" << ip;
 198   droneSettings->setValue("droneIp",ip);
 199   emit configChanged();
 200 };
 201 QString DroneControl::confActiveUI()
 202 {
 203     qDebug() << "confActiveUI:" << droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
 204     return droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
 205 };
 206 void DroneControl::setConfActiveUI(QString ui)
 207 {
 208     qDebug() << "setConfActiveUI:" << ui;
 209   droneSettings->setValue("activeUI",ui);
 210   emit configChanged();
 211 };
 212 bool DroneControl::confShowDebug()
 213 {
 214 return droneSettings->value("showDebug",true).toBool();
 215 };
 216 void DroneControl::setConfShowDebug(bool val)
 217 {
 218     droneSettings->setValue("showDebug",val);
 219       emit configChanged();
 220 };
 221 bool DroneControl::confShowJSIndicators()
 222 {
 223 return droneSettings->value("showJSIndicators",true).toBool();
 224 };
 225 void DroneControl::setConfShowJSIndicators(bool val)
 226 {
 227     droneSettings->setValue("showJSIndicators",val);
 228       emit configChanged();
 229 };
 230 bool DroneControl::confShowHorizon()
 231 {
 232     return droneSettings->value("showHorizon",true).toBool();
 233 };
 234 void DroneControl::setConfShowHorizon(bool val)
 235 {
 236     droneSettings->setValue("showHorizon",val);
 237     emit configChanged();
 238 };
 239 bool DroneControl::confShowGauges()
 240 {
 241     return droneSettings->value("showGuges",true).toBool();
 242 };
 243 void DroneControl::setConfShowGauges(bool val)
 244 {
 245     droneSettings->setValue("showGauges",val);
 246     emit configChanged();
 247 };
 248
 249 bool DroneControl::confUseAccel()
 250 {
 251     //return droneSettings->value("useAccel",false).toBool();
 252     return m_useAccel; // return cached value
 253 };
 254 void DroneControl::setConfUseAccel(bool val)
 255 {
 256     droneSettings->setValue("useAccel",val);
 257     m_useAccel=val;
 258     emit configChanged();
 259 };
 260 bool DroneControl::confUseJoyStick()
 261 {
 262     return droneSettings->value("useJoyStick",false).toBool();
 263 };
 264 void DroneControl::setConfUseJoyStick(bool val)
 265 {
 266     droneSettings->setValue("useJoyStick",val);
 267     m_useJoyStick=val;
 268     emit configChanged();
 269 };
 270
 271 int DroneControl::confSimuMode()
 272 {
 273     return droneSettings->value("SimuMode",true).toInt();
 274 };
 275 void DroneControl::setConfSimuMode(int val)
 276 {
 277     droneSettings->setValue("SimuMode",val);
 278     emit configChanged();
 279 };
 280
 281 bool DroneControl::confFullScreen()
 282 {
 283     return droneSettings->value("fullScreen",true).toBool();
 284 };
 285 void DroneControl::setConfFullScreen(bool val)
 286 {
 287     droneSettings->setValue("fullScreen",val);
 288     emit configChanged();
 289 };
 290
 291
 292 float DroneControl::confForwardGain()
 293 {
 294     return droneSettings->value("forwardGain",1.0).toFloat();
 295 };
 296 void DroneControl::setConfForwardGain(float val)
 297 {
 298     droneSettings->setValue("forwardGain",val);
 299     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 300     emit configChanged();
 301 };
 302 float DroneControl::confCtrlTrsh()
 303 {
 304     return m_ctrlTrsh;
 305 };
 306 void DroneControl::setConfCtrlTrsh(float val)
 307 {
 308     droneSettings->setValue("ctrlTreshold",val);
 309     m_ctrlTrsh=val;
 310     emit configChanged();
 311 };
 312
 313 float DroneControl::confBackwardGain()
 314 {
 315     return droneSettings->value("backwardGain",1.0).toFloat();
 316 };
 317 void DroneControl::setConfBackwardGain(float val)
 318 {
 319     droneSettings->setValue("backwardGain",val);
 320     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 321     emit configChanged();
 322 };
 323
 324 float DroneControl::confLeftGain()
 325 {
 326     return droneSettings->value("leftGain",1.0).toFloat();
 327 };
 328 void DroneControl::setConfLeftGain(float val)
 329 {
 330     droneSettings->setValue("leftGain",val);
 331     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 332     emit configChanged();
 333 };
 334 float DroneControl::confRightGain()
 335 {
 336     return droneSettings->value("rightGain",1.0).toFloat();
 337 };
 338 void DroneControl::setConfRightGain(float val)
 339 {
 340     droneSettings->setValue("rightGain",val);
 341     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 342     emit configChanged();
 343 };
 344
 345 /*=================================================
 346
 347  DroneThread class starts here
 348
 349 ==================================================*/
 350
 351
 352 void DroneThread::setFly(bool fly)
 353 {
 354     if(state==ready) {
 355         m_fly=fly;
 356         state=flying;
 357         stateTimer->setInterval(50); // More frequent updates
 358         sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
 359
 360     }
 361      if(state==flying) {
 362          if(!fly) {
 363              stateTimer->setInterval(200); // Less frequent updates
 364              state=ready;
 365          }
 366          m_fly=fly;
 367      }
 368      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)));
 369 };
 370
 371 void DroneThread::setEmergency(bool emg)
 372 {
 373     m_emergency=emg;
 374     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)));
 375     // if(m_emergency==1)
 376         state=ready;
 377 }   ;
 378 void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
 379 {
 380     m_pitch=pitch*m_fgain;
 381     m_roll=roll*m_rgain;
 382     m_yaw=yaw;
 383     m_vv=vv;
 384 //    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);
 385 };
 386
 387 void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
 388 {
 389     m_fgain=fgain;
 390     m_bgain=bgain;
 391     m_rgain=rgain;
 392     m_lgain=lgain;
 393 };
 394
 395 void DroneThread::sendCmd(QString cmd)
 396 {
 397     QByteArray dgram;
 398     QString seqCmd=cmd;
 399     noreply++;
 400     if(cmd.contains("%")) seqCmd=cmd.arg(seq++);
 401     dgram=seqCmd.toLatin1();
 402     cmdSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5556);
 403     seqCmd.chop(1); // Remove training cr
 404  //   qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
 405 }
 406
 407 void DroneThread::navDataReady()
 408 {
 409    qint64 l;
 410    char buf[2048];
 411    QHostAddress host;
 412    quint16 port;
 413    QString cmd;
 414    bool ack;
 415    noreply=0; // We got a reply
 416    cmd="";ack=false;
 417    while(navSock->hasPendingDatagrams()) l=navSock->readDatagram(buf,sizeof(buf),&host,&port);
 418 //   qDebug() << "DroneThread::navDataReady state=" << state <<" l=" << l << "read=" << buf << "from"  << host ;
 419    nd.parseRawNavData((char *)&buf,l);
 420    if(nd.state& (ARDRONE_NAVDATA_BOOTSTRAP)) state=notInitialized;
 421    if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
 422        cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 423    if((nd.state& (ARDRONE_COMMAND_MASK ))) {
 424        cmd+=QString("AT*CTRL=%1,%2,0\r").arg(seq++).arg(ACK_CONTROL_MODE);
 425        ack=true;
 426    };
 427     switch(state) {
 428     case notInitialized:
 429       cmd+=QString("AT*CONFIG=%1,\"general:navdata_demo\",\"TRUE\"\r").arg(seq++);
 430   //    cmd+=QString("AT*CONFIG=%1,\"general:navdata_options\",\"%3\"\r").arg(seq++).arg(NAVDATA_OPTION_FULL_MASK);
 431   //    cmd+=QString("AT*CONFIG=%1,\"video:codec\",\"%2\"\r").arg(seq++).arg(0x20);
 432         state=bootstrap;
 433         retry=10;
 434         qDebug() << "Connected to drone" << seq << navData()->decodedState;
 435         parent->setConnected(TRUE); // We are connected to drone
 436         break;
 437     case bootstrap:
 438         if(ack) state=initialized;
 439         else
 440           if(--retry<=0) state=notInitialized;
 441         break;
 442     case initialized:
 443         cmd+=QString("AT*CTRL=%1,0\r").arg(seq++);
 444         cmd+=QString("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r").arg(seq++);
 445         state=ready;
 446         break;
 447     case ready:
 448         break;
 449     }
 450     if(!cmd.isEmpty())sendCmd(cmd); // don't send null cmd
 451 }
 452
 453 void DroneThread::sendNav(QString cmd)
 454 {
 455     QByteArray dgram;
 456     dgram=cmd.toLatin1();
 457 //    qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
 458     navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
 459 }
 460
 461
 462 DroneThread::DroneThread(DroneControl *parentp,QHostAddress host)
 463 {
 464
 465     struct ip_mreq imreq;
 466     QString my_ip;
 467     qDebug() << "DroneThread::DroneThread";
 468     my_ip="192.168.1.2";
 469         stopped=false;
 470         state=notInitialized;
 471         parent=parentp;
 472         navSock=new QUdpSocket();
 473         QList<QNetworkConfiguration> netconfs=QNetworkConfigurationManager().allConfigurations();
 474         foreach (QNetworkConfiguration np,netconfs) {
 475             qDebug() << "network Confifuration name " << np.name() << np.bearerName() << np.bearerTypeName();
 476             QNetworkSession ns(np);
 477             if(ns.interface().addressEntries().empty())
 478                 qDebug() << "network session " << ns.interface().humanReadableName() << "**NotConfig**";
 479             else {
 480                 qDebug() << "network session " << ns.interface().humanReadableName() <<   ns.interface().addressEntries().first().ip();
 481                 // Ubuntu may give wlan0 as "Ethernet"
 482                 if((np.bearerName()==QString("WLAN")) || (ns.interface().humanReadableName()==QString("wlan0")) ) {
 483                    my_ip=ns.interface().addressEntries().first().ip().toString();
 484                    qDebug() << "My IP is " << my_ip;
 485             }
 486             }
 487         }
 488         if(!navSock->bind(QHostAddress::Any,5554))
 489             qDebug() << "Cant open any: 5554" << navSock->errorString() ;
 490         imreq.imr_multiaddr.s_addr=inet_addr("224.1.1.1");
 491         imreq.imr_interface.s_addr=inet_addr(my_ip.toAscii());
 492         setsockopt(navSock->socketDescriptor(),IPPROTO_IP,IP_ADD_MEMBERSHIP,&imreq,sizeof(imreq));
 493         cmdSock=new QUdpSocket();
 494         cmdSock->bind(QHostAddress::Any,5556);
 495         droneHost=host;
 496         seq=1;
 497         m_pitch=0;
 498         m_roll=0;
 499         m_yaw=0;
 500         m_vv=0;
 501         m_fgain=1.0;
 502         m_bgain=1.0;
 503         m_rgain=1.0;
 504         m_lgain=1.0;
 505         noreply=0;
 506         start();
 507
 508
 509 };
 510
 511 void DroneThread::run()
 512 {
 513   qDebug() << "DroneThread::DroneThread";
 514     stateTimer=new QTimer(this);
 515     connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
 516     connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
 517     stateTimer->start(100);
 518     while(!stopped) {
 519         exec();
 520     }
 521
 522 }
 523
 524 void DroneThread::timer()
 525 {
 526    QString cmd;
 527   //  qDebug() << "thread Timer";
 528    noreply++;
 529     if(noreply>50 )  {
 530         cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 531         navData()->setState("**TimeOut**");
 532         qDebug("Timeout");
 533         noreply=0;
 534     };
 535     switch(state) {
 536         case notInitialized:
 537             sendNav("AT\r");
 538          //    cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 539             break;
 540     case bootstrap:
 541     case initialized:
 542         cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 543        break;
 544     case ready:
 545             cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 546             break;
 547     case flying: {
 548             float_or_int_t _pitch,_roll,_yaw,_vv;
 549             int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
 550
 551
 552             _pitch.f=m_pitch;
 553             _roll.f=m_roll;
 554             _yaw.f=m_yaw;
 555             _vv.f=m_vv;
 556 //            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);
 557 //            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);
 558             cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 559             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);
 560
 561
 562             break;
 563         }
 564 }
 565     if(!cmd.isEmpty())sendCmd(cmd);
 566 }
 567
 568
 569