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 bool DroneControl::confFullScreen()
 272 {
 273     return droneSettings->value("fullScreen",true).toBool();
 274 };
 275 void DroneControl::setConfFullScreen(bool val)
 276 {
 277     droneSettings->setValue("fullScreen",val);
 278     emit configChanged();
 279 };
 280
 281 float DroneControl::confForwardGain()
 282 {
 283     return droneSettings->value("forwardGain",1.0).toFloat();
 284 };
 285 void DroneControl::setConfForwardGain(float val)
 286 {
 287     droneSettings->setValue("forwardGain",val);
 288     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 289     emit configChanged();
 290 };
 291 float DroneControl::confCtrlTrsh()
 292 {
 293     return m_ctrlTrsh;
 294 };
 295 void DroneControl::setConfCtrlTrsh(float val)
 296 {
 297     droneSettings->setValue("ctrlTreshold",val);
 298     m_ctrlTrsh=val;
 299     emit configChanged();
 300 };
 301
 302 float DroneControl::confBackwardGain()
 303 {
 304     return droneSettings->value("backwardGain",1.0).toFloat();
 305 };
 306 void DroneControl::setConfBackwardGain(float val)
 307 {
 308     droneSettings->setValue("backwardGain",val);
 309     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 310     emit configChanged();
 311 };
 312
 313 float DroneControl::confLeftGain()
 314 {
 315     return droneSettings->value("leftGain",1.0).toFloat();
 316 };
 317 void DroneControl::setConfLeftGain(float val)
 318 {
 319     droneSettings->setValue("leftGain",val);
 320     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 321     emit configChanged();
 322 };
 323 float DroneControl::confRightGain()
 324 {
 325     return droneSettings->value("rightGain",1.0).toFloat();
 326 };
 327 void DroneControl::setConfRightGain(float val)
 328 {
 329     droneSettings->setValue("rightGain",val);
 330     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 331     emit configChanged();
 332 };
 333
 334 /*=================================================
 335
 336  DroneThread class starts here
 337
 338 ==================================================*/
 339
 340
 341 void DroneThread::setFly(bool fly)
 342 {
 343     if(state==ready) {
 344         m_fly=fly;
 345         state=flying;
 346         stateTimer->setInterval(50); // More frequent updates
 347         sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
 348
 349     }
 350      if(state==flying) {
 351          if(!fly) {
 352              stateTimer->setInterval(200); // Less frequent updates
 353              state=ready;
 354          }
 355          m_fly=fly;
 356      }
 357      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)));
 358 };
 359
 360 void DroneThread::setEmergency(bool emg)
 361 {
 362     m_emergency=emg;
 363     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)));
 364     // if(m_emergency==1)
 365         state=ready;
 366 }   ;
 367 void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
 368 {
 369     m_pitch=pitch*m_fgain;
 370     m_roll=roll*m_rgain;
 371     m_yaw=yaw;
 372     m_vv=vv;
 373 //    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);
 374 };
 375
 376 void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
 377 {
 378     m_fgain=fgain;
 379     m_bgain=bgain;
 380     m_rgain=rgain;
 381     m_lgain=lgain;
 382 };
 383
 384 void DroneThread::sendCmd(QString cmd)
 385 {
 386     QByteArray dgram;
 387     QString seqCmd=cmd;
 388     noreply++;
 389     if(cmd.contains("%")) seqCmd=cmd.arg(seq++);
 390     dgram=seqCmd.toLatin1();
 391     cmdSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5556);
 392     seqCmd.chop(1); // Remove training cr
 393  //   qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
 394 }
 395
 396 void DroneThread::navDataReady()
 397 {
 398    qint64 l;
 399    char buf[2048];
 400    QHostAddress host;
 401    quint16 port;
 402    QString cmd;
 403    bool ack;
 404    noreply=0; // We got a reply
 405    cmd="";ack=false;
 406    while(navSock->hasPendingDatagrams()) l=navSock->readDatagram(buf,sizeof(buf),&host,&port);
 407 //   qDebug() << "DroneThread::navDataReady state=" << state <<" l=" << l << "read=" << buf << "from"  << host ;
 408    nd.parseRawNavData((char *)&buf,l);
 409    if(nd.state& (ARDRONE_NAVDATA_BOOTSTRAP)) state=notInitialized;
 410    if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
 411        cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 412    if((nd.state& (ARDRONE_COMMAND_MASK ))) {
 413        cmd+=QString("AT*CTRL=%1,%2,0\r").arg(seq++).arg(ACK_CONTROL_MODE);
 414        ack=true;
 415    };
 416     switch(state) {
 417     case notInitialized:
 418       cmd+=QString("AT*CONFIG=%1,\"general:navdata_demo\",\"TRUE\"\r").arg(seq++);
 419   //    cmd+=QString("AT*CONFIG=%1,\"general:navdata_options\",\"%3\"\r").arg(seq++).arg(NAVDATA_OPTION_FULL_MASK);
 420   //    cmd+=QString("AT*CONFIG=%1,\"video:codec\",\"%2\"\r").arg(seq++).arg(0x20);
 421         state=bootstrap;
 422         retry=10;
 423         qDebug() << "Connected to drone" << seq << navData()->decodedState;
 424         parent->setConnected(TRUE); // We are connected to drone
 425         break;
 426     case bootstrap:
 427         if(ack) state=initialized;
 428         else
 429           if(--retry<=0) state=notInitialized;
 430         break;
 431     case initialized:
 432         cmd+=QString("AT*CTRL=%1,0\r").arg(seq++);
 433         cmd+=QString("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r").arg(seq++);
 434         state=ready;
 435         break;
 436     case ready:
 437         break;
 438     }
 439     if(!cmd.isEmpty())sendCmd(cmd); // don't send null cmd
 440 }
 441
 442 void DroneThread::sendNav(QString cmd)
 443 {
 444     QByteArray dgram;
 445     dgram=cmd.toLatin1();
 446 //    qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
 447     navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
 448 }
 449
 450
 451 DroneThread::DroneThread(DroneControl *parentp,QHostAddress host)
 452 {
 453
 454     struct ip_mreq imreq;
 455     QString my_ip;
 456     qDebug() << "DroneThread::DroneThread";
 457     my_ip="192.168.1.2";
 458         stopped=false;
 459         state=notInitialized;
 460         parent=parentp;
 461         navSock=new QUdpSocket();
 462         QList<QNetworkConfiguration> netconfs=QNetworkConfigurationManager().allConfigurations();
 463         foreach (QNetworkConfiguration np,netconfs) {
 464             qDebug() << "network Confifuration name " << np.name() << np.bearerName() << np.bearerTypeName();
 465             QNetworkSession ns(np);
 466             if(ns.interface().addressEntries().empty())
 467                 qDebug() << "network session " << ns.interface().humanReadableName() << "**NotConfig**";
 468             else {
 469                 qDebug() << "network session " << ns.interface().humanReadableName() <<   ns.interface().addressEntries().first().ip();
 470                 // Ubuntu may give wlan0 as "Ethernet"
 471                 if((np.bearerName()==QString("WLAN")) || (ns.interface().humanReadableName()==QString("wlan0")) ) {
 472                    my_ip=ns.interface().addressEntries().first().ip().toString();
 473                    qDebug() << "My IP is " << my_ip;
 474             }
 475             }
 476         }
 477         if(!navSock->bind(QHostAddress::Any,5554))
 478             qDebug() << "Cant open any: 5554" << navSock->errorString() ;
 479         imreq.imr_multiaddr.s_addr=inet_addr("224.1.1.1");
 480         imreq.imr_interface.s_addr=inet_addr(my_ip.toAscii());
 481         setsockopt(navSock->socketDescriptor(),IPPROTO_IP,IP_ADD_MEMBERSHIP,&imreq,sizeof(imreq));
 482         cmdSock=new QUdpSocket();
 483         cmdSock->bind(QHostAddress::Any,5556);
 484         droneHost=host;
 485         seq=1;
 486         m_pitch=0;
 487         m_roll=0;
 488         m_yaw=0;
 489         m_vv=0;
 490         m_fgain=1.0;
 491         m_bgain=1.0;
 492         m_rgain=1.0;
 493         m_lgain=1.0;
 494         noreply=0;
 495         start();
 496
 497
 498 };
 499
 500 void DroneThread::run()
 501 {
 502   qDebug() << "DroneThread::DroneThread";
 503     stateTimer=new QTimer(this);
 504     connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
 505     connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
 506     stateTimer->start(100);
 507     while(!stopped) {
 508         exec();
 509     }
 510
 511 }
 512
 513 void DroneThread::timer()
 514 {
 515    QString cmd;
 516   //  qDebug() << "thread Timer";
 517    noreply++;
 518     if(noreply>50 )  {
 519         cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 520         navData()->setState("**TimeOut**");
 521         qDebug("Timeout");
 522         noreply=0;
 523     };
 524     switch(state) {
 525         case notInitialized:
 526             sendNav("AT\r");
 527          //    cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 528             break;
 529     case bootstrap:
 530     case initialized:
 531         cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 532        break;
 533     case ready:
 534             cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
 535             break;
 536     case flying: {
 537             float_or_int_t _pitch,_roll,_yaw,_vv;
 538             int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
 539
 540
 541             _pitch.f=m_pitch;
 542             _roll.f=m_roll;
 543             _yaw.f=m_yaw;
 544             _vv.f=m_vv;
 545 //            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);
 546 //            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);
 547             cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
 548             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);
 549
 550
 551             break;
 552         }
 553 }
 554     if(!cmd.isEmpty())sendCmd(cmd);
 555 }
 556
 557
 558