git.maemo.org Git - mardrone/blob - mardrone/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 "dronecontrol.h"
  22 #include <QDebug>
  23
  24 DroneControl::DroneControl():QObject()
  25 {
  26     qDebug() << "DroneControl::DroneControl";
  27
  28 //    ctlSock=new QTcpSocket();
  29 //    ctlSock->bind(QHostAddress::Any,5559);
  30 //    navSock=new QUdpSocket();
  31 //    navSock->bind(QHostAddress::Any,5554)
  32     //connect(navSocket,SIGNAL(readyRead()),SLOT(navDataReady()));
  33     droneHost.setAddress("192.168.1.1");
  34     droneThread=new DroneThread(this,droneHost);
  35     connect(droneThread->navData(),SIGNAL(navDataUpdated()),this,SLOT(navDataUpdated()));
  36     connect(droneThread->navData(),SIGNAL(stateUpdated()),this,SLOT(statusUpdated()));
  37     droneSettings=new QSettings("katix.org","ardrone");
  38     m_pitch=0;
  39     m_roll=0;
  40     m_yaw=0;
  41     m_vv=0;
  42     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
  43
  44 }
  45 void DroneControl::navDataUpdated()
  46 {
  47     emit navDataChanged();
  48
  49 }
  50 void DroneControl::statusUpdated()
  51 {
  52     emit statusChanged();
  53
  54 }
  55
  56 void DroneControl::setPitch(float val_)
  57 {
  58     m_pitch=val_;
  59 //    qDebug() << "setPitch=" << val_;
  60     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
  61 };
  62 float DroneControl::pitch() {return m_pitch;};
  63
  64 void DroneControl::setRoll(float val_)
  65 {
  66     m_roll=val_;
  67 //    qDebug() << "setRoll=" << val_;
  68     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
  69 };
  70 float DroneControl::roll() {return m_roll;};
  71 void DroneControl::setYaw(float val_) {
  72     m_yaw=val_;
  73  //   qDebug() << "setYaw=" << val_;
  74     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
  75 };
  76 float DroneControl::yaw() {return m_yaw;};
  77 void DroneControl::setVVelocity(float val_) {
  78     m_vv=val_;
  79 //    qDebug() << "setVv=" << val_;
  80     droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
  81 };
  82 float DroneControl::vVelocity() {return m_vv;};
  83 void DroneControl::setAltitude(float val_) {
  84     m_altitude=val_;
  85 };
  86 float DroneControl::altitude() {return m_altitude;};
  87 void DroneControl::setEnabled(int val_) { m_enabled=val_;};
  88 int DroneControl::enabled() {return m_enabled;};
  89 void DroneControl::setFly(bool val_)
  90 {
  91     m_fly=val_;
  92     qDebug() << "setFly=" << val_;
  93     droneThread->setFly(m_fly);
  94 };
  95 bool DroneControl::fly() {return m_fly;};
  96 void DroneControl::setEmergency(bool val_)
  97 {
  98     m_emergency=val_;
  99     qDebug() << "setEmergency=" << val_;
 100     droneThread->setEmergency(m_emergency);
 101 };
 102 bool DroneControl::emergency() {return m_emergency;};
 103
 104
 105 // Getters to drone actual valyes sent by drone
 106 float DroneControl::droneAltitude()
 107 {
 108     return droneThread->navData()->altitude;
 109 };
 110 float DroneControl::dronePitch()
 111 {
 112     return droneThread->navData()->pitch;
 113 };
 114 float DroneControl::droneRoll()
 115 {
 116     return droneThread->navData()->roll;
 117 };
 118 float DroneControl::droneYaw()
 119 {
 120     return droneThread->navData()->yaw;
 121 };
 122 float DroneControl::droneVBat()
 123 {
 124     return droneThread->navData()->vbat;
 125 };
 126 QString DroneControl::decodedStatus()
 127 {
 128     return droneThread->navData()->decodedState;
 129 };
 130 int DroneControl::pwm_motor1()
 131 {
 132     return droneThread->navData()->pwm_motor1;
 133 };
 134 int DroneControl::pwm_motor2()
 135 {
 136     return droneThread->navData()->pwm_motor2;
 137 };
 138 int DroneControl::pwm_motor3()
 139 {
 140     return droneThread->navData()->pwm_motor3;
 141 };
 142 int DroneControl::pwm_motor4()
 143 {
 144     return droneThread->navData()->pwm_motor4;
 145 };
 146
 147
 148
 149
 150
 151 QString DroneControl::confDroneIp()
 152 {
 153     qDebug() << "confDroneIp:" << droneSettings->value("droneIp","192.168.1.1").toString();
 154     return droneSettings->value("droneIp","192.168.1.1").toString();
 155 };
 156 void DroneControl::setConfDroneIp(QString ip)
 157 {
 158     qDebug() << "setConfDroneIp:" << ip;
 159   droneSettings->setValue("droneIp",ip);
 160   emit configChanged();
 161 };
 162 bool DroneControl::confShowDebug()
 163 {
 164 return droneSettings->value("showDebug",true).toBool();
 165 };
 166 void DroneControl::setConfShowDebug(bool val)
 167 {
 168     droneSettings->setValue("showDebug",val);
 169       emit configChanged();
 170 };
 171 bool DroneControl::confShowHorizon()
 172 {
 173     return droneSettings->value("showHorizon",true).toBool();
 174 };
 175 void DroneControl::setConfShowHorizon(bool val)
 176 {
 177     droneSettings->setValue("showHorizon",val);
 178     emit configChanged();
 179 };
 180 bool DroneControl::confShowGauges()
 181 {
 182     return droneSettings->value("showGuges",true).toBool();
 183 };
 184 void DroneControl::setConfShowGauges(bool val)
 185 {
 186     droneSettings->setValue("showGauges",val);
 187     emit configChanged();
 188 };
 189 bool DroneControl::confUseAccel()
 190 {
 191     return droneSettings->value("useAccel",false).toBool();
 192 };
 193 void DroneControl::setConfUseAccel(bool val)
 194 {
 195     droneSettings->setValue("useAccel",val);
 196     emit configChanged();
 197 };
 198 bool DroneControl::confUseJoyStick()
 199 {
 200     return droneSettings->value("useJouStick",false).toBool();
 201 };
 202 void DroneControl::setConfUseJoyStick(bool val)
 203 {
 204     droneSettings->setValue("useJoyStick",val);
 205     emit configChanged();
 206 };
 207
 208 bool DroneControl::confFullScreen()
 209 {
 210     return droneSettings->value("fullScreen",true).toBool();
 211 };
 212 void DroneControl::setConfFullScreen(bool val)
 213 {
 214     droneSettings->setValue("fullScreen",val);
 215     emit configChanged();
 216 };
 217
 218 float DroneControl::confForwardGain()
 219 {
 220     return droneSettings->value("forwardGain",1.0).toFloat();
 221 };
 222 void DroneControl::setConfForwardGain(float val)
 223 {
 224     droneSettings->setValue("forwardGain",val);
 225     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 226     emit configChanged();
 227 };
 228
 229
 230 float DroneControl::confBackwardGain()
 231 {
 232     return droneSettings->value("backwardGain",1.0).toFloat();
 233 };
 234 void DroneControl::setConfBackwardGain(float val)
 235 {
 236     droneSettings->setValue("backwardGain",val);
 237     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 238     emit configChanged();
 239 };
 240
 241 float DroneControl::confLeftGain()
 242 {
 243     return droneSettings->value("leftGain",1.0).toFloat();
 244 };
 245 void DroneControl::setConfLeftGain(float val)
 246 {
 247     droneSettings->setValue("leftGain",val);
 248     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 249     emit configChanged();
 250 };
 251 float DroneControl::confRightGain()
 252 {
 253     return droneSettings->value("rightGain",1.0).toFloat();
 254 };
 255 void DroneControl::setConfRightGain(float val)
 256 {
 257     droneSettings->setValue("rightGain",val);
 258     droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 259     emit configChanged();
 260 };
 261
 262 /*=================================================
 263
 264  DroneThread class starts here
 265
 266 ==================================================*/
 267
 268
 269 void DroneThread::setFly(bool fly)
 270 {
 271     if(state==ready) {
 272         m_fly=fly;
 273         state=flying;
 274         stateTimer->setInterval(50); // More frequent updates
 275         sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
 276
 277     }
 278      if(state==flying) {
 279          if(!fly) {
 280              stateTimer->setInterval(200); // Less frequent updates
 281              state=ready;
 282          }
 283          m_fly=fly;
 284      }
 285      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)));
 286 };
 287
 288 void DroneThread::setEmergency(bool emg)
 289 {
 290     m_emergency=emg;
 291     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)));
 292     // if(m_emergency==1)
 293         state=ready;
 294 }   ;
 295 void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
 296 {
 297     m_pitch=pitch/200.0*m_fgain;
 298     m_roll=roll/200.0*m_rgain;
 299     m_yaw=yaw/200.0;
 300     m_vv=vv/200.0;
 301     qDebug()  << QString("pitch=%1 roll=%2 yaw=%3 vv=%4\r").arg(m_pitch,3,'F',2).arg(m_roll,3,'F',2).arg(m_yaw,3,'F',2).arg(m_vv,3,'F',2);
 302 };
 303
 304 void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
 305 {
 306     m_fgain=fgain;
 307     m_bgain=bgain;
 308     m_rgain=rgain;
 309     m_lgain=lgain;
 310 };
 311
 312 void DroneThread::sendCmd(QString cmd)
 313 {
 314     QByteArray dgram;
 315     QString seqCmd=cmd;
 316     if(cmd.contains("%")) seqCmd=cmd.arg(seq++);
 317     dgram=seqCmd.toLatin1();
 318     cmdSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5556);
 319     seqCmd.chop(1); // Remove training cr
 320     qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
 321 }
 322
 323 void DroneThread::navDataReady()
 324 {
 325    qint64 l;
 326    char buf[2048];
 327    QHostAddress host;
 328    quint16 port;
 329    while(navSock->hasPendingDatagrams()) l=navSock->readDatagram(buf,sizeof(buf),&host,&port);
 330 //   qDebug() << "DroneThread::navDataReady state=" << state <<" l=" << l << "read=" << buf << "from"  << host ;
 331    nd.parseRawNavData((char *)&buf,l);
 332    if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
 333        sendCmd(QString("AT*COMWDG=%1\r").arg(seq++));
 334     switch(state) {
 335     case notInitialized:
 336         sendCmd("AT*CONFIG=%1,\"general:navdata_demo\",\"FALSE\"\r");
 337         state=initialized;
 338         break;
 339     case initialized:
 340         sendCmd("AT*CTRL=0\r");
 341         state=ready;
 342         break;
 343     case ready:
 344         break;
 345     }
 346 }
 347
 348 void DroneThread::sendNav(QString cmd)
 349 {
 350     QByteArray dgram;
 351     dgram=cmd.toLatin1();
 352     qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
 353     navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
 354 }
 355
 356
 357 DroneThread::DroneThread(DroneControl *parentp,QHostAddress host)
 358 {
 359     qDebug() << "DroneThread::DroneThread";
 360         stopped=false;
 361         state=notInitialized;
 362         parent=parentp;
 363         navSock=new QUdpSocket();
 364         navSock->bind(QHostAddress::Any,5554);
 365         cmdSock=new QUdpSocket();
 366         cmdSock->bind(QHostAddress::Any,5556);
 367         droneHost=host;
 368         seq=1;
 369         m_pitch=0;
 370         m_roll=0;
 371         m_yaw=0;
 372         m_vv=0;
 373         m_fgain=1.0;
 374         m_bgain=1.0;
 375         m_rgain=1.0;
 376         m_lgain=1.0;
 377         start();
 378
 379
 380 };
 381
 382 void DroneThread::run()
 383 {
 384   qDebug() << "DroneThread::DroneThread";
 385     stateTimer=new QTimer(this);
 386     connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
 387     connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
 388     stateTimer->start(1000);
 389     while(!stopped) {
 390         exec();
 391     }
 392
 393 }
 394
 395 void DroneThread::timer()
 396 {
 397   //  qDebug() << "thread Timer";
 398     switch(state) {
 399         case notInitialized:
 400             sendNav("AT");
 401             break;
 402         case initialized:
 403             break;
 404     case ready:
 405             sendCmd(QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28)));
 406             break;
 407     case flying: {
 408             float_or_int_t _pitch,_roll,_yaw,_vv;
 409             int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
 410
 411
 412             _pitch.f=m_pitch;
 413             _roll.f=m_roll;
 414             _yaw.f=m_yaw;
 415             _vv.f=m_vv;
 416 //            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);
 417 //            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);
 418             sendCmd(QString("AT*COMWDG=%1\r").arg(seq++));
 419             sendCmd(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));
 420
 421
 422             break;
 423         }
 424 }
 425 }
 426
 427
 428