diff --git a/src/js/msp/MSPHelper.js b/src/js/msp/MSPHelper.js index 38a01ae63d..587c53e1e2 100644 --- a/src/js/msp/MSPHelper.js +++ b/src/js/msp/MSPHelper.js @@ -604,6 +604,19 @@ MspHelper.prototype.process_data = function (dataHandler) { } break; case MSPCodes.MSP_SERVO_MIX_RULES: + FC.SERVO_RULES = []; // empty the array as new data is coming in + for (let i = 0; i < 16; i++) { + const array = { + targetChannel: data.readU8(), + inputSource: data.readU8(), + rate: data.readU8(), + speed: data.readU8(), + min: data.readU8(), + max: data.readU8(), + box: data.readU8(), + }; + FC.SERVO_RULES.push(array); + } break; case MSPCodes.MSP_SERVO_CONFIGURATIONS: @@ -2521,6 +2534,7 @@ MspHelper.prototype.sendServoConfigurations = function (onCompleteCallback) { if (out == undefined) { out = 255; // Cleanflight defines "CHANNEL_FORWARDING_DISABLED" as "(uint8_t)0xFF" } + buffer.push8(out).push32(servoConfiguration.reversedInputSources); // prepare for next iteration diff --git a/src/js/tabs/servos.js b/src/js/tabs/servos.js index c9ebbcbc66..581d534e9f 100644 --- a/src/js/tabs/servos.js +++ b/src/js/tabs/servos.js @@ -8,6 +8,7 @@ import { gui_log } from "../gui_log"; import $ from "jquery"; const servos = {}; + servos.initialize = function (callback) { if (GUI.active_tab !== "servos") { GUI.active_tab = "servos"; @@ -42,19 +43,22 @@ servos.initialize = function (callback) { $(".tab-servos").addClass("supported"); - let servoCheckbox = ""; + // setup header let servoHeader = ""; for (let i = 0; i < FC.RC.active_channels - 4; i++) { servoHeader += `A${i + 1}`; } + servoHeader += ''; + $("div.tab-servos table.fields tr.main").append(servoHeader); + + // setup checkboxes + let servoCheckbox = ""; for (let i = 0; i < FC.RC.active_channels; i++) { - servoCheckbox += ``; + servoCheckbox += ``; } - $("div.tab-servos table.fields tr.main").append(servoHeader); - /* * function: void process_servos(string, object) */ @@ -93,6 +97,19 @@ servos.initialize = function (callback) { $("div.tab-servos table.fields tr:last").data("info", { obj: obj }); + // check if input sources are reversed + for (const rule of FC.SERVO_RULES) { + const inputSource = rule.inputSource; + const reversed = FC.SERVO_CONFIG[obj].reversedInputSources & (1 << inputSource); + if (reversed) { + FC.SERVO_CONFIG[obj].reversedInputSources |= 1 << inputSource; + const inputSourceElement = $(`#channel${inputSource}`).find("input"); + if (inputSourceElement) { + inputSourceElement.prop("checked", true).parent().css("background-color", "red"); + } + } + } + // UI hooks // only one checkbox for indicating a channel to forward can be selected at a time, perhaps a radio group would be best here. @@ -123,8 +140,7 @@ servos.initialize = function (callback) { FC.SERVO_CONFIG[info.obj].min = parseInt($(".min input", this).val()); FC.SERVO_CONFIG[info.obj].max = parseInt($(".max input", this).val()); - const val = parseInt($(".direction select", this).val()); - FC.SERVO_CONFIG[info.obj].rate = val; + FC.SERVO_CONFIG[info.obj].rate = parseInt($(".direction select", this).val()); }); //