#include <QNetworkConfigurationManager>
#include <QNetworkSession>
#include <QList>
+#include "navdata.h"
+#include "ardrone_api.h"
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_connected=FALSE;
m_useAccel=droneSettings->value("useAccel",false).toBool();
m_useJoyStick=droneSettings->value("useJoyStick",false).toBool();
m_ctrlTrsh=droneSettings->value("ctrlTreshold",10.0).toFloat();
+ droneHost.setAddress(confDroneIp());
+ droneThread=new DroneThread(this,droneHost);
+ connect(droneThread->navData(),SIGNAL(navDataUpdated()),this,SLOT(navDataUpdated()));
+ connect(droneThread->navData(),SIGNAL(stateUpdated()),this,SLOT(statusUpdated()));
#ifdef QT_SENSORS
rSensor=new QRotationSensor();
rSensor->start();
#endif
}
+void DroneControl::setConnected(bool val_)
+{
+ m_connected=val_;
+ emit connectedChanged();
+}
void DroneControl::navDataUpdated()
{
emit navDataChanged();
m_pitch=val_;
// qDebug() << "setPitch=" << val_;
droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
+ emit pitchRollChanged();
};
float DroneControl::pitch() {return m_pitch;};
m_roll=val_;
// qDebug() << "setRoll=" << val_;
droneThread->setDroneControl(m_pitch,m_roll,m_yaw,m_vv);
+ emit pitchRollChanged();
};
float DroneControl::roll() {return m_roll;};
void DroneControl::setYaw(float 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);
+ emit pitchRollChanged();
};
float DroneControl::yaw() {return m_yaw;};
void DroneControl::setVVelocity(float 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);
+ emit pitchRollChanged();
};
float DroneControl::vVelocity() {return m_vv;};
void DroneControl::setAltitude(float val_) {m_altitude=val_;};
{
QByteArray dgram;
QString seqCmd=cmd;
+ noreply++;
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 ;
+ // qDebug() << "DroneThread::sendCmd= " << seqCmd << "to " << droneHost ;
}
void DroneThread::navDataReady()
char buf[2048];
QHostAddress host;
quint16 port;
+ QString cmd;
+ bool ack;
+ noreply=0; // We got a reply
+ cmd="";ack=false;
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_NAVDATA_BOOTSTRAP)) state=notInitialized;
if(nd.state& (ARDRONE_COM_WATCHDOG_MASK ))
- sendCmd(QString("AT*COMWDG=%1\r").arg(seq++));
+ cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
+ if((nd.state& (ARDRONE_COMMAND_MASK ))) {
+ cmd+=QString("AT*CTRL=%1,%2,0\r").arg(seq++).arg(ACK_CONTROL_MODE);
+ ack=true;
+ };
switch(state) {
case notInitialized:
- sendCmd("AT*CONFIG=%1,\"general:navdata_demo\",\"FALSE\"\r");
- state=initialized;
+ cmd+=QString("AT*CONFIG=%1,\"general:navdata_demo\",\"TRUE\"\r").arg(seq++);
+ // cmd+=QString("AT*CONFIG=%1,\"general:navdata_options\",\"%3\"\r").arg(seq++).arg(NAVDATA_OPTION_FULL_MASK);
+ // cmd+=QString("AT*CONFIG=%1,\"video:codec\",\"%2\"\r").arg(seq++).arg(0x20);
+ state=bootstrap;
+ retry=10;
+ qDebug() << "Connected to drone" << seq << navData()->decodedState;
+ parent->setConnected(TRUE); // We are connected to drone
+ break;
+ case bootstrap:
+ if(ack) state=initialized;
+ else
+ if(--retry<=0) state=notInitialized;
break;
case initialized:
- sendCmd("AT*CTRL=0\r");
- sendCmd("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r");
+ cmd+=QString("AT*CTRL=%1,0\r").arg(seq++);
+ cmd+=QString("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r").arg(seq++);
state=ready;
break;
case ready:
break;
}
+ if(!cmd.isEmpty())sendCmd(cmd); // don't send null cmd
}
void DroneThread::sendNav(QString cmd)
{
struct ip_mreq imreq;
+ QString my_ip;
qDebug() << "DroneThread::DroneThread";
+ my_ip="192.168.1.2";
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();
+ 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();
+ else {
+ qDebug() << "network session " << ns.interface().humanReadableName() << ns.interface().addressEntries().first().ip();
+ // Ubuntu may give wlan0 as "Ethernet"
+ if((np.bearerName()==QString("WLAN")) || (ns.interface().humanReadableName()==QString("wlan0")) ) {
+ my_ip=ns.interface().addressEntries().first().ip().toString();
+ qDebug() << "My IP is " << my_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");
+ imreq.imr_interface.s_addr=inet_addr(my_ip.toAscii());
setsockopt(navSock->socketDescriptor(),IPPROTO_IP,IP_ADD_MEMBERSHIP,&imreq,sizeof(imreq));
cmdSock=new QUdpSocket();
cmdSock->bind(QHostAddress::Any,5556);
m_bgain=1.0;
m_rgain=1.0;
m_lgain=1.0;
+ noreply=0;
start();
stateTimer=new QTimer(this);
connect(stateTimer,SIGNAL(timeout()),this,SLOT(timer()));
connect(navSock,SIGNAL(readyRead()),this,SLOT(navDataReady()));
- stateTimer->start(1000);
+ stateTimer->start(100);
while(!stopped) {
exec();
}
void DroneThread::timer()
{
+ QString cmd;
// qDebug() << "thread Timer";
+ noreply++;
+ if(noreply>50 ) {
+ cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
+ navData()->setState("**TimeOut**");
+ qDebug("Timeout");
+ noreply=0;
+ };
switch(state) {
case notInitialized:
- sendNav("AT");
- break;
- case initialized:
+ sendNav("AT\r");
+ // cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
break;
+ case bootstrap:
+ case initialized:
+ cmd+=QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28));
+ break;
case ready:
- sendCmd(QString("AT*REF=%1,%2\r").arg(seq++).arg((1<<18) + (1<<20) + (1<<22) + (1<<24) +(1<<28)));
+ cmd+=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;
_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));
+ cmd+=QString("AT*COMWDG=%1\r").arg(seq++);
+ 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);
break;
}
}
+ if(!cmd.isEmpty())sendCmd(cmd);
}
projects / mardrone / blobdiff