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