From 3f95ce9acfeb794e29a03ebbb9c87110fa8839e0 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 28 Apr 2020 11:10:51 +0200 Subject: Cleanup for server --- js/msp/MSPHelper.js | 310 ++++++++++++++++------------------------------------ 1 file changed, 92 insertions(+), 218 deletions(-) (limited to 'js') diff --git a/js/msp/MSPHelper.js b/js/msp/MSPHelper.js index 37c95592..8a75b6e4 100644 --- a/js/msp/MSPHelper.js +++ b/js/msp/MSPHelper.js @@ -20,17 +20,6 @@ var mspHelper = (function (gui) { '921600' ]; - self.BAUD_RATES_pre1_6_3 = [ - 'AUTO', - '9600', - '19200', - '38400', - '57600', - '115200', - '230400', - '250000' - ]; - self.SERIAL_PORT_FUNCTIONS = { 'MSP': 0, 'GPS': 1, @@ -415,10 +404,6 @@ var mspHelper = (function (gui) { REVERSIBLE_MOTORS.deadband_high = data.getUint16(offset, true); offset += 2; REVERSIBLE_MOTORS.neutral = data.getUint16(offset, true); - if (semver.lt(CONFIG.apiVersion, "1.17.0")) { - offset += 2; - REVERSIBLE_MOTORS.deadband_throttle = data.getUint16(offset, true); - } break; case MSPCodes.MSP_MOTOR_PINS: console.log(data); @@ -634,9 +619,7 @@ var mspHelper = (function (gui) { RC_deadband.deadband = data.getUint8(offset++); RC_deadband.yaw_deadband = data.getUint8(offset++); RC_deadband.alt_hold_deadband = data.getUint8(offset++); - if (semver.gte(CONFIG.apiVersion, "1.24.0")) { - REVERSIBLE_MOTORS.deadband_throttle = data.getUint16(offset, true); - } + REVERSIBLE_MOTORS.deadband_throttle = data.getUint16(offset, true); break; case MSPCodes.MSP_SENSOR_ALIGNMENT: SENSOR_ALIGNMENT.align_gyro = data.getUint8(offset++); @@ -814,7 +797,7 @@ var mspHelper = (function (gui) { var serialPortCount = data.byteLength / bytesPerPort; for (i = 0; i < serialPortCount; i++) { - var BAUD_RATES = (semver.gte(CONFIG.flightControllerVersion, "1.6.3")) ? mspHelper.BAUD_RATES_post1_6_3 : mspHelper.BAUD_RATES_pre1_6_3; + var BAUD_RATES = mspHelper.BAUD_RATES_post1_6_3; var serialPort = { identifier: data.getUint8(offset), @@ -836,7 +819,7 @@ var mspHelper = (function (gui) { var serialPortCount = data.byteLength / bytesPerPort; for (i = 0; i < serialPortCount; i++) { - var BAUD_RATES = (semver.gte(CONFIG.flightControllerVersion, "1.6.3")) ? mspHelper.BAUD_RATES_post1_6_3 : mspHelper.BAUD_RATES_pre1_6_3; + var BAUD_RATES = mspHelper.BAUD_RATES_post1_6_3; var serialPort = { identifier: data.getUint8(offset), @@ -923,15 +906,13 @@ var mspHelper = (function (gui) { offset += 2; RX_CONFIG.rx_max_usec = data.getUint16(offset, true); offset += 2; - if (semver.gte(CONFIG.apiVersion, "1.21.0")) { - offset += 4; // 4 null bytes for betaflight compatibility - RX_CONFIG.spirx_protocol = data.getUint8(offset); - offset++; - RX_CONFIG.spirx_id = data.getUint32(offset, true); - offset += 4; - RX_CONFIG.spirx_channel_count = data.getUint8(offset); - offset += 1; - } + offset += 4; // 4 null bytes for betaflight compatibility + RX_CONFIG.spirx_protocol = data.getUint8(offset); + offset++; + RX_CONFIG.spirx_id = data.getUint32(offset, true); + offset += 4; + RX_CONFIG.spirx_channel_count = data.getUint8(offset); + offset += 1; // unused byte for fpvCamAngleDegrees, for compatiblity with betaflight offset += 1; RX_CONFIG.receiver_type = data.getUint8(offset); @@ -987,11 +968,7 @@ var mspHelper = (function (gui) { //noinspection JSUndeclaredVariable LED_STRIP = []; - var ledCount = data.byteLength / 7; // v1.4.0 and below incorrectly reported 4 bytes per led. - - if (semver.gte(CONFIG.apiVersion, "1.20.0")) - ledCount = data.byteLength / 4; - + var ledCount = data.byteLength / 4; var directionMask, directions, directionLetterIndex, @@ -1103,26 +1080,23 @@ var mspHelper = (function (gui) { console.log('Led strip colors saved'); break; case MSPCodes.MSP_LED_STRIP_MODECOLOR: - if (semver.gte(CONFIG.apiVersion, "1.19.0")) { - - //noinspection JSUndeclaredVariable - LED_MODE_COLORS = []; + //noinspection JSUndeclaredVariable + LED_MODE_COLORS = []; - colorCount = data.byteLength / 3; + colorCount = data.byteLength / 3; - for (i = 0; offset < data.byteLength && i < colorCount; i++) { + for (i = 0; offset < data.byteLength && i < colorCount; i++) { - var mode = data.getUint8(offset++); - var direction = data.getUint8(offset++); + var mode = data.getUint8(offset++); + var direction = data.getUint8(offset++); - color = data.getUint8(offset++); + color = data.getUint8(offset++); - LED_MODE_COLORS.push({ - mode: mode, - direction: direction, - color: color - }); - } + LED_MODE_COLORS.push({ + mode: mode, + direction: direction, + color: color + }); } break; case MSPCodes.MSP_SET_LED_STRIP_MODECOLOR: @@ -1315,10 +1289,7 @@ var mspHelper = (function (gui) { CALIBRATION_DATA.magZero.X = data.getInt16(13, true); CALIBRATION_DATA.magZero.Y = data.getInt16(15, true); CALIBRATION_DATA.magZero.Z = data.getInt16(17, true); - - if (semver.gte(CONFIG.flightControllerVersion, "2.2.0")) { - CALIBRATION_DATA.opflow.Scale = (data.getInt16(19, true) / 256.0); - } + CALIBRATION_DATA.opflow.Scale = (data.getInt16(19, true) / 256.0); break; @@ -1757,16 +1728,14 @@ var mspHelper = (function (gui) { buffer.push(highByte(RX_CONFIG.rx_min_usec)); buffer.push(lowByte(RX_CONFIG.rx_max_usec)); buffer.push(highByte(RX_CONFIG.rx_max_usec)); - if (semver.gte(CONFIG.apiVersion, "1.21.0")) { - buffer.push(0); // 4 null bytes for betaflight compatibility - buffer.push(0); - buffer.push(0); - buffer.push(0); - buffer.push(RX_CONFIG.spirx_protocol); - // spirx_id - 4 bytes - buffer.push32(RX_CONFIG.spirx_id); - buffer.push(RX_CONFIG.spirx_channel_count); - } + buffer.push(0); // 4 null bytes for betaflight compatibility + buffer.push(0); + buffer.push(0); + buffer.push(0); + buffer.push(RX_CONFIG.spirx_protocol); + // spirx_id - 4 bytes + buffer.push32(RX_CONFIG.spirx_id); + buffer.push(RX_CONFIG.spirx_channel_count); // unused byte for fpvCamAngleDegrees, for compatiblity with betaflight buffer.push(0); // receiver type in RX_CONFIG rather than in BF_CONFIG.features @@ -1822,7 +1791,7 @@ var mspHelper = (function (gui) { buffer.push(specificByte(functionMask, 0)); buffer.push(specificByte(functionMask, 1)); - var BAUD_RATES = (semver.gte(CONFIG.flightControllerVersion, "1.6.3")) ? mspHelper.BAUD_RATES_post1_6_3 : mspHelper.BAUD_RATES_pre1_6_3; + var BAUD_RATES = mspHelper.BAUD_RATES_post1_6_3; buffer.push(BAUD_RATES.indexOf(serialPort.msp_baudrate)); buffer.push(BAUD_RATES.indexOf(serialPort.sensors_baudrate)); buffer.push(BAUD_RATES.indexOf(serialPort.telemetry_baudrate)); @@ -1842,7 +1811,7 @@ var mspHelper = (function (gui) { buffer.push(specificByte(functionMask, 2)); buffer.push(specificByte(functionMask, 3)); - var BAUD_RATES = (semver.gte(CONFIG.flightControllerVersion, "1.6.3")) ? mspHelper.BAUD_RATES_post1_6_3 : mspHelper.BAUD_RATES_pre1_6_3; + var BAUD_RATES = mspHelper.BAUD_RATES_post1_6_3; buffer.push(BAUD_RATES.indexOf(serialPort.msp_baudrate)); buffer.push(BAUD_RATES.indexOf(serialPort.sensors_baudrate)); buffer.push(BAUD_RATES.indexOf(serialPort.telemetry_baudrate)); @@ -1857,20 +1826,14 @@ var mspHelper = (function (gui) { buffer.push(highByte(REVERSIBLE_MOTORS.deadband_high)); buffer.push(lowByte(REVERSIBLE_MOTORS.neutral)); buffer.push(highByte(REVERSIBLE_MOTORS.neutral)); - if (semver.lt(CONFIG.apiVersion, "1.17.0")) { - buffer.push(lowByte(REVERSIBLE_MOTORS.deadband_throttle)); - buffer.push(highByte(REVERSIBLE_MOTORS.deadband_throttle)); - } break; case MSPCodes.MSP_SET_RC_DEADBAND: buffer.push(RC_deadband.deadband); buffer.push(RC_deadband.yaw_deadband); buffer.push(RC_deadband.alt_hold_deadband); - if (semver.gte(CONFIG.apiVersion, "1.24.0")) { - buffer.push(lowByte(REVERSIBLE_MOTORS.deadband_throttle)); - buffer.push(highByte(REVERSIBLE_MOTORS.deadband_throttle)); - } + buffer.push(lowByte(REVERSIBLE_MOTORS.deadband_throttle)); + buffer.push(highByte(REVERSIBLE_MOTORS.deadband_throttle)); break; case MSPCodes.MSP_SET_SENSOR_ALIGNMENT: @@ -1970,10 +1933,8 @@ var mspHelper = (function (gui) { buffer.push(lowByte(CALIBRATION_DATA.magZero.Z)); buffer.push(highByte(CALIBRATION_DATA.magZero.Z)); - if (semver.gte(CONFIG.flightControllerVersion, "2.2.0")) { buffer.push(lowByte(Math.round(CALIBRATION_DATA.opflow.Scale * 256))); buffer.push(highByte(Math.round(CALIBRATION_DATA.opflow.Scale * 256))); - } break; case MSPCodes.MSP_SET_POSITION_ESTIMATION_CONFIG: @@ -2216,16 +2177,11 @@ var mspHelper = (function (gui) { var buffer = []; var messageId = MSPCodes.MSP_SET_BLACKBOX_CONFIG; buffer.push(BLACKBOX.blackboxDevice & 0xFF); - if (semver.gte(CONFIG.apiVersion, "2.3.0")) { messageId = MSPCodes.MSP2_SET_BLACKBOX_CONFIG; buffer.push(lowByte(BLACKBOX.blackboxRateNum)); buffer.push(highByte(BLACKBOX.blackboxRateNum)); buffer.push(lowByte(BLACKBOX.blackboxRateDenom)); buffer.push(highByte(BLACKBOX.blackboxRateDenom)); - } else { - buffer.push(BLACKBOX.blackboxRateNum & 0xFF); - buffer.push(BLACKBOX.blackboxRateDenom & 0xFF); - } //noinspection JSUnusedLocalSymbols MSP.send_message(messageId, buffer, false, function (response) { onDataCallback(); @@ -2306,40 +2262,21 @@ var mspHelper = (function (gui) { var servoRule = SERVO_RULES.get()[servoIndex]; - if (semver.lt(CONFIG.flightControllerVersion, "2.2.0")) { - buffer.push(servoIndex); - buffer.push(servoRule.getTarget()); - buffer.push(servoRule.getInput()); - buffer.push(lowByte(servoRule.getRate())); - buffer.push(highByte(servoRule.getRate())); - buffer.push(servoRule.getSpeed()); - buffer.push(0); - buffer.push(0); - buffer.push(0); + //INAV 2.2 uses different MSP frame + buffer.push(servoIndex); + buffer.push(servoRule.getTarget()); + buffer.push(servoRule.getInput()); + buffer.push(lowByte(servoRule.getRate())); + buffer.push(highByte(servoRule.getRate())); + buffer.push(servoRule.getSpeed()); + buffer.push(servoRule.getConditionId()); - // prepare for next iteration - servoIndex++; - if (servoIndex == SERVO_RULES.getServoRulesCount()) { //This is the last rule. Not pretty, but we have to send all rules - nextFunction = onCompleteCallback; - } - MSP.send_message(MSPCodes.MSP_SET_SERVO_MIX_RULE, buffer, false, nextFunction); - } else { - //INAV 2.2 uses different MSP frame - buffer.push(servoIndex); - buffer.push(servoRule.getTarget()); - buffer.push(servoRule.getInput()); - buffer.push(lowByte(servoRule.getRate())); - buffer.push(highByte(servoRule.getRate())); - buffer.push(servoRule.getSpeed()); - buffer.push(servoRule.getConditionId()); - - // prepare for next iteration - servoIndex++; - if (servoIndex == SERVO_RULES.getServoRulesCount()) { //This is the last rule. Not pretty, but we have to send all rules - nextFunction = onCompleteCallback; - } - MSP.send_message(MSPCodes.MSP2_INAV_SET_SERVO_MIXER, buffer, false, nextFunction); + // prepare for next iteration + servoIndex++; + if (servoIndex == SERVO_RULES.getServoRulesCount()) { //This is the last rule. Not pretty, but we have to send all rules + nextFunction = onCompleteCallback; } + MSP.send_message(MSPCodes.MSP2_INAV_SET_SERVO_MIXER, buffer, false, nextFunction); } }; @@ -2391,18 +2328,14 @@ var mspHelper = (function (gui) { }; self.loadLogicConditions = function (callback) { - if (semver.gte(CONFIG.flightControllerVersion, "2.2.0")) { - MSP.send_message(MSPCodes.MSP2_INAV_LOGIC_CONDITIONS, false, false, callback); - } else { - callback(); - } + MSP.send_message(MSPCodes.MSP2_INAV_LOGIC_CONDITIONS, false, false, callback); } self.sendLogicConditions = function (onCompleteCallback) { let nextFunction = sendCondition, conditionIndex = 0; - if (LOGIC_CONDITIONS.getCount() == 0 || semver.lt(CONFIG.flightControllerVersion, "2.2.0")) { + if (LOGIC_CONDITIONS.getCount() == 0) { onCompleteCallback(); } else { nextFunction(); @@ -2444,18 +2377,14 @@ var mspHelper = (function (gui) { }; self.loadGlobalFunctions = function (callback) { - if (semver.gte(CONFIG.flightControllerVersion, "2.4.0")) { - MSP.send_message(MSPCodes.MSP2_INAV_GLOBAL_FUNCTIONS, false, false, callback); - } else { - callback(); - } + MSP.send_message(MSPCodes.MSP2_INAV_GLOBAL_FUNCTIONS, false, false, callback); } self.sendGlobalFunctions = function (onCompleteCallback) { let nextFunction = sendGlobalFunction, functionIndex = 0; - if (GLOBAL_FUNCTIONS.getCount() == 0 || semver.lt(CONFIG.flightControllerVersion, "2.4.0")) { + if (GLOBAL_FUNCTIONS.getCount() == 0) { onCompleteCallback(); } else { nextFunction(); @@ -2703,83 +2632,52 @@ var mspHelper = (function (gui) { buffer.push(ledIndex); - if (semver.lt(CONFIG.apiVersion, "1.20.0")) { - var directionMask = 0; - for (directionLetterIndex = 0; directionLetterIndex < led.directions.length; directionLetterIndex++) { - - bitIndex = MSP.ledDirectionLetters.indexOf(led.directions[directionLetterIndex]); - if (bitIndex >= 0) { - directionMask = bit_set(directionMask, bitIndex); - } - + var mask = 0; + /* + ledDirectionLetters: ['n', 'e', 's', 'w', 'u', 'd'], // in LSB bit order + ledFunctionLetters: ['i', 'w', 'f', 'a', 't', 'r', 'c', 'g', 's', 'b', 'l'], // in LSB bit order + ledBaseFunctionLetters: ['c', 'f', 'a', 'l', 's', 'g', 'r'], // in LSB bit + ledOverlayLetters: ['t', 'o', 'b', 'n', 'i', 'w'], // in LSB bit + + */ + mask |= (led.y << 0); + mask |= (led.x << 4); + + for (functionLetterIndex = 0; functionLetterIndex < led.functions.length; functionLetterIndex++) { + var fnIndex = MSP.ledBaseFunctionLetters.indexOf(led.functions[functionLetterIndex]); + if (fnIndex >= 0) { + mask |= (fnIndex << 8); + break; } - buffer.push(specificByte(directionMask, 0)); - buffer.push(specificByte(directionMask, 1)); - - var functionMask = 0; - for (functionLetterIndex = 0; functionLetterIndex < led.functions.length; functionLetterIndex++) { + } - bitIndex = MSP.ledFunctionLetters.indexOf(led.functions[functionLetterIndex]); - if (bitIndex >= 0) { - functionMask = bit_set(functionMask, bitIndex); - } + for (var overlayLetterIndex = 0; overlayLetterIndex < led.functions.length; overlayLetterIndex++) { + bitIndex = MSP.ledOverlayLetters.indexOf(led.functions[overlayLetterIndex]); + if (bitIndex >= 0) { + mask |= bit_set(mask, bitIndex + 12); } - buffer.push(specificByte(functionMask, 0)); - buffer.push(specificByte(functionMask, 1)); - - buffer.push(led.x); - buffer.push(led.y); - buffer.push(led.color); - } else { - var mask = 0; - /* - ledDirectionLetters: ['n', 'e', 's', 'w', 'u', 'd'], // in LSB bit order - ledFunctionLetters: ['i', 'w', 'f', 'a', 't', 'r', 'c', 'g', 's', 'b', 'l'], // in LSB bit order - ledBaseFunctionLetters: ['c', 'f', 'a', 'l', 's', 'g', 'r'], // in LSB bit - ledOverlayLetters: ['t', 'o', 'b', 'n', 'i', 'w'], // in LSB bit - - */ - mask |= (led.y << 0); - mask |= (led.x << 4); - - for (functionLetterIndex = 0; functionLetterIndex < led.functions.length; functionLetterIndex++) { - var fnIndex = MSP.ledBaseFunctionLetters.indexOf(led.functions[functionLetterIndex]); - if (fnIndex >= 0) { - mask |= (fnIndex << 8); - break; - } - } + } - for (var overlayLetterIndex = 0; overlayLetterIndex < led.functions.length; overlayLetterIndex++) { + mask |= (led.color << 18); - bitIndex = MSP.ledOverlayLetters.indexOf(led.functions[overlayLetterIndex]); - if (bitIndex >= 0) { - mask |= bit_set(mask, bitIndex + 12); - } + for (directionLetterIndex = 0; directionLetterIndex < led.directions.length; directionLetterIndex++) { + bitIndex = MSP.ledDirectionLetters.indexOf(led.directions[directionLetterIndex]); + if (bitIndex >= 0) { + mask |= bit_set(mask, bitIndex + 22); } - mask |= (led.color << 18); - - for (directionLetterIndex = 0; directionLetterIndex < led.directions.length; directionLetterIndex++) { - - bitIndex = MSP.ledDirectionLetters.indexOf(led.directions[directionLetterIndex]); - if (bitIndex >= 0) { - mask |= bit_set(mask, bitIndex + 22); - } - - } + } - mask |= (0 << 28); // parameters + mask |= (0 << 28); // parameters - buffer.push(specificByte(mask, 0)); - buffer.push(specificByte(mask, 1)); - buffer.push(specificByte(mask, 2)); - buffer.push(specificByte(mask, 3)); - } + buffer.push(specificByte(mask, 0)); + buffer.push(specificByte(mask, 1)); + buffer.push(specificByte(mask, 2)); + buffer.push(specificByte(mask, 3)); // prepare for next iteration ledIndex++; @@ -2862,11 +2760,7 @@ var mspHelper = (function (gui) { }; self.loadPidData = function (callback) { - if (semver.gte(CONFIG.flightControllerVersion, '2.2.0')) { - MSP.send_message(MSPCodes.MSP2_PID, false, false, callback); - } else { - MSP.send_message(MSPCodes.MSP_PID, false, false, callback); - } + MSP.send_message(MSPCodes.MSP2_PID, false, false, callback); }; self.loadPidNames = function (callback) { @@ -2906,11 +2800,7 @@ var mspHelper = (function (gui) { }; self.loadRxConfig = function (callback) { - if (semver.gte(CONFIG.apiVersion, "1.21.0")) { - MSP.send_message(MSPCodes.MSP_RX_CONFIG, false, false, callback); - } else { - callback(); - } + MSP.send_message(MSPCodes.MSP_RX_CONFIG, false, false, callback); }; self.load3dConfig = function (callback) { @@ -2970,11 +2860,7 @@ var mspHelper = (function (gui) { }; self.savePidData = function (callback) { - if (semver.gte(CONFIG.flightControllerVersion, '2.2.0')) { - MSP.send_message(MSPCodes.MSP2_SET_PID, mspHelper.crunch(MSPCodes.MSP2_SET_PID), false, callback); - } else { - MSP.send_message(MSPCodes.MSP_SET_PID, mspHelper.crunch(MSPCodes.MSP_SET_PID), false, callback); - } + MSP.send_message(MSPCodes.MSP2_SET_PID, mspHelper.crunch(MSPCodes.MSP2_SET_PID), false, callback); }; self.saveRcTuningData = function (callback) { @@ -3022,11 +2908,7 @@ var mspHelper = (function (gui) { }; self.saveRxConfig = function (callback) { - if (semver.gte(CONFIG.apiVersion, "1.21.0")) { - MSP.send_message(MSPCodes.MSP_SET_RX_CONFIG, mspHelper.crunch(MSPCodes.MSP_SET_RX_CONFIG), false, callback); - } else { - callback(); - } + MSP.send_message(MSPCodes.MSP_SET_RX_CONFIG, mspHelper.crunch(MSPCodes.MSP_SET_RX_CONFIG), false, callback); }; self.saveSensorConfig = function (callback) { @@ -3260,11 +3142,7 @@ var mspHelper = (function (gui) { }; self.loadServoMixRules = function (callback) { - if (semver.gte(CONFIG.flightControllerVersion, "2.2.0")) { - MSP.send_message(MSPCodes.MSP2_INAV_SERVO_MIXER, false, false, callback); - } else { - MSP.send_message(MSPCodes.MSP_SERVO_MIX_RULES, false, false, callback); - } + MSP.send_message(MSPCodes.MSP2_INAV_SERVO_MIXER, false, false, callback); }; self.loadMotorMixRules = function (callback) { @@ -3337,11 +3215,7 @@ var mspHelper = (function (gui) { } self.loadLogicConditionsStatus = function (callback) { - if (semver.gte(CONFIG.flightControllerVersion, "2.3.0")) { - MSP.send_message(MSPCodes.MSP2_INAV_LOGIC_CONDITIONS_STATUS, false, false, callback); - } else { - callback(); - } + MSP.send_message(MSPCodes.MSP2_INAV_LOGIC_CONDITIONS_STATUS, false, false, callback); }; self.loadGlobalVariablesStatus = function (callback) { -- cgit v1.2.3