1 /*==================================================================
3 ! mardrone application AR-Drone for MeeGo
5 ! Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
8 ! Author:Kate Alhola kate.alhola@nokia.com
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.
20 *===================================================================*/
21 #include "dronecontrol.h"
24 DroneControl::DroneControl():QObject()
26 qDebug() << "DroneControl::DroneControl";
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");
43 m_useAccel=droneSettings->value("useAccel",false).toBool();
44 m_ctrlTrsh=droneSettings->value("ctrlTreshold",10.0).toFloat();
45 rSensor=new QRotationSensor();
47 connect(rSensor,SIGNAL(readingChanged()),this,SLOT(rotationReadingsChanged()));
48 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
52 void DroneControl::rotationReadingsChanged()
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);
66 void DroneControl::navDataUpdated()
68 emit navDataChanged();
71 void DroneControl::statusUpdated()
77 void DroneControl::setPitch(float val_)
80 // qDebug() << "setPitch=" << val_;
81 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
83 float DroneControl::pitch() {return m_pitch;};
85 void DroneControl::setRoll(float val_)
88 // qDebug() << "setRoll=" << val_;
89 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
91 float DroneControl::roll() {return m_roll;};
92 void DroneControl::setYaw(float 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);
98 float DroneControl::yaw() {return m_yaw;};
99 void DroneControl::setVVelocity(float 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);
105 float DroneControl::vVelocity() {return m_vv;};
106 void DroneControl::setAltitude(float val_) {
109 float DroneControl::altitude() {return m_altitude;};
110 void DroneControl::setEnabled(int val_) { m_enabled=val_;};
111 int DroneControl::enabled() {return m_enabled;};
113 bool DroneControl::ctrlActive()
117 void DroneControl::setCtrlActive(bool val_)
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);
126 if(!m_ctrlActive) { m_pitch=0;m_roll=0; };
127 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
128 emit pitchRollChanged();
133 void DroneControl::setFly(bool val_)
136 qDebug() << "setFly=" << val_;
137 droneThread->setFly(m_fly);
139 bool DroneControl::fly() {return m_fly;};
140 void DroneControl::setEmergency(bool val_)
143 qDebug() << "setEmergency=" << val_;
144 droneThread->setEmergency(m_emergency);
146 bool DroneControl::emergency() {return m_emergency;};
149 // Getters to drone actual valyes sent by drone
150 float DroneControl::droneAltitude()
152 return droneThread->navData()->altitude;
154 float DroneControl::dronePitch()
156 return droneThread->navData()->pitch;
158 float DroneControl::droneRoll()
160 return droneThread->navData()->roll;
162 float DroneControl::droneYaw()
164 return droneThread->navData()->yaw;
166 float DroneControl::droneVBat()
168 return droneThread->navData()->vbat;
170 QString DroneControl::decodedStatus()
172 return droneThread->navData()->decodedState;
174 int DroneControl::pwm_motor1()
176 return droneThread->navData()->pwm_motor1;
178 int DroneControl::pwm_motor2()
180 return droneThread->navData()->pwm_motor2;
182 int DroneControl::pwm_motor3()
184 return droneThread->navData()->pwm_motor3;
186 int DroneControl::pwm_motor4()
188 return droneThread->navData()->pwm_motor4;
195 QString DroneControl::confDroneIp()
197 qDebug() << "confDroneIp:" << droneSettings->value("droneIp","192.168.1.1").toString();
198 return droneSettings->value("droneIp","192.168.1.1").toString();
200 void DroneControl::setConfDroneIp(QString ip)
202 qDebug() << "setConfDroneIp:" << ip;
203 droneSettings->setValue("droneIp",ip);
204 emit configChanged();
206 bool DroneControl::confShowDebug()
208 return droneSettings->value("showDebug",true).toBool();
210 void DroneControl::setConfShowDebug(bool val)
212 droneSettings->setValue("showDebug",val);
213 emit configChanged();
215 bool DroneControl::confShowHorizon()
217 return droneSettings->value("showHorizon",true).toBool();
219 void DroneControl::setConfShowHorizon(bool val)
221 droneSettings->setValue("showHorizon",val);
222 emit configChanged();
224 bool DroneControl::confShowGauges()
226 return droneSettings->value("showGuges",true).toBool();
228 void DroneControl::setConfShowGauges(bool val)
230 droneSettings->setValue("showGauges",val);
231 emit configChanged();
233 bool DroneControl::confUseAccel()
235 //return droneSettings->value("useAccel",false).toBool();
236 return m_useAccel; // return cached value
238 void DroneControl::setConfUseAccel(bool val)
240 droneSettings->setValue("useAccel",val);
242 emit configChanged();
244 bool DroneControl::confUseJoyStick()
246 return droneSettings->value("useJouStick",false).toBool();
248 void DroneControl::setConfUseJoyStick(bool val)
250 droneSettings->setValue("useJoyStick",val);
251 emit configChanged();
254 bool DroneControl::confFullScreen()
256 return droneSettings->value("fullScreen",true).toBool();
258 void DroneControl::setConfFullScreen(bool val)
260 droneSettings->setValue("fullScreen",val);
261 emit configChanged();
264 float DroneControl::confForwardGain()
266 return droneSettings->value("forwardGain",1.0).toFloat();
268 void DroneControl::setConfForwardGain(float val)
270 droneSettings->setValue("forwardGain",val);
271 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
272 emit configChanged();
274 float DroneControl::confCtrlTrsh()
278 void DroneControl::setConfCtrlTrsh(float val)
280 droneSettings->setValue("ctrlTreshold",val);
282 emit configChanged();
285 float DroneControl::confBackwardGain()
287 return droneSettings->value("backwardGain",1.0).toFloat();
289 void DroneControl::setConfBackwardGain(float val)
291 droneSettings->setValue("backwardGain",val);
292 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
293 emit configChanged();
296 float DroneControl::confLeftGain()
298 return droneSettings->value("leftGain",1.0).toFloat();
300 void DroneControl::setConfLeftGain(float val)
302 droneSettings->setValue("leftGain",val);
303 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
304 emit configChanged();
306 float DroneControl::confRightGain()
308 return droneSettings->value("rightGain",1.0).toFloat();
310 void DroneControl::setConfRightGain(float val)
312 droneSettings->setValue("rightGain",val);
313 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
314 emit configChanged();
317 /*=================================================
319 DroneThread class starts here
321 ==================================================*/
324 void DroneThread::setFly(bool fly)
329 stateTimer->setInterval(50); // More frequent updates
330 sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
335 stateTimer->setInterval(200); // Less frequent updates
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)));
343 void DroneThread::setEmergency(bool 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)
350 void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
352 m_pitch=pitch/200.0*m_fgain;
353 m_roll=roll/200.0*m_rgain;
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);
359 void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
367 void DroneThread::sendCmd(QString 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 ;
378 void DroneThread::navDataReady()
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++));
391 sendCmd("AT*CONFIG=%1,\"general:navdata_demo\",\"FALSE\"\r");
395 sendCmd("AT*CTRL=0\r");
396 sendCmd("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r");
404 void DroneThread::sendNav(QString cmd)
407 dgram=cmd.toLatin1();
408 qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
409 navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
413 DroneThread::DroneThread(DroneControl *parentp,QHostAddress host)
415 qDebug() << "DroneThread::DroneThread";
417 state=notInitialized;
419 navSock=new QUdpSocket();
420 navSock->bind(QHostAddress::Any,5554);
421 cmdSock=new QUdpSocket();
422 cmdSock->bind(QHostAddress::Any,5556);
438 void DroneThread::run()
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);
451 void DroneThread::timer()
453 // qDebug() << "thread Timer";
461 sendCmd(QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28)));
464 float_or_int_t _pitch,_roll,_yaw,_vv;
465 int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
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));