From e59726e204072ee045a3e8383aa56d2eef8315aa Mon Sep 17 00:00:00 2001
From: "U-PAWEL-X220\\pawel" <pspychalski@gmail.com>
Date: Wed, 4 Jan 2017 19:37:59 +0100
Subject: [PATCH] backend layer for MSP_NAV_POSHOLD

---
 js/fc.js            | 14 +++++++++++++-
 js/msp/MSPCodes.js  |  3 +++
 js/msp/MSPHelper.js | 37 +++++++++++++++++++++++++++++++++++++
 3 files changed, 53 insertions(+), 1 deletion(-)

diff --git a/js/fc.js b/js/fc.js
index 15dbb993..6dc0ec24 100644
--- a/js/fc.js
+++ b/js/fc.js
@@ -43,6 +43,7 @@ var PID_ADVANCED;
 var FILTER_CONFIG;
 var SENSOR_STATUS;
 var SENSOR_CONFIG;
+var NAV_POSHOLD;
 
 var FC = {
     isRatesInDps: function () {
@@ -258,6 +259,17 @@ var FC = {
             accSoftLpfHz: null
         };
 
+        NAV_POSHOLD = {
+            userControlMode: null,
+            maxSpeed: null,
+            maxClimbRate: null,
+            maxManualSpeed: null,
+            maxManualClimbRate: null,
+            maxBankAngle: null,
+            useThrottleMidForAlthold: null,
+            hoverThrottle: null
+        };
+
         _3D = {
             deadband3d_low: 0,
             deadband3d_high: 0,
@@ -677,5 +689,5 @@ var FC = {
         }
 
         return retVal;
-    },
+    }
 };
diff --git a/js/msp/MSPCodes.js b/js/msp/MSPCodes.js
index 8992219e..41edaba7 100644
--- a/js/msp/MSPCodes.js
+++ b/js/msp/MSPCodes.js
@@ -10,6 +10,9 @@ var MSPCodes = {
     MSP_INAV_PID:               6,
     MSP_SET_INAV_PID:           7,
 
+    MSP_NAV_POSHOLD:            12,
+    MSP_SET_NAV_POSHOLD:        13,
+
     // MSP commands for Cleanflight original features
     MSP_CHANNEL_FORWARDING:     32,
     MSP_SET_CHANNEL_FORWARDING: 33,
diff --git a/js/msp/MSPHelper.js b/js/msp/MSPHelper.js
index 712a7146..f508602e 100644
--- a/js/msp/MSPHelper.js
+++ b/js/msp/MSPHelper.js
@@ -866,6 +866,21 @@ var mspHelper = (function (gui) {
                 console.log("MSP_INAV_PID saved");
                 break;
 
+            case MSPCodes.MSP_NAV_POSHOLD:
+                NAV_POSHOLD.userControlMode = data.getUint8(0);
+                NAV_POSHOLD.maxSpeed = data.getUint16(1, true);
+                NAV_POSHOLD.maxClimbRate = data.getUint16(3, true);
+                NAV_POSHOLD.maxManualSpeed = data.getUint16(5, true);
+                NAV_POSHOLD.maxManualClimbRate = data.getUint16(7, true);
+                NAV_POSHOLD.maxBankAngle = data.getUint8(9);
+                NAV_POSHOLD.useThrottleMidForAlthold = data.getUint8(10);
+                NAV_POSHOLD.hoverThrottle = data.getUint16(11, true);
+                break;
+
+            case MSPCodes.MSP_SET_NAV_POSHOLD:
+                console.log('NAV_POSHOLD saved');
+                break;
+
             case MSPCodes.MSP_SET_MODE_RANGE:
                 console.log('Mode range saved');
                 break;
@@ -1172,6 +1187,28 @@ var mspHelper = (function (gui) {
                 buffer.push(0); //reserved
                 break;
 
+            case MSPCodes.MSP_SET_NAV_POSHOLD:
+                buffer.push(NAV_POSHOLD.userControlMode);
+
+                buffer.push(lowByte(NAV_POSHOLD.maxSpeed));
+                buffer.push(highByte(NAV_POSHOLD.maxSpeed));
+
+                buffer.push(lowByte(NAV_POSHOLD.maxClimbRate));
+                buffer.push(highByte(NAV_POSHOLD.maxClimbRate));
+
+                buffer.push(lowByte(NAV_POSHOLD.maxManualSpeed));
+                buffer.push(highByte(NAV_POSHOLD.maxManualSpeed));
+
+                buffer.push(lowByte(NAV_POSHOLD.maxManualClimbRate));
+                buffer.push(highByte(NAV_POSHOLD.maxManualClimbRate));
+
+                buffer.push(NAV_POSHOLD.maxBankAngle);
+                buffer.push(NAV_POSHOLD.useThrottleMidForAlthold);
+
+                buffer.push(lowByte(NAV_POSHOLD.hoverThrottle));
+                buffer.push(highByte(NAV_POSHOLD.hoverThrottle));
+                break;
+
             case MSPCodes.MSP_SET_FILTER_CONFIG:
                 buffer.push(FILTER_CONFIG.gyroSoftLpfHz);