diff --git a/src/js/fc.js b/src/js/fc.js index d258f75f..932f4399 100644 --- a/src/js/fc.js +++ b/src/js/fc.js @@ -62,6 +62,7 @@ var COPY_PROFILE; var VTX_CONFIG; var VTXTABLE_BAND; var VTXTABLE_POWERLEVEL; +var MULTIPLE_MSP; var DEFAULT; var FC = { @@ -533,6 +534,10 @@ var FC = { vtxtable_powerlevel_label: 0, }; + MULTIPLE_MSP = { + msp_commands: [], + }; + DEFAULT = { gyro_lowpass_hz: 100, gyro_lowpass_dyn_min_hz: 150, diff --git a/src/js/msp/MSPCodes.js b/src/js/msp/MSPCodes.js index b0319e26..919ccd2f 100644 --- a/src/js/msp/MSPCodes.js +++ b/src/js/msp/MSPCodes.js @@ -156,6 +156,8 @@ var MSPCodes = { MSP_SET_VTXTABLE_BAND: 227, MSP_SET_VTXTABLE_POWERLEVEL: 228, + MSP_MULTIPLE_MSP: 230, + MSP_MODE_RANGES_EXTRA: 238, MSP_SET_ACC_TRIM: 239, MSP_ACC_TRIM: 240, diff --git a/src/js/msp/MSPHelper.js b/src/js/msp/MSPHelper.js index 075cc2be..eab429a6 100644 --- a/src/js/msp/MSPHelper.js +++ b/src/js/msp/MSPHelper.js @@ -35,6 +35,8 @@ function MspHelper () { }; self.SIGNATURE_LENGTH = 32; + + self.mspMultipleCache = []; } MspHelper.prototype.reorderPwmProtocols = function (protocol) { @@ -1469,6 +1471,51 @@ MspHelper.prototype.process_data = function(dataHandler) { case MSPCodes.MSP_SET_RTC: console.log('Real time clock set'); break; + + case MSPCodes.MSP_MULTIPLE_MSP: + + let hasReturnedSomeCommand = false; // To avoid infinite loops + + while (data.offset < data.byteLength) { + + hasReturnedSomeCommand = true; + + let command = self.mspMultipleCache.shift(); + let payloadSize = data.readU8(); + + if (payloadSize != 0) { + + let currentDataHandler = { + code : command, + dataView : new DataView(data.buffer, data.offset, payloadSize), + callbacks : [], + } + + self.process_data(currentDataHandler); + + data.offset += payloadSize; + } + } + + if (hasReturnedSomeCommand) { + // Send again MSP messages missing, the buffer in the FC was too small + if (self.mspMultipleCache.length > 0) { + + var partialBuffer = []; + for (let i = 0; i < self.mspMultipleCache.length; i++) { + partialBuffer.push8(self.mspMultipleCache[i]); + } + + MSP.send_message(MSPCodes.MSP_MULTIPLE_MSP, partialBuffer, false, dataHandler.callbacks); + dataHandler.callbacks = []; + } + } else { + console.log("MSP Multiple can't process the command"); + self.mspMultipleCache = []; + } + + break; + default: console.log('Unknown code detected: ' + code); } else { @@ -1508,6 +1555,7 @@ MspHelper.prototype.process_data = function(dataHandler) { MspHelper.prototype.crunch = function(code) { var buffer = []; var self = this; + switch (code) { case MSPCodes.MSP_SET_FEATURE_CONFIG: var featureMask = FEATURE_CONFIG.features.getMask(); @@ -2064,6 +2112,18 @@ MspHelper.prototype.crunch = function(code) { break; + case MSPCodes.MSP_MULTIPLE_MSP: + + while (MULTIPLE_MSP.msp_commands.length > 0) { + + let mspCommand = MULTIPLE_MSP.msp_commands.shift(); + + self.mspMultipleCache.push(mspCommand); + buffer.push8(mspCommand); + } + + break; + default: return false; }