Acceleration sensor control and basic vision framework

[mardrone] / mardrone / dronecontrol.cpp

diff --git a/mardrone/dronecontrol.cpp b/mardrone/dronecontrol.cpp
index cbc4f23..03862f9 100644 (file)
--- a/mardrone/dronecontrol.cpp
+++ b/mardrone/dronecontrol.cpp
@@ -35,13 +35,34 @@ DroneControl::DroneControl():QObject()
     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();
@@ -68,14 +89,16 @@ void DroneControl::setRoll(float 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_;
+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);
 };
@@ -86,6 +109,27 @@ void DroneControl::setAltitude(float 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;
+};
+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_;
@@ -188,11 +232,13 @@ void DroneControl::setConfShowGauges(bool 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()
@@ -225,7 +271,16 @@ void DroneControl::setConfForwardGain(float 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()
 {
@@ -338,6 +393,7 @@ void DroneThread::navDataReady()
         break;
     case initialized:
         sendCmd("AT*CTRL=0\r");
+        sendCmd("AT*CONFIG=%1,\"detect:detect_type\",\"2\"\r");
         state=ready;
         break;
     case ready: