--- /dev/null
+/*==================================================================
+ !
+ ! mardrone application AR-Drone for MeeGo
+
+ ! Copyright (C) 2010 Nokia Corporation and/or its subsidiary(-ies).
+ ! All rights reserved.
+ !
+ ! Author:Kate Alhola kate.alhola@nokia.com
+ !
+ ! GNU Lesser General Public License Usage
+ ! This file may be used under the terms of the GNU Lesser
+ ! General Public License version 2.1 as published by the Free Software
+ ! Foundation and appearing in the file LICENSE.LGPL included in the
+ ! packaging of this file. Please review the following information to
+ ! ensure the GNU Lesser General Public License version 2.1 requirements
+ ! will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
+ !
+ !
+ !
+ *===================================================================*/
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include "dronecontrol.h"
+#include <QDebug>
+#include <QNetworkConfigurationManager>
+#include <QNetworkSession>
+#include <QList>
+
+DroneControl::DroneControl():QObject()
+{
+ qDebug() << "DroneControl::DroneControl";
+
+// ctlSock=new QTcpSocket();
+// ctlSock->bind(QHostAddress::Any,5559);
+// navSock=new QUdpSocket();
+// navSock->bind(QHostAddress::Any,5554)
+ //connect(navSocket,SIGNAL(readyRead()),SLOT(navDataReady()));
+ droneHost.setAddress("192.168.1.1");
+ droneThread=new DroneThread(this,droneHost);
+ connect(droneThread->navData(),SIGNAL(navDataUpdated()),this,SLOT(navDataUpdated()));
+ connect(droneThread->navData(),SIGNAL(stateUpdated()),this,SLOT(statusUpdated()));
+ droneSettings=new QSettings("katix.org","mardrone");
+ m_ctrlActive=false;
+ m_pitch=0;
+ m_roll=0;
+ m_yaw=0;
+ m_vv=0;
+ m_useAccel=droneSettings->value("useAccel",false).toBool();
+ m_useJoyStick=droneSettings->value("useJoyStick",false).toBool();
+ m_ctrlTrsh=droneSettings->value("ctrlTreshold",10.0).toFloat();
+#ifdef QT_SENSORS
+ rSensor=new QRotationSensor();
+ rSensor->start();
+ connect(rSensor,SIGNAL(readingChanged()),this,SLOT(rotationReadingsChanged()));
+#endif
+
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
+
+}
+
+void DroneControl::axisValueChanged(int axis, int value)
+{
+#ifdef HAVE_SDL
+ qDebug()<< "DroneControl::axisValueChanged(" << axis << value << ")";
+#endif
+
+};
+void DroneControl::rotationReadingsChanged()
+{
+#ifdef QT_SENSORS
+ if(m_useAccel && m_ctrlActive) {
+ m_pitch=rSensor->reading()->x()-m_rotRefX;
+ m_roll =rSensor->reading()->y()-m_rotRefY;
+ m_pitch=(fabs(m_pitch)<m_ctrlTrsh ) ? 0:(m_pitch>0 ? m_pitch-m_ctrlTrsh:m_pitch+m_ctrlTrsh);
+ m_roll =(fabs(m_roll )<m_ctrlTrsh ) ? 0:(m_roll>0 ? m_roll- m_ctrlTrsh:m_roll+ m_ctrlTrsh);
+ droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
+ emit pitchRollChanged();
+ //m_rotRefZ=rSensor->reading()->z();
+ //qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
+ }
+#endif
+}
+
+void DroneControl::navDataUpdated()
+{
+ emit navDataChanged();
+
+}
+void DroneControl::statusUpdated()
+{
+ emit statusChanged();
+
+}
+
+void DroneControl::setPitch(float val_)
+{
+ m_pitch=val_;
+// qDebug() << "setPitch=" << val_;
+ droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
+};
+float DroneControl::pitch() {return m_pitch;};
+
+void DroneControl::setRoll(float val_)
+{
+ m_roll=val_;
+// qDebug() << "setRoll=" << val_;
+ droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
+};
+float DroneControl::roll() {return m_roll;};
+void DroneControl::setYaw(float val) {
+ m_yaw=val;
+ // qDebug() << "setYaw=" << val;
+ 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));
+ droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
+};
+float DroneControl::yaw() {return m_yaw;};
+void DroneControl::setVVelocity(float val) {
+ m_vv=val;
+ 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));
+// qDebug() << "setVv=" << val_;
+ droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
+};
+float DroneControl::vVelocity() {return m_vv;};
+void DroneControl::setAltitude(float val_) {m_altitude=val_;};
+float DroneControl::altitude() {return m_altitude;};
+void DroneControl::setEnabled(int val_) { m_enabled=val_;};
+int DroneControl::enabled() {return m_enabled;};
+bool DroneControl::ctrlActive() { return m_ctrlActive;};
+bool DroneControl::emergency() {return m_emergency;};
+
+void DroneControl::setCtrlActive(bool val_)
+{
+#if QT_SENSORS
+ if(val_ && !m_ctrlActive) { // Ctrl switched to active
+ m_rotRefX=rSensor->reading()->x();
+ m_rotRefY=rSensor->reading()->y();
+ m_rotRefZ=rSensor->reading()->z();
+ qDebug("setCtrlActive ref(%3.1f,%3.1f,%3.1f)",m_rotRefX,m_rotRefY,m_rotRefZ);
+ }
+#endif
+ m_ctrlActive=val_;
+ if(!m_ctrlActive) { m_pitch=0;m_roll=0; };
+ droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
+ emit pitchRollChanged();
+
+};
+
+
+void DroneControl::setFly(bool val_)
+{
+ m_fly=val_;
+ qDebug() << "setFly=" << val_;
+ droneThread->setFly(m_fly);
+};
+bool DroneControl::fly() {return m_fly;};
+void DroneControl::setEmergency(bool val_)
+{
+ m_emergency=val_;
+ qDebug() << "setEmergency=" << val_;
+ droneThread->setEmergency(m_emergency);
+};
+
+
+
+// Getters to drone actual valyes sent by drone
+float DroneControl::droneAltitude() { return droneThread->navData()->altitude; };
+float DroneControl::dronePitch() { return droneThread->navData()->pitch; };
+float DroneControl::droneRoll() { return droneThread->navData()->roll; };
+float DroneControl::droneYaw() { return droneThread->navData()->yaw; };
+float DroneControl::droneSpeed() { return droneThread->navData()->vx;};
+float DroneControl::droneVBat() { return droneThread->navData()->vbat;};
+QString DroneControl::decodedStatus() { return droneThread->navData()->decodedState; };
+int DroneControl::pwm_motor1() { return droneThread->navData()->pwm_motor1; };
+int DroneControl::pwm_motor2() { return droneThread->navData()->pwm_motor2; };
+int DroneControl::pwm_motor3() { return droneThread->navData()->pwm_motor3;};
+int DroneControl::pwm_motor4() { return droneThread->navData()->pwm_motor4;};
+
+QString DroneControl::confDroneIp()
+{
+ qDebug() << "confDroneIp:" << droneSettings->value("droneIp","192.168.1.1").toString();
+ return droneSettings->value("droneIp","192.168.1.1").toString();
+};
+void DroneControl::setConfDroneIp(QString ip)
+{
+ qDebug() << "setConfDroneIp:" << ip;
+ droneSettings->setValue("droneIp",ip);
+ emit configChanged();
+};
+QString DroneControl::confActiveUI()
+{
+ qDebug() << "confActiveUI:" << droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
+ return droneSettings->value("activeUI","ardrone_harmattan.qml").toString();
+};
+void DroneControl::setConfActiveUI(QString ui)
+{
+ qDebug() << "setConfActiveUI:" << ui;
+ droneSettings->setValue("activeUI",ui);
+ emit configChanged();
+};
+bool DroneControl::confShowDebug()
+{
+return droneSettings->value("showDebug",true).toBool();
+};
+void DroneControl::setConfShowDebug(bool val)
+{
+ droneSettings->setValue("showDebug",val);
+ emit configChanged();
+};
+bool DroneControl::confShowJSIndicators()
+{
+return droneSettings->value("showJSIndicators",true).toBool();
+};
+void DroneControl::setConfShowJSIndicators(bool val)
+{
+ droneSettings->setValue("showJSIndicators",val);
+ emit configChanged();
+};
+bool DroneControl::confShowHorizon()
+{
+ return droneSettings->value("showHorizon",true).toBool();
+};
+void DroneControl::setConfShowHorizon(bool val)
+{
+ droneSettings->setValue("showHorizon",val);
+ emit configChanged();
+};
+bool DroneControl::confShowGauges()
+{
+ return droneSettings->value("showGuges",true).toBool();
+};
+void DroneControl::setConfShowGauges(bool val)
+{
+ droneSettings->setValue("showGauges",val);
+ emit configChanged();
+};
+
+bool DroneControl::confUseAccel()
+{
+ //return droneSettings->value("useAccel",false).toBool();
+ return m_useAccel; // return cached value
+};
+void DroneControl::setConfUseAccel(bool val)
+{
+ droneSettings->setValue("useAccel",val);
+ m_useAccel=val;
+ emit configChanged();
+};
+bool DroneControl::confUseJoyStick()
+{
+ return droneSettings->value("useJoyStick",false).toBool();
+};
+void DroneControl::setConfUseJoyStick(bool val)
+{
+ droneSettings->setValue("useJoyStick",val);
+ m_useJoyStick=val;
+ emit configChanged();
+};
+
+bool DroneControl::confFullScreen()
+{
+ return droneSettings->value("fullScreen",true).toBool();
+};
+void DroneControl::setConfFullScreen(bool val)
+{
+ droneSettings->setValue("fullScreen",val);
+ emit configChanged();
+};
+
+float DroneControl::confForwardGain()
+{
+ return droneSettings->value("forwardGain",1.0).toFloat();
+};
+void DroneControl::setConfForwardGain(float val)
+{
+ droneSettings->setValue("forwardGain",val);
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
+ emit configChanged();
+};
+float DroneControl::confCtrlTrsh()
+{
+ return m_ctrlTrsh;
+};
+void DroneControl::setConfCtrlTrsh(float val)
+{
+ droneSettings->setValue("ctrlTreshold",val);
+ m_ctrlTrsh=val;
+ emit configChanged();
+};
+
+float DroneControl::confBackwardGain()
+{
+ return droneSettings->value("backwardGain",1.0).toFloat();
+};
+void DroneControl::setConfBackwardGain(float val)
+{
+ droneSettings->setValue("backwardGain",val);
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
+ emit configChanged();
+};
+
+float DroneControl::confLeftGain()
+{
+ return droneSettings->value("leftGain",1.0).toFloat();
+};
+void DroneControl::setConfLeftGain(float val)
+{
+ droneSettings->setValue("leftGain",val);
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
+ emit configChanged();
+};
+float DroneControl::confRightGain()
+{
+ return droneSettings->value("rightGain",1.0).toFloat();
+};
+void DroneControl::setConfRightGain(float val)
+{
+ droneSettings->setValue("rightGain",val);
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
+ emit configChanged();
+};
+
+/*=================================================
+
+ DroneThread class starts here
+
+==================================================*/
+
+
+void DroneThread::setFly(bool fly)
+{
+ if(state==ready) {
+ m_fly=fly;
+ state=flying;
+ stateTimer->setInterval(50); // More frequent updates
+ sendCmd(QString("AT*FTRIM=%1\r").arg(seq++));
+
+ }
+ if(state==flying) {
+ if(!fly) {
+ stateTimer->setInterval(200); // Less frequent updates
+ state=ready;
+ }
+ m_fly=fly;
+ }
+ 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)));
+};
+
+void DroneThread::setEmergency(bool emg)
+{
+ m_emergency=emg;
+ 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)));
+ // if(m_emergency==1)
+ state=ready;
+} ;
+void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
+{
+ m_pitch=pitch*m_fgain;
+ m_roll=roll*m_rgain;
+ m_yaw=yaw;
+ m_vv=vv;
+// 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);
+};
+
+void DroneThread::setDroneGain(float fgain,float bgain,float lgain,float rgain)
+{
+ m_fgain=fgain;
+ m_bgain=bgain;
+ m_rgain=rgain;
+ m_lgain=lgain;
+};
+
+void DroneThread::sendCmd(QString cmd)
+{
+ QByteArray dgram;
+ QString seqCmd=cmd;
+ if(cmd.contains("%")) seqCmd=cmd.arg(seq++);
+ dgram=seqCmd.toLatin1();
+ cmdSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5556);
+ seqCmd.chop(1); // Remove training cr
+// qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
+}
+
+void DroneThread::navDataReady()
+{
+ qint64 l;
+ char buf[2048];
+ QHostAddress host;
+ quint16 port;
+ while(navSock->hasPendingDatagrams()) l=navSock->readDatagram(buf,sizeof(buf),&host,&port);
+// qDebug() << "DroneThread::navDataReady state=" << state <<" l=" << l << "read=" << buf << "from" << host ;
+ nd.parseRawNavData((char *)&buf,l);
+ if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
+ sendCmd(QString("AT*COMWDG=%1\r").arg(seq++));
+ switch(state) {
+ case notInitialized:
+ sendCmd("AT*CONFIG=%1,\"general:navdata_demo\",\"FALSE\"\r");
+ state=initialized;
+ break;
+ case initialized:
+ sendCmd("AT*CTRL=0\r");
+ sendCmd("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r");
+ state=ready;
+ break;
+ case ready:
+ break;
+ }
+}
+
+void DroneThread::sendNav(QString cmd)
+{
+ QByteArray dgram;
+ dgram=cmd.toLatin1();
+// qDebug() << "DroneThread::sendNav= " << cmd+"\n" << "to " << droneHost ;
+ navSock->writeDatagram(dgram.data(),dgram.size(),droneHost,5554);
+}
+
+
+DroneThread::DroneThread(DroneControl *parentp,QHostAddress host)
+{
+
+ struct ip_mreq imreq;
+ qDebug() << "DroneThread::DroneThread";
+ stopped=false;
+ state=notInitialized;
+ parent=parentp;
+ navSock=new QUdpSocket();
+ QList<QNetworkConfiguration> netconfs=QNetworkConfigurationManager().allConfigurations();
+ foreach (QNetworkConfiguration np,netconfs) {
+ qDebug() << "network COnfifuration name " << np.name() << np.bearerName() << np.bearerTypeName();
+ QNetworkSession ns(np);
+ if(ns.interface().addressEntries().empty())
+ qDebug() << "network session " << ns.interface().humanReadableName() << "**NotConfig**";
+ else
+ qDebug() << "network session " << ns.interface().humanReadableName() << ns.interface().addressEntries().first().ip();
+ }
+
+ if(!navSock->bind(QHostAddress::Any,5554))
+ qDebug() << "Cant open any: 5554" << navSock->errorString() ;
+ imreq.imr_multiaddr.s_addr=inet_addr("224.1.1.1");
+ imreq.imr_interface.s_addr=inet_addr("192.168.1.2");
+ setsockopt(navSock->socketDescriptor(),IPPROTO_IP,IP_ADD_MEMBERSHIP,&imreq,sizeof(imreq));
+ cmdSock=new QUdpSocket();
+ cmdSock->bind(QHostAddress::Any,5556);
+ droneHost=host;
+ seq=1;
+ m_pitch=0;
+ m_roll=0;
+ m_yaw=0;
+ m_vv=0;
+ m_fgain=1.0;
+ m_bgain=1.0;
+ m_rgain=1.0;
+ m_lgain=1.0;
+ start();
+
+
+};
+
+void DroneThread::run()
+{
+ qDebug() << "DroneThread::DroneThread";
+ stateTimer=new QTimer(this);
+ connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
+ connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
+ stateTimer->start(1000);
+ while(!stopped) {
+ exec();
+ }
+
+}
+
+void DroneThread::timer()
+{
+ // qDebug() << "thread Timer";
+ switch(state) {
+ case notInitialized:
+ sendNav("AT");
+ break;
+ case initialized:
+ break;
+ case ready:
+ sendCmd(QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28)));
+ break;
+ case flying: {
+ float_or_int_t _pitch,_roll,_yaw,_vv;
+ int r=(m_pitch!=0.0 || m_roll!=0.0)?1:0;
+
+
+ _pitch.f=m_pitch;
+ _roll.f=m_roll;
+ _yaw.f=m_yaw;
+ _vv.f=m_vv;
+// 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);
+// 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);
+ sendCmd(QString("AT*COMWDG=%1\r").arg(seq++));
+ 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));
+
+
+ break;
+ }
+}
+}
+
+
+
projects / mardrone / blobdiff