diff --git a/gulpfile.js b/gulpfile.js index ce98ee23..0ee23e7c 100644 --- a/gulpfile.js +++ b/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', diff --git a/js/fc.js b/js/fc.js index 8ad43d7a..6f7713a1 100644 --- a/js/fc.js +++ b/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, diff --git a/js/msp/MSPCodes.js b/js/msp/MSPCodes.js index 028386f6..c5754645 100644 --- a/js/msp/MSPCodes.js +++ b/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, diff --git a/js/msp/MSPHelper.js b/js/msp/MSPHelper.js index de43f68c..932d7269 100644 --- a/js/msp/MSPHelper.js +++ b/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; diff --git a/js/programmingPid.js b/js/programmingPid.js new file mode 100644 index 00000000..429e702a --- /dev/null +++ b/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; +}; \ No newline at end of file diff --git a/js/programmingPidCollection.js b/js/programmingPidCollection.js new file mode 100644 index 00000000..96782902 --- /dev/null +++ b/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; +}; \ No newline at end of file