From 31dad7a8260203fa5312a2493259586a25ed8af8 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 23 Jan 2017 21:03:29 +0100
Subject: [PATCH] backend for MSP_POSITION_ESTIMATION_CONFIG

---
 js/fc.js            | 11 +++++++++++
 js/msp/MSPCodes.js  |  3 +++
 js/msp/MSPHelper.js | 34 ++++++++++++++++++++++++++++++++++
 3 files changed, 48 insertions(+)

diff --git a/js/fc.js b/js/fc.js
index e34e75be..9b67badf 100644
--- a/js/fc.js
+++ b/js/fc.js
@@ -44,6 +44,7 @@ var FILTER_CONFIG;
 var SENSOR_STATUS;
 var SENSOR_CONFIG;
 var NAV_POSHOLD;
+var POSITION_ESTIMATOR;
 
 var FC = {
     isRatesInDps: function () {
@@ -337,6 +338,16 @@ var FC = {
             nrf24rx_id: 0
         };
 
+        POSITION_ESTIMATOR = {
+            w_z_baro_p: null,
+            w_z_gps_p: null,
+            w_z_gps_v: null,
+            w_xy_gps_p: null,
+            w_xy_gps_v: null,
+            gps_min_sats: null,
+            use_gps_velned: null
+        };
+
         FAILSAFE_CONFIG = {
             failsafe_delay: 0,
             failsafe_off_delay: 0,
diff --git a/js/msp/MSPCodes.js b/js/msp/MSPCodes.js
index f27564c5..507c5dcf 100644
--- a/js/msp/MSPCodes.js
+++ b/js/msp/MSPCodes.js
@@ -13,6 +13,9 @@ var MSPCodes = {
     MSP_NAV_POSHOLD:            12,
     MSP_SET_NAV_POSHOLD:        13,
 
+    MSP_POSITION_ESTIMATION_CONFIG: 16,
+    MSP_SET_POSITION_ESTIMATION_CONFIG: 17,
+
     // 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 3ef295ae..fa86f879 100644
--- a/js/msp/MSPHelper.js
+++ b/js/msp/MSPHelper.js
@@ -900,6 +900,20 @@ var mspHelper = (function (gui) {
                 console.log('NAV_POSHOLD saved');
                 break;
 
+            case MSPCodes.MSP_POSITION_ESTIMATION_CONFIG:
+                POSITION_ESTIMATOR.w_z_baro_p = data.getUint16(0, true);
+                POSITION_ESTIMATOR.w_z_gps_p = data.getUint16(2, true);
+                POSITION_ESTIMATOR.w_z_gps_v = data.getUint16(4, true);
+                POSITION_ESTIMATOR.w_xy_gps_p = data.getUint16(6, true);
+                POSITION_ESTIMATOR.w_xy_gps_v = data.getUint16(8, true);
+                POSITION_ESTIMATOR.gps_min_sats = data.getUint8(10);
+                POSITION_ESTIMATOR.use_gps_velned = data.getUint8(11);
+                break;
+
+            case MSPCodes.MSP_SET_POSITION_ESTIMATION_CONFIG:
+                console.log('POSITION_ESTIMATOR saved');
+                break;
+
             case MSPCodes.MSP_SET_MODE_RANGE:
                 console.log('Mode range saved');
                 break;
@@ -1219,6 +1233,26 @@ var mspHelper = (function (gui) {
                 buffer.push(highByte(NAV_POSHOLD.hoverThrottle));
                 break;
 
+            case MSPCodes.MSP_SET_POSITION_ESTIMATION_CONFIG:
+                buffer.push(lowByte(POSITION_ESTIMATOR.w_z_baro_p));
+                buffer.push(highByte(POSITION_ESTIMATOR.w_z_baro_p));
+
+                buffer.push(lowByte(POSITION_ESTIMATOR.w_z_gps_p));
+                buffer.push(highByte(POSITION_ESTIMATOR.w_z_gps_p));
+
+                buffer.push(lowByte(POSITION_ESTIMATOR.w_z_gps_v));
+                buffer.push(highByte(POSITION_ESTIMATOR.w_z_gps_v));
+
+                buffer.push(lowByte(POSITION_ESTIMATOR.w_xy_gps_p));
+                buffer.push(highByte(POSITION_ESTIMATOR.w_xy_gps_p));
+
+                buffer.push(lowByte(POSITION_ESTIMATOR.w_xy_gps_v));
+                buffer.push(highByte(POSITION_ESTIMATOR.w_xy_gps_v));
+
+                buffer.push(POSITION_ESTIMATOR.gps_min_sats);
+                buffer.push(POSITION_ESTIMATOR.use_gps_velned);
+                break;
+
             case MSPCodes.MSP_SET_FILTER_CONFIG:
                 buffer.push(FILTER_CONFIG.gyroSoftLpfHz);