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