MSP Interface for Programming PID

gulpfile.js

@@ -106,6 +106,8 @@ sources.js = [
     './js/logicConditionsCollection.js',
     './js/logicConditionsStatus.js',
     './js/globalVariablesStatus.js',
+    './js/programmingPid.js',
+    './js/programmingPidCollection.js',
     './js/vtx.js',
     './main.js',
     './js/tabs.js',

js/fc.js

@@ -23,6 +23,7 @@ var CONFIG,
     LOGIC_CONDITIONS_STATUS,
     GLOBAL_FUNCTIONS,
     GLOBAL_VARIABLES_STATUS,
+    PROGRAMMING_PID,
     SERIAL_CONFIG,
     SENSOR_DATA,
     MOTOR_DATA,
@@ -177,6 +178,7 @@ var FC = {
         LOGIC_CONDITIONS = new LogicConditionsCollection();
         LOGIC_CONDITIONS_STATUS = new LogicConditionsStatus();
         GLOBAL_VARIABLES_STATUS = new GlobalVariablesStatus();
+        PROGRAMMING_PID = new ProgrammingPidCollection();
 
         MIXER_CONFIG = {
             yawMotorDirection: 0,

js/msp/MSPCodes.js

@@ -211,6 +211,8 @@ var MSPCodes = {
     MSP2_INAV_SET_GLOBAL_FUNCTIONS:     0x2025,
     MSP2_INAV_LOGIC_CONDITIONS_STATUS:  0x2026,
     MSP2_INAV_GVAR_STATUS:              0x2027,
+    MSP2_INAV_PROGRAMMING_PID:          0x2028,
+    MSP2_INAV_SET_PROGRAMMING_PID:      0x2029,
 
     MSP2_PID:                           0x2030,
     MSP2_SET_PID:                       0x2031,

js/msp/MSPHelper.js

@@ -537,6 +537,29 @@ var mspHelper = (function (gui) {
                 console.log("Logic conditions saved");
                 break;
 
+            case MSPCodes.MSP2_INAV_PROGRAMMING_PID:
+                PROGRAMMING_PID.flush();
+                if (data.byteLength % 19 === 0) {
+                    for (i = 0; i < data.byteLength; i += 19) {
+                        PROGRAMMING_PID.put(new ProgrammingPid(
+                            data.getInt8(i),                // enabled
+                            data.getInt8(i + 1),            // setpointType
+                            data.getInt32(i + 2, true),     // setpointValue
+                            data.getInt8(i + 6),            // measurementType
+                            data.getInt32(i + 7, true),     // measurementValue
+                            data.getInt16(i + 11, true),    // gainP
+                            data.getInt16(i + 13, true),    // gainI
+                            data.getInt16(i + 15, true),    // gainD
+                            data.getInt16(i + 17, true)     // gainFF
+                        ));
+                    }
+                }
+                break;
+
+            case MSPCodes.MSP2_INAV_SET_PROGRAMMING_PID:
+                console.log("Programming PID saved");
+                break;
+
             case MSPCodes.MSP2_COMMON_MOTOR_MIXER:
                 MOTOR_RULES.flush();
 
@@ -2363,6 +2386,58 @@ var mspHelper = (function (gui) {
         }
     };
 
+    self.loadProgrammingPid = function (callback) {
+        MSP.send_message(MSPCodes.MSP2_INAV_PROGRAMMING_PID, false, false, callback);
+    }
+
+    self.sendProgrammingPid = function (onCompleteCallback) {
+        let nextFunction = sendPid,
+            pidIndex = 0;
+
+        if (PROGRAMMING_PID.getCount() == 0) {
+            onCompleteCallback();
+        } else {
+            nextFunction();
+        }
+
+        function sendPid() {
+
+            let buffer = [];
+
+            // send one at a time, with index, 20 bytes per one condition
+
+            let pid = PROGRAMMING_PID.get()[pidIndex];
+
+            buffer.push(pidIndex);
+            buffer.push(pid.getEnabled());
+            buffer.push(pid.getSetpointType());
+            buffer.push(specificByte(pid.getSetpointValue(), 0));
+            buffer.push(specificByte(pid.getSetpointValue(), 1));
+            buffer.push(specificByte(pid.getSetpointValue(), 2));
+            buffer.push(specificByte(pid.getSetpointValue(), 3));
+            buffer.push(pid.getMeasurementType());
+            buffer.push(specificByte(pid.getMeasurementValue(), 0));
+            buffer.push(specificByte(pid.getMeasurementValue(), 1));
+            buffer.push(specificByte(pid.getMeasurementValue(), 2));
+            buffer.push(specificByte(pid.getMeasurementValue(), 3));
+            buffer.push(specificByte(pid.getGainP(), 0));
+            buffer.push(specificByte(pid.getGainP(), 1));
+            buffer.push(specificByte(pid.getGainI(), 0));
+            buffer.push(specificByte(pid.getGainI(), 1));
+            buffer.push(specificByte(pid.getGainD(), 0));
+            buffer.push(specificByte(pid.getGainD(), 1));
+            buffer.push(specificByte(pid.getGainFF(), 0));
+            buffer.push(specificByte(pid.getGainFF(), 1));
+
+            // prepare for next iteration
+            pidIndex++;
+            if (pidIndex == PROGRAMMING_PID.getCount()) { //This is the last rule. Not pretty, but we have to send all rules
+                nextFunction = onCompleteCallback;
+            }
+            MSP.send_message(MSPCodes.MSP2_INAV_SET_PROGRAMMING_PID, buffer, false, nextFunction);
+        }
+    };
+
     self.sendModeRanges = function (onCompleteCallback) {
         var nextFunction = send_next_mode_range;
 

js/programmingPid.js

@@ -0,0 +1,80 @@
+/*global $,FC*/
+'use strict';
+
+let ProgrammingPid = function (enabled, setpointType, setpointValue, measurementType, measurementValue, gainP, gainI, gainD, gainFF) {
+    let self = {};
+
+    self.getEnabled = function () {
+        return !!enabled;
+    };
+
+    self.setEnabled = function (data) {
+        enabled = !!data;
+    };
+
+    self.getSetpointType = function () {
+        return setpointType;
+    };
+
+    self.setSetpointType = function (data) {
+        setpointType = data;
+    };
+
+    self.getSetpointValue = function () {
+        return setpointValue;
+    };
+
+    self.setSetpointValue = function (data) {
+        setpointValue = data;
+    };
+
+    self.getMeasurementType = function () {
+        return measurementType;
+    };
+
+    self.setMeasurementType = function (data) {
+        measurementType = data;
+    };
+
+    self.getMeasurementValue = function () {
+        return measurementValue;
+    };
+
+    self.setMeasurementValue = function (data) {
+        measurementValue = data;
+    };
+
+    self.getGainP = function () {
+        return gainP;
+    };
+
+    self.setGainP = function (data) {
+        gainP = data;
+    };
+
+    self.getGainI = function () {
+        return gainI;
+    };
+
+    self.setGainI = function (data) {
+        gainI = data;
+    };
+
+    self.getGainD = function () {
+        return gainD;
+    };
+
+    self.setGainD = function (data) {
+        gainD = data;
+    };
+
+    self.getGainFF = function () {
+        return gainFF;
+    };
+
+    self.setGainFF = function (data) {
+        gainFF = data;
+    };
+
+    return self;
+};

js/programmingPidCollection.js

@@ -0,0 +1,33 @@
+'use strict';
+
+let ProgrammingPidCollection = function () {
+
+    let self = {},
+        data = [],
+        $container;
+
+    self.put = function (element) {
+        data.push(element);
+    };
+
+    self.get = function () {
+        return data;
+    };
+
+    self.flush = function () {
+        data = [];
+    };
+
+    self.getCount = function () {
+        return data.length
+    };
+
+    self.open = function () {
+        self.render();
+        $container.show();
+    };
+
+    
+
+    return self;
+};