controll gains for roll and pitch implemented

[mardrone] / mardrone / dronecontrol.cpp

diff --git a/mardrone/dronecontrol.cpp b/mardrone/dronecontrol.cpp
index c205edb..cbc4f23 100644 (file)
--- a/mardrone/dronecontrol.cpp
+++ b/mardrone/dronecontrol.cpp
@@ -39,6 +39,7 @@ DroneControl::DroneControl():QObject()
     m_roll=0;
     m_yaw=0;
     m_vv=0;
+    droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
 
 }
 void DroneControl::navDataUpdated()
@@ -221,6 +222,7 @@ float DroneControl::confForwardGain()
 void DroneControl::setConfForwardGain(float val)
 {
     droneSettings->setValue("forwardGain",val);
+    droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
     emit configChanged();
 };
 
@@ -232,6 +234,7 @@ float DroneControl::confBackwardGain()
 void DroneControl::setConfBackwardGain(float val)
 {
     droneSettings->setValue("backwardGain",val);
+    droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
     emit configChanged();
 };
 
@@ -242,6 +245,7 @@ float DroneControl::confLeftGain()
 void DroneControl::setConfLeftGain(float val)
 {
     droneSettings->setValue("leftGain",val);
+    droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
     emit configChanged();
 };
 float DroneControl::confRightGain()
@@ -251,6 +255,7 @@ float DroneControl::confRightGain()
 void DroneControl::setConfRightGain(float val)
 {
     droneSettings->setValue("rightGain",val);
+    droneThread->setDroneGain(confForwardGain(),confBackwardGain(),confLeftGain(),confRightGain());
     emit configChanged();
 };
 
@@ -289,8 +294,8 @@ void DroneThread::setEmergency(bool emg)
 }   ;
 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);