+/*==================================================================
+ !
+ ! 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 "dronecontrol.h"
#include <QDebug>
connect(droneThread->navData(),SIGNAL(navDataUpdated()),this,SLOT(navDataUpdated()));
connect(droneThread->navData(),SIGNAL(stateUpdated()),this,SLOT(statusUpdated()));
droneSettings=new QSettings("katix.org","ardrone");
+ m_ctrlActive=false;
m_pitch=0;
m_roll=0;
m_yaw=0;
m_vv=0;
+ m_useAccel=droneSettings->value("useAccel",false).toBool();
+ m_ctrlTrsh=droneSettings->value("ctrlTreshold",10.0).toFloat();
+ rSensor=new QRotationSensor();
+ rSensor->start();
+ connect(rSensor,SIGNAL(readingChanged()),this,SLOT(rotationReadingsChanged()));
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
}
+
+void DroneControl::rotationReadingsChanged()
+{
+ 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);
+ }
+}
+
void DroneControl::navDataUpdated()
{
emit navDataChanged();
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_;
+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_;
+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::altitude() {return m_altitude;};
void DroneControl::setEnabled(int val_) { m_enabled=val_;};
int DroneControl::enabled() {return m_enabled;};
+
+bool DroneControl::ctrlActive()
+{
+ return m_ctrlActive;
+};
+void DroneControl::setCtrlActive(bool val_)
+{
+ 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);
+ }
+ 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_;
};
bool DroneControl::confUseAccel()
{
- return droneSettings->value("useAccel",false).toBool();
+ //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()
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()
{
void DroneControl::setConfBackwardGain(float val)
{
droneSettings->setValue("backwardGain",val);
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
emit configChanged();
};
void DroneControl::setConfLeftGain(float val)
{
droneSettings->setValue("leftGain",val);
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
emit configChanged();
};
float DroneControl::confRightGain()
void DroneControl::setConfRightGain(float val)
{
droneSettings->setValue("rightGain",val);
+ droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
emit configChanged();
};
} ;
void DroneThread::setDroneControl(float pitch,float roll,float yaw,float vv)
{
- m_pitch=pitch/100.0;
- m_roll=roll/100.0;
+ m_pitch=pitch/200.0*m_fgain;
+ m_roll=roll/200.0*m_rgain;
m_yaw=yaw/200.0;
m_vv=vv/200.0;
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);
break;
case initialized:
sendCmd("AT*CTRL=0\r");
+ sendCmd("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r");
state=ready;
break;
case ready:
projects / mardrone / blobdiff