diff --git a/src/js/injected_methods.js b/src/js/injected_methods.js index 5bce5a18..ca708062 100644 --- a/src/js/injected_methods.js +++ b/src/js/injected_methods.js @@ -89,4 +89,8 @@ DataView.prototype.read32 = function() { } else { return null; } -}; \ No newline at end of file +}; + +DataView.prototype.remaining = function() { + return this.byteLength - this.offset; +}; diff --git a/src/js/msp.js b/src/js/msp.js index d346d737..954c52e5 100644 --- a/src/js/msp.js +++ b/src/js/msp.js @@ -1,6 +1,39 @@ 'use strict'; var MSP = { + symbols: { + BEGIN: '$'.charCodeAt(0), + PROTO_V1: 'M'.charCodeAt(0), + PROTO_V2: 'X'.charCodeAt(0), + FROM_MWC: '>'.charCodeAt(0), + TO_MWC: '<'.charCodeAt(0), + UNSUPPORTED: '!'.charCodeAt(0), + }, + constants: { + PROTOCOL_V1: 1, + PROTOCOL_V2: 2, + JUMBO_FRAME_MIN_SIZE: 255, + }, + decoder_states: { + IDLE: 0, + PROTO_IDENTIFIER: 1, + DIRECTION_V1: 2, + DIRECTION_V2: 3, + FLAG_V2: 4, + PAYLOAD_LENGTH_V1: 5, + PAYLOAD_LENGTH_JUMBO_LOW: 6, + PAYLOAD_LENGTH_JUMBO_HIGH: 7, + PAYLOAD_LENGTH_V2_LOW: 8, + PAYLOAD_LENGTH_V2_HIGH: 9, + CODE_V1: 10, + CODE_JUMBO_V1: 11, + CODE_V2_LOW: 12, + CODE_V2_HIGH: 13, + PAYLOAD_V1: 14, + PAYLOAD_V2: 15, + CHECKSUM_V1: 16, + CHECKSUM_V2: 17, + }, state: 0, message_direction: 1, code: 0, @@ -27,112 +60,166 @@ var MSP = { for (var i = 0; i < data.length; i++) { switch (this.state) { - case 0: // sync char 1 - if (data[i] == 36) { // $ - this.state++; - } - break; - case 1: // sync char 2 - if (data[i] == 77) { // M - this.state++; - } else { // restart and try again - this.state = 0; - } - break; - case 2: // direction (should be >) - this.unsupported = 0; - if (data[i] == 62) { // > + case this.decoder_states.IDLE: // sync char 1 + if (data[i] === this.symbols.BEGIN) { + this.state = this.decoder_states.PROTO_IDENTIFIER; + } + break; + case this.decoder_states.PROTO_IDENTIFIER: // sync char 2 + switch (data[i]) { + case this.symbols.PROTO_V1: + this.state = this.decoder_states.DIRECTION_V1; + break; + case this.symbols.PROTO_V2: + this.state = this.decoder_states.DIRECTION_V2; + break; + default: + console.log(`Unknown protocol char ${String.fromCharCode(data[i])}`); + this.state = this.decoder_states.IDLE; + } + break; + case this.decoder_states.DIRECTION_V1: // direction (should be >) + case this.decoder_states.DIRECTION_V2: + this.unsupported = 0; + switch (data[i]) { + case this.symbols.FROM_MWC: this.message_direction = 1; - } else if (data[i] == 60) { // < + break; + case this.symbols.TO_MWC: this.message_direction = 0; - } else if (data[i] == 33) { // ! - // FC reports unsupported message error + break; + case this.symbols.UNSUPPORTED: this.unsupported = 1; - } + break; + } + this.state = this.state === this.decoder_states.DIRECTION_V1 ? + this.decoder_states.PAYLOAD_LENGTH_V1 : + this.decoder_states.FLAG_V2; + break; + case this.decoder_states.FLAG_V2: + // Ignored for now + this.state = this.decoder_states.CODE_V2_LOW; + break; + case this.decoder_states.PAYLOAD_LENGTH_V1: + this.message_length_expected = data[i]; - this.state++; - break; - case 3: - this.message_length_expected = data[i]; - if (this.message_length_expected === this.JUMBO_FRAME_SIZE_LIMIT) { - this.messageIsJumboFrame = true; - } + if (this.message_length_expected === this.constants.JUMBO_FRAME_MIN_SIZE) { + this.state = this.decoder_states.CODE_JUMBO_V1; + } else { + this._initialize_read_buffer(); + this.state = this.decoder_states.CODE_V1; + } - this.message_checksum = data[i]; - - this.state++; - break; - case 4: - this.code = data[i]; - this.message_checksum ^= data[i]; - - if (this.message_length_expected > 0) { - // process payload - if (this.messageIsJumboFrame) { - this.state++; - } else { - this.state = this.state + 3; - } + break; + case this.decoder_states.PAYLOAD_LENGTH_V2_LOW: + this.message_length_expected = data[i]; + this.state = this.decoder_states.PAYLOAD_LENGTH_V2_HIGH; + break; + case this.decoder_states.PAYLOAD_LENGTH_V2_HIGH: + this.message_length_expected |= data[i] << 8; + this._initialize_read_buffer(); + this.state = this.message_length_expected > 0 ? + this.decoder_states.PAYLOAD_V2 : + this.decoder_states.CHECKSUM_V2; + break; + case this.decoder_states.CODE_V1: + case this.decoder_states.CODE_JUMBO_V1: + this.code = data[i]; + if (this.message_length_expected > 0) { + // process payload + if (this.state === this.decoder_states.CODE_JUMBO_V1) { + this.state = this.decoder_states.PAYLOAD_LENGTH_JUMBO_LOW; } else { - // no payload - this.state += 5; + this.state = this.decoder_states.PAYLOAD_V1; } - break; - case 5: - this.message_length_expected = data[i]; + } else { + // no payload + this.state = this.decoder_states.CHECKSUM_V1; + } + break; + case this.decoder_states.CODE_V2_LOW: + this.code = data[i]; + this.state = this.decoder_states.CODE_V2_HIGH; + break; + case this.decoder_states.CODE_V2_HIGH: + this.code |= data[i] << 8; + this.state = this.decoder_states.PAYLOAD_LENGTH_V2_LOW; + break; + case this.decoder_states.PAYLOAD_LENGTH_JUMBO_LOW: + this.message_length_expected = data[i]; + this.state = this.decoder_states.PAYLOAD_LENGTH_JUMBO_HIGH; + break; + case this.decoder_states.PAYLOAD_LENGTH_JUMBO_HIGH: + this.message_length_expected |= data[i] << 8; + this._initialize_read_buffer(); + this.state = this.decoder_states.PAYLOAD_V1; + break; + case this.decoder_states.PAYLOAD_V1: + case this.decoder_states.PAYLOAD_V2: + this.message_buffer_uint8_view[this.message_length_received] = data[i]; + this.message_length_received++; - this.message_checksum ^= data[i]; - - this.state++; - - break; - case 6: - this.message_length_expected = this.message_length_expected + 256 * data[i]; - - this.message_checksum ^= data[i]; - - this.state++; - - break; - case 7: - // setup arraybuffer - this.message_buffer = new ArrayBuffer(this.message_length_expected); - this.message_buffer_uint8_view = new Uint8Array(this.message_buffer); - - this.state++; - case 8: // payload - this.message_buffer_uint8_view[this.message_length_received] = data[i]; - this.message_checksum ^= data[i]; - this.message_length_received++; - - if (this.message_length_received >= this.message_length_expected) { - this.state++; - } - break; - case 9: - if (this.message_checksum == data[i]) { - // message received, store dataview - this.dataView = new DataView(this.message_buffer, 0, this.message_length_expected); - } else { - console.log('code: ' + this.code + ' - crc failed'); - this.packet_error++; - this.crcError = true; - this.dataView = new DataView(new ArrayBuffer(0)); - } - // Reset variables - this.message_length_received = 0; - this.state = 0; - this.messageIsJumboFrame = false; - this.notify(); - this.crcError = false; - break; - - default: - console.log('Unknown state detected: ' + this.state); + if (this.message_length_received >= this.message_length_expected) { + this.state = this.state === this.decoder_states.PAYLOAD_V1 ? + this.decoder_states.CHECKSUM_V1 : + this.decoder_states.CHECKSUM_V2; + } + break; + case this.decoder_states.CHECKSUM_V1: + if (this.message_length_expected >= this.constants.JUMBO_FRAME_MIN_SIZE) { + this.message_checksum = this.constants.JUMBO_FRAME_MIN_SIZE; + } else { + this.message_checksum = this.message_length_expected; + } + this.message_checksum ^= this.code; + if (this.message_length_expected >= this.constants.JUMBO_FRAME_MIN_SIZE) { + this.message_checksum ^= this.message_length_expected & 0xFF; + this.message_checksum ^= (this.message_length_expected & 0xFF00) >> 8; + } + for (let ii = 0; ii < this.message_length_received; ii++) { + this.message_checksum ^= this.message_buffer_uint8_view[ii]; + } + this._dispatch_message(data[i]); + break; + case this.decoder_states.CHECKSUM_V2: + this.message_checksum = 0; + this.message_checksum = this.crc8_dvb_s2(this.message_checksum, 0); // flag + this.message_checksum = this.crc8_dvb_s2(this.message_checksum, this.code & 0xFF); + this.message_checksum = this.crc8_dvb_s2(this.message_checksum, (this.code & 0xFF00) >> 8); + this.message_checksum = this.crc8_dvb_s2(this.message_checksum, this.message_length_expected & 0xFF); + this.message_checksum = this.crc8_dvb_s2(this.message_checksum, (this.message_length_expected & 0xFF00) >> 8); + for (let ii = 0; ii < this.message_length_received; ii++) { + this.message_checksum = this.crc8_dvb_s2(this.message_checksum, this.message_buffer_uint8_view[ii]); + } + this._dispatch_message(data[i]); + break; + default: + console.log('Unknown state detected: ' + this.state); } } this.last_received_timestamp = Date.now(); }, + _initialize_read_buffer: function() { + this.message_buffer = new ArrayBuffer(this.message_length_expected); + this.message_buffer_uint8_view = new Uint8Array(this.message_buffer); + }, + _dispatch_message: function(expectedChecksum) { + if (this.message_checksum === expectedChecksum) { + // message received, store dataview + this.dataView = new DataView(this.message_buffer, 0, this.message_length_expected); + } else { + console.log('code: ' + this.code + ' - crc failed'); + this.packet_error++; + this.crcError = true; + this.dataView = new DataView(new ArrayBuffer(0)); + } + // Reset variables + this.message_length_received = 0; + this.state = 0; + this.messageIsJumboFrame = false; + this.notify(); + this.crcError = false; + }, notify: function() { var self = this; this.listeners.forEach(function(listener) { @@ -147,13 +234,26 @@ var MSP = { clearListeners: function() { this.listeners = []; }, - send_message: function (code, data, callback_sent, callback_msp, doCallbackOnError) { - if (code === undefined) { - return; + crc8_dvb_s2: function(crc, ch) { + crc ^= ch; + for (let ii = 0; ii < 8; ii++) { + if (crc & 0x80) { + crc = ((crc << 1) & 0xFF) ^ 0xD5; + } else { + crc = (crc << 1) & 0xFF; + } } - + return crc; + }, + crc8_dvb_s2_data: function(data, start, end) { + let crc = 0; + for (let ii = start; ii < end; ii++) { + crc = this.crc8_dvb_s2(crc, data[ii]); + } + return crc; + }, + encode_message_v1: function(code, data) { let bufferOut; - // always reserve 6 bytes for protocol overhead ! if (data) { var size = data.length + 6, @@ -188,6 +288,38 @@ var MSP = { bufView[4] = code; // code bufView[5] = bufView[3] ^ bufView[4]; // checksum } + return bufferOut; + }, + encode_message_v2: function (code, data) { + const dataLength = data ? data.length : 0; + // 9 bytes for protocol overhead + const bufferSize = dataLength + 9; + const bufferOut = new ArrayBuffer(bufferSize); + const bufView = new Uint8Array(bufferOut); + bufView[0] = 36; // $ + bufView[1] = 88; // X + bufView[2] = 60; // < + bufView[3] = 0; // flag + bufView[4] = code & 0xFF; + bufView[5] = (code >> 8) & 0xFF; + bufView[6] = dataLength & 0xFF; + bufView[7] = (dataLength >> 8) & 0xFF; + for (let ii = 0; ii < dataLength; ii++) { + bufView[8 + ii] = data[ii]; + } + bufView[bufferSize - 1] = this.crc8_dvb_s2_data(bufView, 3, bufferSize - 1); + return bufferOut; + }, + send_message: function (code, data, callback_sent, callback_msp, doCallbackOnError) { + if (code === undefined) { + return; + } + let bufferOut; + if (code <= 254) { + bufferOut = this.encode_message_v1(code, data); + } else { + bufferOut = this.encode_message_v2(code, data); + } var obj = {'code': code, 'requestBuffer': bufferOut, 'callback': callback_msp ? callback_msp : false, 'timer': false, 'callbackOnError': doCallbackOnError}; diff --git a/src/js/msp/MSPCodes.js b/src/js/msp/MSPCodes.js index 919ccd2f..e8c50e8e 100644 --- a/src/js/msp/MSPCodes.js +++ b/src/js/msp/MSPCodes.js @@ -171,5 +171,9 @@ var MSPCodes = { MSP_EEPROM_WRITE: 250, MSP_DEBUGMSG: 253, // Not used - MSP_DEBUG: 254 + MSP_DEBUG: 254, + + // MSPv2 + MSP2_COMMON_SERIAL_CONFIG: 0x1009, + MSP2_COMMON_SET_SERIAL_CONFIG: 0x100A, }; diff --git a/src/js/msp/MSPHelper.js b/src/js/msp/MSPHelper.js index 4b4b4719..8f33ea91 100644 --- a/src/js/msp/MSPHelper.js +++ b/src/js/msp/MSPHelper.js @@ -836,8 +836,8 @@ MspHelper.prototype.process_data = function(dataHandler) { } else { SERIAL_CONFIG.ports = []; var bytesPerPort = 1 + 2 + (1 * 4); - var serialPortCount = data.byteLength / bytesPerPort; + var serialPortCount = data.byteLength / bytesPerPort; for (var i = 0; i < serialPortCount; i++) { var serialPort = { identifier: data.readU8(), @@ -853,10 +853,34 @@ MspHelper.prototype.process_data = function(dataHandler) { } break; + case MSPCodes.MSP2_COMMON_SERIAL_CONFIG: + const count = data.readU8(); + const portConfigSize = data.remaining() / count; + for (let ii = 0; ii < count; ii++) { + const start = data.remaining(); + const serialPort = { + identifier: data.readU8(), + functions: self.serialPortFunctionMaskToFunctions(data.readU32()), + msp_baudrate: self.BAUD_RATES[data.readU8()], + gps_baudrate: self.BAUD_RATES[data.readU8()], + telemetry_baudrate: self.BAUD_RATES[data.readU8()], + blackbox_baudrate: self.BAUD_RATES[data.readU8()], + }; + SERIAL_CONFIG.ports.push(serialPort); + while(start - data.remaining() < portConfigSize && data.remaining() > 0) { + data.readU8(); + } + } + break; + case MSPCodes.MSP_SET_CF_SERIAL_CONFIG: console.log('Serial config saved'); break; + case MSPCodes.MSP2_COMMON_SET_SERIAL_CONFIG: + console.log('Serial config saved (MSPv2)'); + break; + case MSPCodes.MSP_MODE_RANGES: MODE_RANGES = []; // empty the array as new data is coming in @@ -1853,6 +1877,7 @@ MspHelper.prototype.crunch = function(code) { var functionMask = self.serialPortFunctionsToMask(serialPort.functions); buffer.push16(functionMask) + .push8(self.BAUD_RATES.indexOf(serialPort.msp_baudrate)) .push8(self.BAUD_RATES.indexOf(serialPort.msp_baudrate)) .push8(self.BAUD_RATES.indexOf(serialPort.gps_baudrate)) .push8(self.BAUD_RATES.indexOf(serialPort.telemetry_baudrate)) @@ -1861,6 +1886,24 @@ MspHelper.prototype.crunch = function(code) { } break; + case MSPCodes.MSP2_COMMON_SET_SERIAL_CONFIG: + buffer.push8(SERIAL_CONFIG.ports.length); + + for (let i = 0; i < SERIAL_CONFIG.ports.length; i++) { + const serialPort = SERIAL_CONFIG.ports[i]; + + buffer.push8(serialPort.identifier); + + const functionMask = self.serialPortFunctionsToMask(serialPort.functions); + buffer.push32(functionMask) + .push8(self.BAUD_RATES.indexOf(serialPort.msp_baudrate)) + .push8(self.BAUD_RATES.indexOf(serialPort.msp_baudrate)) + .push8(self.BAUD_RATES.indexOf(serialPort.gps_baudrate)) + .push8(self.BAUD_RATES.indexOf(serialPort.telemetry_baudrate)) + .push8(self.BAUD_RATES.indexOf(serialPort.blackbox_baudrate)); + } + break; + case MSPCodes.MSP_SET_MOTOR_3D_CONFIG: buffer.push16(MOTOR_3D_CONFIG.deadband3d_low) .push16(MOTOR_3D_CONFIG.deadband3d_high) @@ -2690,6 +2733,16 @@ MspHelper.prototype.setArmingEnabled = function(doEnable, disableRunawayTakeoffP } } +MspHelper.prototype.loadSerialConfig = function(callback) { + const mspCode = semver.gte(CONFIG.apiVersion, "1.43.0") ? MSPCodes.MSP2_COMMON_SERIAL_CONFIG : MSPCodes.MSP_CF_SERIAL_CONFIG; + MSP.send_message(mspCode, false, false, callback); +}; + +MspHelper.prototype.sendSerialConfig = function(callback) { + const mspCode = semver.gte(CONFIG.apiVersion, "1.43.0") ? MSPCodes.MSP2_COMMON_SET_SERIAL_CONFIG : MSPCodes.MSP_SET_CF_SERIAL_CONFIG; + MSP.send_message(mspCode, mspHelper.crunch(mspCode), false, callback); +}; + MSP.SDCARD_STATE_NOT_PRESENT = 0; //TODO, move these to better place MSP.SDCARD_STATE_FATAL = 1; MSP.SDCARD_STATE_CARD_INIT = 2; diff --git a/src/js/tabs/auxiliary.js b/src/js/tabs/auxiliary.js index 0b76bb36..bc9538a4 100644 --- a/src/js/tabs/auxiliary.js +++ b/src/js/tabs/auxiliary.js @@ -29,7 +29,7 @@ TABS.auxiliary.initialize = function (callback) { } function get_serial_config() { - MSP.send_message(MSPCodes.MSP_CF_SERIAL_CONFIG, false, false, load_html); + mspHelper.loadSerialConfig(load_html); } function load_html() { diff --git a/src/js/tabs/configuration.js b/src/js/tabs/configuration.js index ab658bd2..cbacd1ff 100644 --- a/src/js/tabs/configuration.js +++ b/src/js/tabs/configuration.js @@ -35,7 +35,7 @@ TABS.configuration.initialize = function (callback, scrollPosition) { } function load_serial_config() { - MSP.send_message(MSPCodes.MSP_CF_SERIAL_CONFIG, false, false, load_board_alignment_config); + mspHelper.loadSerialConfig(load_board_alignment_config); } function load_board_alignment_config() { @@ -1210,7 +1210,7 @@ TABS.configuration.initialize = function (callback, scrollPosition) { function save_serial_config() { var next_callback = save_feature_config; - MSP.send_message(MSPCodes.MSP_SET_CF_SERIAL_CONFIG, mspHelper.crunch(MSPCodes.MSP_SET_CF_SERIAL_CONFIG), false, next_callback); + mspHelper.sendSerialConfig(next_callback); } function save_feature_config() { diff --git a/src/js/tabs/failsafe.js b/src/js/tabs/failsafe.js index 94fca351..1ba11065 100644 --- a/src/js/tabs/failsafe.js +++ b/src/js/tabs/failsafe.js @@ -39,7 +39,7 @@ TABS.failsafe.initialize = function (callback, scrollPosition) { } function get_ports_config() { - MSP.send_message(MSPCodes.MSP_CF_SERIAL_CONFIG, false, false, get_rc_data); + mspHelper.loadSerialConfig(get_rc_data); } function get_rc_data() { diff --git a/src/js/tabs/ports.js b/src/js/tabs/ports.js index 763f8fb3..dbbdcd31 100644 --- a/src/js/tabs/ports.js +++ b/src/js/tabs/ports.js @@ -111,13 +111,15 @@ TABS.ports.initialize = function (callback, scrollPosition) { load_configuration_from_fc(); function load_configuration_from_fc() { + let promise; if(semver.gte(CONFIG.apiVersion, "1.42.0")) { - MSP.promise(MSPCodes.MSP_VTX_CONFIG).then(function() { - return MSP.send_message(MSPCodes.MSP_CF_SERIAL_CONFIG, false, false, on_configuration_loaded_handler); - }); + promise = MSP.promise(MSPCodes.MSP_VTX_CONFIG); } else { - MSP.send_message(MSPCodes.MSP_CF_SERIAL_CONFIG, false, false, on_configuration_loaded_handler); + promise = Promise.resolve(); } + promise.then(function() { + mspHelper.loadSerialConfig(on_configuration_loaded_handler); + }); function on_configuration_loaded_handler() { $('#content').load("./tabs/ports.html", on_tab_loaded_handler); @@ -392,7 +394,7 @@ TABS.ports.initialize = function (callback, scrollPosition) { SERIAL_CONFIG.ports.push(serialPort); }); - MSP.send_message(MSPCodes.MSP_SET_CF_SERIAL_CONFIG, mspHelper.crunch(MSPCodes.MSP_SET_CF_SERIAL_CONFIG), false, save_to_eeprom); + mspHelper.sendSerialConfig(save_to_eeprom); function save_to_eeprom() { MSP.send_message(MSPCodes.MSP_EEPROM_WRITE, false, false, on_saved_handler);