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 *===================================================================*/
22 #include <sys/types.h>
23 #include <sys/socket.h>
24 #include <netinet/in.h>
25 #include <arpa/inet.h>
26 #include "dronecontrol.h"
28 #include <QNetworkConfigurationManager>
29 #include <QNetworkSession>
32 #include "ardrone_api.h"
34 DroneControl::DroneControl():QObject()
36 qDebug() << "DroneControl::DroneControl";
40 droneSettings=new QSettings("katix.org","mardrone");
47 m_useAccel=droneSettings->value("useAccel",false).toBool();
48 m_useJoyStick=droneSettings->value("useJoyStick",false).toBool();
49 m_ctrlTrsh=droneSettings->value("ctrlTreshold",10.0).toFloat();
50 droneHost.setAddress(confDroneIp());
51 droneThread=new DroneThread(this,droneHost);
52 connect(droneThread->navData(),SIGNAL(navDataUpdated()),this,SLOT(navDataUpdated()));
53 connect(droneThread->navData(),SIGNAL(stateUpdated()),this,SLOT(statusUpdated()));
55 rSensor=new QRotationSensor();
57 connect(rSensor,SIGNAL(readingChanged()),this,SLOT(rotationReadingsChanged()));
60 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
64 void DroneControl::axisValueChanged(int axis, int value)
67 qDebug()<< "DroneControl::axisValueChanged(" << axis << value << ")";
71 void DroneControl::rotationReadingsChanged()
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);
87 void DroneControl::setConnected(bool val_)
90 emit connectedChanged();
92 void DroneControl::navDataUpdated()
94 emit navDataChanged();
97 void DroneControl::statusUpdated()
103 void DroneControl::setPitch(float val_)
106 // qDebug() << "setPitch=" << val_;
107 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
108 emit pitchRollChanged();
110 float DroneControl::pitch() {return m_pitch;};
112 void DroneControl::setRoll(float val_)
115 // qDebug() << "setRoll=" << val_;
116 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
117 emit pitchRollChanged();
119 float DroneControl::roll() {return m_roll;};
120 void DroneControl::setYaw(float val) {
122 // qDebug() << "setYaw=" << val;
123 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));
124 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
125 emit pitchRollChanged();
127 float DroneControl::yaw() {return m_yaw;};
128 void DroneControl::setVVelocity(float val) {
130 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));
131 // qDebug() << "setVv=" << val_;
132 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
133 emit pitchRollChanged();
135 float DroneControl::vVelocity() {return m_vv;};
136 void DroneControl::setAltitude(float val_) {m_altitude=val_;};
137 float DroneControl::altitude() {return m_altitude;};
138 void DroneControl::setEnabled(int val_) { m_enabled=val_;};
139 int DroneControl::enabled() {return m_enabled;};
140 bool DroneControl::ctrlActive() { return m_ctrlActive;};
141 bool DroneControl::emergency() {return m_emergency;};
143 void DroneControl::setCtrlActive(bool val_)
146 if(val_ && !m_ctrlActive) { // Ctrl switched to active
147 m_rotRefX=rSensor->reading()->x();
148 m_rotRefY=rSensor->reading()->y();
149 m_rotRefZ=rSensor->reading()->z();
150 qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
154 if(!m_ctrlActive) { m_pitch=0;m_roll=0; };
155 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
156 emit pitchRollChanged();
161 void DroneControl::setFly(bool val_)
164 qDebug() << "setFly=" << val_;
165 droneThread->setFly(m_fly);
167 bool DroneControl::fly() {return m_fly;};
168 void DroneControl::setEmergency(bool val_)
171 qDebug() << "setEmergency=" << val_;
172 droneThread->setEmergency(m_emergency);
177 // Getters to drone actual valyes sent by drone
178 float DroneControl::droneAltitude() { return droneThread->navData()->altitude; };
179 float DroneControl::dronePitch() { return droneThread->navData()->pitch; };
180 float DroneControl::droneRoll() { return droneThread->navData()->roll; };
181 float DroneControl::droneYaw() { return droneThread->navData()->yaw; };
182 float DroneControl::droneSpeed() { return droneThread->navData()->vx;};
183 float DroneControl::droneVBat() { return droneThread->navData()->vbat;};
184 QString DroneControl::decodedStatus() { return droneThread->navData()->decodedState; };
185 int DroneControl::pwm_motor1() { return droneThread->navData()->pwm_motor1; };
186 int DroneControl::pwm_motor2() { return droneThread->navData()->pwm_motor2; };
187 int DroneControl::pwm_motor3() { return droneThread->navData()->pwm_motor3;};
188 int DroneControl::pwm_motor4() { return droneThread->navData()->pwm_motor4;};
190 QString DroneControl::confDroneIp()
192 qDebug() << "confDroneIp:" << droneSettings->value("droneIp","192.168.1.1").toString();
193 return droneSettings->value("droneIp","192.168.1.1").toString();
195 void DroneControl::setConfDroneIp(QString ip)
197 qDebug() << "setConfDroneIp:" << ip;
198 droneSettings->setValue("droneIp",ip);
199 emit configChanged();
201 QString DroneControl::confActiveUI()
203 qDebug() << "confActiveUI:" << droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
204 return droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
206 void DroneControl::setConfActiveUI(QString ui)
208 qDebug() << "setConfActiveUI:" << ui;
209 droneSettings->setValue("activeUI",ui);
210 emit configChanged();
212 bool DroneControl::confShowDebug()
214 return droneSettings->value("showDebug",true).toBool();
216 void DroneControl::setConfShowDebug(bool val)
218 droneSettings->setValue("showDebug",val);
219 emit configChanged();
221 bool DroneControl::confShowJSIndicators()
223 return droneSettings->value("showJSIndicators",true).toBool();
225 void DroneControl::setConfShowJSIndicators(bool val)
227 droneSettings->setValue("showJSIndicators",val);
228 emit configChanged();
230 bool DroneControl::confShowHorizon()
232 return droneSettings->value("showHorizon",true).toBool();
234 void DroneControl::setConfShowHorizon(bool val)
236 droneSettings->setValue("showHorizon",val);
237 emit configChanged();
239 bool DroneControl::confShowGauges()
241 return droneSettings->value("showGuges",true).toBool();
243 void DroneControl::setConfShowGauges(bool val)
245 droneSettings->setValue("showGauges",val);
246 emit configChanged();
249 bool DroneControl::confUseAccel()
251 //return droneSettings->value("useAccel",false).toBool();
252 return m_useAccel; // return cached value
254 void DroneControl::setConfUseAccel(bool val)
256 droneSettings->setValue("useAccel",val);
258 emit configChanged();
260 bool DroneControl::confUseJoyStick()
262 return droneSettings->value("useJoyStick",false).toBool();
264 void DroneControl::setConfUseJoyStick(bool val)
266 droneSettings->setValue("useJoyStick",val);
268 emit configChanged();
271 int DroneControl::confSimuMode()
273 return droneSettings->value("SimuMode",true).toInt();
275 void DroneControl::setConfSimuMode(int val)
277 droneSettings->setValue("SimuMode",val);
278 emit configChanged();
281 bool DroneControl::confFullScreen()
283 return droneSettings->value("fullScreen",true).toBool();
285 void DroneControl::setConfFullScreen(bool val)
287 droneSettings->setValue("fullScreen",val);
288 emit configChanged();
292 float DroneControl::confForwardGain()
294 return droneSettings->value("forwardGain",1.0).toFloat();
296 void DroneControl::setConfForwardGain(float val)
298 droneSettings->setValue("forwardGain",val);
299 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
300 emit configChanged();
302 float DroneControl::confCtrlTrsh()
306 void DroneControl::setConfCtrlTrsh(float val)
308 droneSettings->setValue("ctrlTreshold",val);
310 emit configChanged();
313 float DroneControl::confBackwardGain()
315 return droneSettings->value("backwardGain",1.0).toFloat();
317 void DroneControl::setConfBackwardGain(float val)
319 droneSettings->setValue("backwardGain",val);
320 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
321 emit configChanged();
324 float DroneControl::confLeftGain()
326 return droneSettings->value("leftGain",1.0).toFloat();
328 void DroneControl::setConfLeftGain(float val)
330 droneSettings->setValue("leftGain",val);
331 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
332 emit configChanged();
334 float DroneControl::confRightGain()
336 return droneSettings->value("rightGain",1.0).toFloat();
338 void DroneControl::setConfRightGain(float val)
340 droneSettings->setValue("rightGain",val);
341 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
342 emit configChanged();
345 /*=================================================
347 DroneThread class starts here
349 ==================================================*/
352 void DroneThread::setFly(bool fly)
357 stateTimer->setInterval(50); // More frequent updates
358 sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
363 stateTimer->setInterval(200); // Less frequent updates
368 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)));
371 void DroneThread::setEmergency(bool emg)
374 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)));
375 // if(m_emergency==1)
378 void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
380 m_pitch=pitch*m_fgain;
384 // 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);
387 void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
395 void DroneThread::sendCmd(QString cmd)
400 if(cmd.contains("%")) seqCmd=cmd.arg(seq++);
401 dgram=seqCmd.toLatin1();
402 cmdSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5556);
403 seqCmd.chop(1); // Remove training cr
404 // qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
407 void DroneThread::navDataReady()
415 noreply=0; // We got a reply
417 while(navSock->hasPendingDatagrams()) l=navSock->readDatagram(buf,sizeof(buf),&host,&port);
418 // qDebug() << "DroneThread::navDataReady state=" << state <<" l=" << l << "read=" << buf << "from" << host ;
419 nd.parseRawNavData((char *)&buf,l);
420 if(nd.state& (ARDRONE_NAVDATA_BOOTSTRAP)) state=notInitialized;
421 if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
422 cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
423 if((nd.state& (ARDRONE_COMMAND_MASK ))) {
424 cmd+=QString("AT*CTRL=%1,%2,0\r").arg(seq++).arg(ACK_CONTROL_MODE);
429 cmd+=QString("AT*CONFIG=%1,\"general:navdata_demo\",\"TRUE\"\r").arg(seq++);
430 // cmd+=QString("AT*CONFIG=%1,\"general:navdata_options\",\"%3\"\r").arg(seq++).arg(NAVDATA_OPTION_FULL_MASK);
431 // cmd+=QString("AT*CONFIG=%1,\"video:codec\",\"%2\"\r").arg(seq++).arg(0x20);
434 qDebug() << "Connected to drone" << seq << navData()->decodedState;
435 parent->setConnected(TRUE); // We are connected to drone
438 if(ack) state=initialized;
440 if(--retry<=0) state=notInitialized;
443 cmd+=QString("AT*CTRL=%1,0\r").arg(seq++);
444 cmd+=QString("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r").arg(seq++);
450 if(!cmd.isEmpty())sendCmd(cmd); // don't send null cmd
453 void DroneThread::sendNav(QString cmd)
456 dgram=cmd.toLatin1();
457 // qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
458 navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
462 DroneThread::DroneThread(DroneControl *parentp,QHostAddress host)
465 struct ip_mreq imreq;
467 qDebug() << "DroneThread::DroneThread";
470 state=notInitialized;
472 navSock=new QUdpSocket();
473 QList<QNetworkConfiguration> netconfs=QNetworkConfigurationManager().allConfigurations();
474 foreach (QNetworkConfiguration np,netconfs) {
475 qDebug() << "network Confifuration name " << np.name() << np.bearerName() << np.bearerTypeName();
476 QNetworkSession ns(np);
477 if(ns.interface().addressEntries().empty())
478 qDebug() << "network session " << ns.interface().humanReadableName() << "**NotConfig**";
480 qDebug() << "network session " << ns.interface().humanReadableName() << ns.interface().addressEntries().first().ip();
481 // Ubuntu may give wlan0 as "Ethernet"
482 if((np.bearerName()==QString("WLAN")) || (ns.interface().humanReadableName()==QString("wlan0")) ) {
483 my_ip=ns.interface().addressEntries().first().ip().toString();
484 qDebug() << "My IP is " << my_ip;
488 if(!navSock->bind(QHostAddress::Any,5554))
489 qDebug() << "Cant open any: 5554" << navSock->errorString() ;
490 imreq.imr_multiaddr.s_addr=inet_addr("224.1.1.1");
491 imreq.imr_interface.s_addr=inet_addr(my_ip.toAscii());
492 setsockopt(navSock->socketDescriptor(),IPPROTO_IP,IP_ADD_MEMBERSHIP,&imreq,sizeof(imreq));
493 cmdSock=new QUdpSocket();
494 cmdSock->bind(QHostAddress::Any,5556);
511 void DroneThread::run()
513 qDebug() << "DroneThread::DroneThread";
514 stateTimer=new QTimer(this);
515 connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
516 connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
517 stateTimer->start(100);
524 void DroneThread::timer()
527 // qDebug() << "thread Timer";
530 cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
531 navData()->setState("**TimeOut**");
538 // cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
542 cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
545 cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
548 float_or_int_t _pitch,_roll,_yaw,_vv;
549 int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
556 // 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);
557 // 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);
558 cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
559 cmd+=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);
565 if(!cmd.isEmpty())sendCmd(cmd);