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