From eb30a3a7a3cbec5297d905b009dce83401f67c14 Mon Sep 17 00:00:00 2001
From: Gus Grubba <gus@auterion.com>
Date: Sat, 27 Jul 2019 11:44:23 -0400
Subject: [PATCH] Keep track of gimbal position locally.

---
 src/Joystick/Joystick.cc | 32 +++++++++++++++++++++++++++-----
 src/Joystick/Joystick.h  |  7 +++++++
 2 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc
index ec40fe39c..27cc1336e 100644
--- a/src/Joystick/Joystick.cc
+++ b/src/Joystick/Joystick.cc
@@ -680,6 +680,7 @@ void Joystick::startPolling(Vehicle* vehicle)
             disconnect(this, &Joystick::gimbalPitchStep,    _activeVehicle, &Vehicle::gimbalPitchStep);
             disconnect(this, &Joystick::gimbalYawStep,      _activeVehicle, &Vehicle::gimbalYawStep);
             disconnect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
+            disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
         }
         // Always set up the new vehicle
         _activeVehicle = vehicle;
@@ -700,6 +701,7 @@ void Joystick::startPolling(Vehicle* vehicle)
             connect(this, &Joystick::gimbalPitchStep,    _activeVehicle, &Vehicle::gimbalPitchStep);
             connect(this, &Joystick::gimbalYawStep,      _activeVehicle, &Vehicle::gimbalYawStep);
             connect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
+            connect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
             // FIXME: ****
             //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
         }
@@ -724,6 +726,7 @@ void Joystick::stopPolling(void)
             disconnect(this, &Joystick::gimbalPitchStep,    _activeVehicle, &Vehicle::gimbalPitchStep);
             disconnect(this, &Joystick::gimbalYawStep,      _activeVehicle, &Vehicle::gimbalYawStep);
             disconnect(this, &Joystick::centerGimbal,       _activeVehicle, &Vehicle::centerGimbal);
+            disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue);
         }
         // FIXME: ****
         //disconnect(this, &Joystick::buttonActionTriggered,  uas, &UAS::triggerAction);
@@ -1003,20 +1006,39 @@ void Joystick::_executeButtonAction(const QString& action)
     } else if(action == _buttonActionToggleVideoRecord) {
         emit toggleVideoRecord();
     } else if(action == _buttonActionGimbalUp) {
-        emit gimbalPitchStep(1);
+        _pitchStep(1);
     } else if(action == _buttonActionGimbalDown) {
-        emit gimbalPitchStep(-1);
+        _pitchStep(-1);
     } else if(action == _buttonActionGimbalLeft) {
-        emit gimbalYawStep(-1);
+        _yawStep(-1);
     } else if(action == _buttonActionGimbalRight) {
-        emit gimbalYawStep(1);
+        _yawStep(1);
     } else if(action == _buttonActionGimbalCenter) {
-        emit centerGimbal();
+        _localPitch = 0.0;
+        _localYaw   = 0.0;
+        emit gimbalControlValue(0.0, 0.0);
     } else {
         qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
     }
 }
 
+void Joystick::_pitchStep(int direction)
+{
+    _localPitch += static_cast<double>(direction);
+    //-- Arbitrary range
+    if(_localPitch < -90.0) _localPitch = -90.0;
+    if(_localPitch >  35.0) _localPitch =  35.0;
+    emit gimbalControlValue(_localPitch, _localYaw);
+}
+
+void Joystick::_yawStep(int direction)
+{
+    _localYaw += static_cast<double>(direction);
+    if(_localYaw < -180.0) _localYaw = -180.0;
+    if(_localYaw >  180.0) _localYaw =  180.0;
+    emit gimbalControlValue(_localPitch, _localYaw);
+}
+
 bool Joystick::_validAxis(int axis)
 {
     if(axis >= 0 && axis < _axisCount) {
diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h
index 60c7b75d1..17a5d9560 100644
--- a/src/Joystick/Joystick.h
+++ b/src/Joystick/Joystick.h
@@ -213,6 +213,7 @@ signals:
     void gimbalPitchStep            (int direction);
     void gimbalYawStep              (int direction);
     void centerGimbal               ();
+    void gimbalControlValue         (double pitch, double yaw);
     void setArmed                   (bool arm);
     void setVtolInFwdFlight         (bool set);
     void setFlightMode              (const QString& flightMode);
@@ -230,6 +231,11 @@ protected:
     void    _handleAxis             ();
     void    _handleButtons          ();
 
+    void    _pitchStep              (int direction);
+    void    _yawStep                (int direction);
+    double  _localYaw       = 0.0;
+    double  _localPitch     = 0.0;
+
 private:
     virtual bool _open      ()          = 0;
     virtual void _close     ()          = 0;
@@ -290,6 +296,7 @@ protected:
     QStringList                     _availableActionTitles;
     MultiVehicleManager*            _multiVehicleManager = nullptr;
 
+
 private:
     static const char*  _rgFunctionSettingsKey[maxFunction];
 
-- 
2.22.0