git.maemo.org Git - mardrone/blob - mardrone/dronecontrol.cpp

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