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>
33 #include "ardrone_api.h"
35 DroneControl::DroneControl():QObject()
37 qDebug() << "DroneControl::DroneControl";
41 droneSettings=new QSettings("katix.org","mardrone");
48 m_useAccel=droneSettings->value("useAccel",false).toBool();
49 m_useJoyStick=droneSettings->value("useJoyStick",false).toBool();
50 m_ctrlTrsh=droneSettings->value("ctrlTreshold",10.0).toFloat();
51 droneHost.setAddress(confDroneIp());
52 droneThread=new DroneThread(this,droneHost);
53 connect(droneThread->navData(),SIGNAL(navDataUpdated()),this,SLOT(navDataUpdated()));
54 connect(droneThread->navData(),SIGNAL(stateUpdated()),this,SLOT(statusUpdated()));
56 rSensor=new QRotationSensor();
58 connect(rSensor,SIGNAL(readingChanged()),this,SLOT(rotationReadingsChanged()));
61 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
65 void DroneControl::axisValueChanged(int axis, int value)
68 qDebug()<< "DroneControl::axisValueChanged(" << axis << value << ")";
72 void DroneControl::rotationReadingsChanged()
75 if(m_useAccel && m_ctrlActive) {
76 m_pitch=rSensor->reading()->x()-m_rotRefX;
77 m_roll =rSensor->reading()->y()-m_rotRefY;
78 m_pitch=(fabs(m_pitch)<m_ctrlTrsh ) ? 0:(m_pitch>0 ? m_pitch-m_ctrlTrsh:m_pitch+m_ctrlTrsh);
79 m_roll =(fabs(m_roll )<m_ctrlTrsh ) ? 0:(m_roll>0 ? m_roll- m_ctrlTrsh:m_roll+ m_ctrlTrsh);
80 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
81 emit pitchRollChanged();
82 //m_rotRefZ=rSensor->reading()->z();
83 //qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
88 void DroneControl::setConnected(bool val_)
91 emit connectedChanged();
93 void DroneControl::navDataUpdated()
95 emit navDataChanged();
98 void DroneControl::statusUpdated()
100 emit statusChanged();
104 void DroneControl::setPitch(float val_)
107 // qDebug() << "setPitch=" << val_;
108 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
109 emit pitchRollChanged();
111 float DroneControl::pitch() {return m_pitch;};
113 void DroneControl::setRoll(float val_)
116 // qDebug() << "setRoll=" << val_;
117 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
118 emit pitchRollChanged();
120 float DroneControl::roll() {return m_roll;};
121 void DroneControl::setYaw(float val) {
123 // qDebug() << "setYaw=" << val;
124 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));
125 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
126 emit pitchRollChanged();
128 float DroneControl::yaw() {return m_yaw;};
129 void DroneControl::setVVelocity(float val) {
131 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));
132 // qDebug() << "setVv=" << val_;
133 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
134 emit pitchRollChanged();
136 float DroneControl::vVelocity() {return m_vv;};
137 void DroneControl::setAltitude(float val_) {m_altitude=val_;};
138 float DroneControl::altitude() {return m_altitude;};
139 void DroneControl::setEnabled(int val_) { m_enabled=val_;};
140 int DroneControl::enabled() {return m_enabled;};
141 bool DroneControl::ctrlActive() { return m_ctrlActive;};
142 bool DroneControl::emergency() {return m_emergency;};
144 void DroneControl::setCtrlActive(bool val_)
147 if(val_ && !m_ctrlActive) { // Ctrl switched to active
148 m_rotRefX=rSensor->reading()->x();
149 m_rotRefY=rSensor->reading()->y();
150 m_rotRefZ=rSensor->reading()->z();
151 qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
155 if(!m_ctrlActive) { m_pitch=0;m_roll=0; };
156 droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
157 emit pitchRollChanged();
162 void DroneControl::setFly(bool val_)
165 qDebug() << "setFly=" << val_;
166 droneThread->setFly(m_fly);
168 bool DroneControl::fly() {return m_fly;};
169 void DroneControl::setEmergency(bool val_)
172 qDebug() << "setEmergency=" << val_;
173 droneThread->setEmergency(m_emergency);
176 bool DroneControl::recordLog()
178 return droneThread? droneThread->getRecordLog():false;
181 void DroneControl::setRecordLog(bool val_)
183 if(droneThread) droneThread->setRecordLog(val_);
185 QString DroneControl::logFileName()
187 return droneThread? droneThread->getLogFileName():"";
189 void DroneControl::setLogFileName(QString val_)
191 if(droneThread) droneThread->setLogFileName(val_);
193 unsigned int DroneControl::logSeq()
195 return droneThread? droneThread->getLogSeq():0;
198 void DroneControl::setLogSeq(int val_)
200 if(droneThread) droneThread->setLogSeq(val_);
205 // Getters to drone actual valyes sent by drone
206 float DroneControl::droneAltitude() { return droneThread->navData()->altitude; };
207 float DroneControl::dronePitch() { return droneThread->navData()->pitch; };
208 float DroneControl::droneRoll() { return droneThread->navData()->roll; };
209 float DroneControl::droneYaw() { return droneThread->navData()->yaw; };
210 float DroneControl::droneSpeed() { return droneThread->navData()->vx;};
211 float DroneControl::droneVBat() { return droneThread->navData()->vbat;};
212 QString DroneControl::decodedStatus() { return droneThread->navData()->decodedState; };
213 int DroneControl::pwm_motor1() { return droneThread->navData()->pwm_motor1; };
214 int DroneControl::pwm_motor2() { return droneThread->navData()->pwm_motor2; };
215 int DroneControl::pwm_motor3() { return droneThread->navData()->pwm_motor3;};
216 int DroneControl::pwm_motor4() { return droneThread->navData()->pwm_motor4;};
221 Setters and Getters for Config variables
227 QString DroneControl::confDroneIp()
229 qDebug() << "confDroneIp:" << droneSettings->value("droneIp","192.168.1.1").toString();
230 return droneSettings->value("droneIp","192.168.1.1").toString();
232 void DroneControl::setConfDroneIp(QString ip)
234 qDebug() << "setConfDroneIp:" << ip;
235 droneSettings->setValue("droneIp",ip);
236 emit configChanged();
238 QString DroneControl::confActiveUI()
240 qDebug() << "confActiveUI:" << droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
241 return droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
243 void DroneControl::setConfActiveUI(QString ui)
245 qDebug() << "setConfActiveUI:" << ui;
246 droneSettings->setValue("activeUI",ui);
247 emit configChanged();
249 bool DroneControl::confShowDebug()
251 return droneSettings->value("showDebug",true).toBool();
253 void DroneControl::setConfShowDebug(bool val)
255 droneSettings->setValue("showDebug",val);
256 emit configChanged();
258 bool DroneControl::confShowJSIndicators()
260 return droneSettings->value("showJSIndicators",true).toBool();
262 void DroneControl::setConfShowJSIndicators(bool val)
264 droneSettings->setValue("showJSIndicators",val);
265 emit configChanged();
267 bool DroneControl::confShowHorizon()
269 return droneSettings->value("showHorizon",true).toBool();
271 void DroneControl::setConfShowHorizon(bool val)
273 droneSettings->setValue("showHorizon",val);
274 emit configChanged();
276 bool DroneControl::confShowGauges()
278 return droneSettings->value("showGuges",true).toBool();
280 void DroneControl::setConfShowGauges(bool val)
282 droneSettings->setValue("showGauges",val);
283 emit configChanged();
286 bool DroneControl::confUseAccel()
288 //return droneSettings->value("useAccel",false).toBool();
289 return m_useAccel; // return cached value
291 void DroneControl::setConfUseAccel(bool val)
293 droneSettings->setValue("useAccel",val);
295 emit configChanged();
297 bool DroneControl::confUseJoyStick()
299 return droneSettings->value("useJoyStick",false).toBool();
301 void DroneControl::setConfUseJoyStick(bool val)
303 droneSettings->setValue("useJoyStick",val);
305 emit configChanged();
308 int DroneControl::confSimuMode()
310 return droneSettings->value("SimuMode",true).toInt();
312 void DroneControl::setConfSimuMode(int val)
314 droneSettings->setValue("SimuMode",val);
315 emit configChanged();
318 bool DroneControl::confFullScreen()
320 return droneSettings->value("fullScreen",true).toBool();
322 void DroneControl::setConfFullScreen(bool val)
324 droneSettings->setValue("fullScreen",val);
325 emit configChanged();
329 float DroneControl::confForwardGain()
331 return droneSettings->value("forwardGain",1.0).toFloat();
333 void DroneControl::setConfForwardGain(float val)
335 droneSettings->setValue("forwardGain",val);
336 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
337 emit configChanged();
339 float DroneControl::confCtrlTrsh()
343 void DroneControl::setConfCtrlTrsh(float val)
345 droneSettings->setValue("ctrlTreshold",val);
347 emit configChanged();
350 float DroneControl::confBackwardGain()
352 return droneSettings->value("backwardGain",1.0).toFloat();
354 void DroneControl::setConfBackwardGain(float val)
356 droneSettings->setValue("backwardGain",val);
357 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
358 emit configChanged();
361 float DroneControl::confLeftGain()
363 return droneSettings->value("leftGain",1.0).toFloat();
365 void DroneControl::setConfLeftGain(float val)
367 droneSettings->setValue("leftGain",val);
368 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
369 emit configChanged();
371 float DroneControl::confRightGain()
373 return droneSettings->value("rightGain",1.0).toFloat();
375 void DroneControl::setConfRightGain(float val)
377 droneSettings->setValue("rightGain",val);
378 droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
379 emit configChanged();
382 QString DroneControl::errorString()
384 return m_errorString;
386 void DroneControl::setErrorString(QString val_)
391 /*=================================================
393 DroneThread class starts here
395 ==================================================*/
397 DroneThread::DroneThread(DroneControl *parentp,QHostAddress host)
400 struct ip_mreq imreq;
402 qDebug() << "DroneThread::DroneThread";
405 state=notInitialized;
407 navSock=new QUdpSocket();
408 QList<QNetworkConfiguration> netconfs=QNetworkConfigurationManager().allConfigurations();
409 foreach (QNetworkConfiguration np,netconfs) {
410 qDebug() << "network Confifuration name " << np.name() << np.bearerName() << np.bearerTypeName();
411 QNetworkSession ns(np);
412 if(ns.interface().addressEntries().empty())
413 qDebug() << "network session " << ns.interface().humanReadableName() << "**NotConfig**";
415 qDebug() << "network session " << ns.interface().humanReadableName() << ns.interface().addressEntries().first().ip();
416 // Ubuntu may give wlan0 as "Ethernet"
417 if((np.bearerName()==QString("WLAN")) || (ns.interface().humanReadableName()==QString("wlan0")) ) {
418 my_ip=ns.interface().addressEntries().first().ip().toString();
419 qDebug() << "My IP is " << my_ip;
423 if(!navSock->bind(QHostAddress::Any,5554))
424 qDebug() << "Cant open any: 5554" << navSock->errorString() ;
427 imreq.imr_multiaddr.s_addr=inet_addr("224.1.1.1");
428 imreq.imr_interface.s_addr=inet_addr(my_ip.toAscii());
429 setsockopt(navSock->socketDescriptor(),IPPROTO_IP,IP_ADD_MEMBERSHIP,&imreq,sizeof(imreq));
432 if(!navSock->joinMulticastGroup(QHostAddress("224.1.1.1")))
433 qDebug() << "can't join multicast Group 224.1.1.1" << navSock->errorString();
435 cmdSock=new QUdpSocket();
436 cmdSock->bind(QHostAddress::Any,5556);
455 void DroneThread::setFly(bool fly)
460 stateTimer->setInterval(50); // More frequent updates
461 sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
466 stateTimer->setInterval(200); // Less frequent updates
471 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)));
474 void DroneThread::setRecordLog(bool log)
476 qDebug() << "DroneThread::setRecordLog" << log;
477 QDateTime fileDate=QDateTime::currentDateTime();
478 if(!recordLog && log) {
479 if(logFileName.isEmpty()) logFileName="dronerec"+fileDate.toString("yyyyMMddhhmm");
480 qDebug() << "logFileName=" << logFileName;
481 //emit frameSeqChanged();
484 if(!recordLog && logFile) { // Stop recording
494 bool DroneThread::getRecordLog()
499 QString DroneThread::getLogFileName()
504 void DroneThread::setLogFileName(QString name)
509 unsigned int DroneThread::getLogSeq()
514 void DroneThread::setLogSeq(unsigned int val)
519 void DroneThread::setEmergency(bool emg)
522 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)));
523 // if(m_emergency==1)
526 void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
528 m_pitch=pitch*m_fgain;
532 // 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);
535 void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
543 void DroneThread::sendCmd(QString cmd)
548 if(cmd.contains("%")) seqCmd=cmd.arg(seq++);
549 dgram=seqCmd.toLatin1();
550 cmdSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5556);
551 seqCmd.chop(1); // Remove training cr
552 // qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
555 void DroneThread::navDataReady()
563 noreply=0; // We got a reply
565 while(navSock->hasPendingDatagrams()) l=navSock->readDatagram(buf,sizeof(buf),&host,&port);
566 // qDebug() << "DroneThread::navDataReady state=" << state <<" l=" << l << "read=" << buf << "from" << host ;
567 nd.parseRawNavData((char *)&buf,l);
568 if(nd.state& (ARDRONE_NAVDATA_BOOTSTRAP)) state=notInitialized;
569 if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
570 cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
571 if((nd.state& (ARDRONE_COMMAND_MASK ))) {
572 cmd+=QString("AT*CTRL=%1,%2,0\r").arg(seq++).arg(ACK_CONTROL_MODE);
577 cmd+=QString("AT*CONFIG=%1,\"general:navdata_demo\",\"TRUE\"\r").arg(seq++);
578 // cmd+=QString("AT*CONFIG=%1,\"general:navdata_options\",\"%3\"\r").arg(seq++).arg(NAVDATA_OPTION_FULL_MASK);
579 // cmd+=QString("AT*CONFIG=%1,\"video:codec\",\"%2\"\r").arg(seq++).arg(0x20);
582 qDebug() << "Connected to drone" << seq << navData()->decodedState;
583 parent->setConnected(TRUE); // We are connected to drone
586 if(ack) state=initialized;
588 if(--retry<=0) state=notInitialized;
591 cmd+=QString("AT*CTRL=%1,0\r").arg(seq++);
592 cmd+=QString("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r").arg(seq++);
598 if(!cmd.isEmpty())sendCmd(cmd); // don't send null cmd
601 void DroneThread::sendNav(QString cmd)
604 dgram=cmd.toLatin1();
605 // qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
606 navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
611 void DroneThread::run()
613 qDebug() << "DroneThread::DroneThread";
614 stateTimer=new QTimer(this);
615 connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
616 connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
617 stateTimer->start(100);
624 void DroneThread::timer()
627 // qDebug() << "thread Timer";
630 cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
631 navData()->setState("**TimeOut**");
632 // qDebug("Timeout");
638 // cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
642 cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
645 cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
648 float_or_int_t _pitch,_roll,_yaw,_vv;
649 int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
656 // 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);
657 // 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);
658 cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
659 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);
665 if(!cmd.isEmpty())sendCmd(cmd);