From b9611485ca40e9e1e05c2d1e62b70a7e4166b3cf Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 13 Jul 2022 20:03:06 +0200 Subject: [PATCH] Load RPM --- js/fc.js | 10 +- js/msp/MSPCodes.js | 4 +- js/msp/MSPHelper.js | 19 +++ js/msp_balanced_interval.js | 1 - tabs/outputs.js | 281 +++++++++++++++++------------------- tabs/programming.js | 2 +- 6 files changed, 160 insertions(+), 157 deletions(-) diff --git a/js/fc.js b/js/fc.js index 7500f541..fe0eb5a3 100644 --- a/js/fc.js +++ b/js/fc.js @@ -64,7 +64,8 @@ var CONFIG, SAFEHOMES, BOARD_ALIGNMENT, CURRENT_METER_CONFIG, - FEATURES; + FEATURES, + ESC_SENSOR_DATA; var FC = { restartRequired: false, @@ -532,7 +533,7 @@ var FC = { boostSpeedThreshold: null, boostDisengageSpeed: null, bankAngle: null - } + }; RXFAIL_CONFIG = []; @@ -541,6 +542,11 @@ var FC = { SETTINGS = {}; SAFEHOMES = new SafehomeCollection(); + + ESC_SENSOR_DATA = { + motors: 0, + rpm: [] + } }, getOutputUsages: function() { return { diff --git a/js/msp/MSPCodes.js b/js/msp/MSPCodes.js index 966a701d..c1db7118 100644 --- a/js/msp/MSPCodes.js +++ b/js/msp/MSPCodes.js @@ -231,5 +231,7 @@ var MSPCodes = { MSP2_INAV_SAFEHOME: 0x2038, MSP2_INAV_SET_SAFEHOME: 0x2039, - MSP2_INAV_LOGIC_CONDITIONS_SINGLE: 0x203B + MSP2_INAV_LOGIC_CONDITIONS_SINGLE: 0x203B, + + MSP2_INAV_ESC_RPM: 0x2040, }; diff --git a/js/msp/MSPHelper.js b/js/msp/MSPHelper.js index ef448dda..65e80f93 100644 --- a/js/msp/MSPHelper.js +++ b/js/msp/MSPHelper.js @@ -1465,6 +1465,14 @@ var mspHelper = (function (gui) { console.log('Safehome points saved'); break; + case MSPCodes.MSP2_INAV_ESC_RPM: + ESC_SENSOR_DATA.motors = dataHandler.message_length_expected / 4; + + for (i = 0; i < ESC_SENSOR_DATA.motors; i++) { + ESC_SENSOR_DATA.rpm[i] = data.getUint32((i * 4), true); + } + break; + default: console.log('Unknown code detected: ' + dataHandler.code); } else { @@ -3190,6 +3198,10 @@ var mspHelper = (function (gui) { MSP.send_message(MSPCodes.MSP_MOTOR, false, false, callback); }; + self.loadServos = function (callback) { + MSP.send_message(MSPCodes.MSP_SERVO, false, false, callback); + }; + self.getCraftName = function (callback) { MSP.send_message(MSPCodes.MSP_NAME, false, false, function (resp) { var name = resp.data.readString(); @@ -3263,6 +3275,13 @@ var mspHelper = (function (gui) { MSP.send_message(MSPCodes.MSP2_INAV_PROGRAMMING_PID_STATUS, false, false, callback); }; + self.loadEscRpm = function (callback) { + MSP.send_message(MSPCodes.MSP2_INAV_ESC_RPM, false, false, callback); + } + + self.loadImuRaw = function (callback) { + MSP.send_message(MSPCodes.MSP_RAW_IMU, false, false, callback); + } return self; })(GUI); diff --git a/js/msp_balanced_interval.js b/js/msp_balanced_interval.js index 4190c774..52fab101 100644 --- a/js/msp_balanced_interval.js +++ b/js/msp_balanced_interval.js @@ -55,7 +55,6 @@ helper.mspBalancedInterval = (function (mspQueue, intervalHandler) { ); } } - }; /** diff --git a/tabs/outputs.js b/tabs/outputs.js index aa9daf53..2542bdc6 100644 --- a/tabs/outputs.js +++ b/tabs/outputs.js @@ -20,7 +20,16 @@ TABS.outputs.initialize = function (callback) { googleAnalytics.sendAppView('Outputs'); } - var loadChainer = new MSPChainerClass(); + var loadChainer = new MSPChainerClass(), + statusChainer = new MSPChainerClass(); + + statusChainer.setChain([ + mspHelper.loadEscRpm, + mspHelper.loadImuRaw, + mspHelper.loadMotors, + mspHelper.loadServos + ]); + statusChainer.setExitPoint(onStatusPullDone); loadChainer.setChain([ mspHelper.loadMiscV2, @@ -68,14 +77,126 @@ TABS.outputs.initialize = function (callback) { Settings.saveInputs().then(onComplete); } + let samples_accel_i = 0, + accel_data = initDataArray(3), + accel_max_read = [0, 0, 0], + accel_offset = [0, 0, 0], + accel_offset_established = false, + full_block_scale = MISC.maxthrottle - MISC.mincommand; + + let motors_wrapper, + servos_wrapper, + $motorTitles, + $motorSliders, + $motorValues; + + function update_accel_graph() { + + if (!accel_offset_established) { + for (var i = 0; i < 3; i++) { + accel_offset[i] = SENSOR_DATA.accelerometer[i] * -1; + } + + accel_offset_established = true; + } + + var accel_with_offset = [ + accel_offset[0] + SENSOR_DATA.accelerometer[0], + accel_offset[1] + SENSOR_DATA.accelerometer[1], + accel_offset[2] + SENSOR_DATA.accelerometer[2] + ]; + + samples_accel_i = addSampleToData(accel_data, samples_accel_i, accel_with_offset); + + // Compute RMS of acceleration in displayed period of time + // This is particularly useful for motor balancing as it + // eliminates the need for external tools + var sum = 0.0; + for (var j = 0; j < accel_data.length; j++) + for (var k = 0; k < accel_data[j].length; k++) + sum += accel_data[j][k][1] * accel_data[j][k][1]; + + let rms = Math.sqrt(sum / (accel_data[0].length + accel_data[1].length + accel_data[2].length)); + $(".acc-rms").text(rms.toFixed(4)); + + for (var i = 0; i < 3; i++) { + if (Math.abs(accel_with_offset[i]) > Math.abs(accel_max_read[i])) accel_max_read[i] = accel_with_offset[i]; + } + } + + function updateMotorsAndServos() { + var previousArmState = self.armed, + block_height = $('div.m-block:first').height(), + data, + margin_top, + height, + color, + i; + + for (i = 0; i < MOTOR_DATA.length; i++) { + data = MOTOR_DATA[i] - MISC.mincommand; + margin_top = block_height - (data * (block_height / full_block_scale)).clamp(0, block_height); + height = (data * (block_height / full_block_scale)).clamp(0, block_height); + color = parseInt(data * 0.009); + + $('.motor-' + i + ' .label', motors_wrapper).text(getMotorOutputValue(MOTOR_DATA[i])); + $('.motor-' + i + ' .indicator', motors_wrapper).css({ 'margin-top': margin_top + 'px', 'height': height + 'px', 'background-color': '#37a8db' + color + ')' }); + } + + // servo indicators are still using old (not flexible block scale), it will be changed in the future accordingly + for (i = 0; i < SERVO_DATA.length; i++) { + data = SERVO_DATA[i] - 1000; + margin_top = block_height - (data * (block_height / 1000)).clamp(0, block_height); + height = (data * (block_height / 1000)).clamp(0, block_height); + color = parseInt(data * 0.009); + + $('.servo-' + i + ' .label', servos_wrapper).text(SERVO_DATA[i]); + $('.servo-' + i + ' .indicator', servos_wrapper).css({ 'margin-top': margin_top + 'px', 'height': height + 'px', 'background-color': '#37a8db' + color + ')' }); + } + //keep the following here so at least we get a visual cue of our motor setup + update_arm_status(); + if (!self.allowTestMode) return; + + if (self.armed) { + $motorsEnableTestMode.prop('disabled', true); + $motorsEnableTestMode.prop('checked', false); + } else { + if (self.allowTestMode) { + $motorsEnableTestMode.prop('disabled', false); + } + } + + if (previousArmState != self.armed) { + console.log('arm state change detected'); + $motorsEnableTestMode.change(); + } + } + + function onStatusPullDone() { + + update_accel_graph(); + + $(".current-current").html(ANALOG.amperage.toFixed(2)); + $(".current-voltage").html(ANALOG.voltage.toFixed(2)); + + updateMotorsAndServos(); + } + function onLoad() { self.feature3DEnabled = bit_check(FEATURES, 12); + // Always start with default/empty sensor data array, clean slate all + initSensorData(); + process_motors(); process_servos(); processConfiguration(); + helper.mspBalancedInterval.add('data_pull', 75, 4, function () { + statusChainer.execute(); + }); + finalize(); } @@ -166,8 +287,6 @@ TABS.outputs.initialize = function (callback) { function showHideReversibleMotorInfo() { const reversibleMotorEnabled = $reversibleMotorCheckbox.is(':checked'); - console.log(reversibleMotorEnabled); - if (reversibleMotorEnabled) { $reversibleMotorBox.show(); } else { @@ -397,6 +516,12 @@ TABS.outputs.initialize = function (callback) { function process_motors() { $motorsEnableTestMode = $('#motorsEnableTestMode'); + motors_wrapper = $('.motors .bar-wrapper'), + servos_wrapper = $('.servos .bar-wrapper'), + $motorTitles = $('.motor-titles'), + $motorSliders = $('.motor-sliders'), + $motorValues = $('.motor-values'); + if (self.feature3DEnabled && !self.feature3DSupported) { self.allowTestMode = false; } @@ -406,82 +531,6 @@ TABS.outputs.initialize = function (callback) { update_model(MIXER_CONFIG.appliedMixerPreset); - // Always start with default/empty sensor data array, clean slate all - initSensorData(); - - // Setup variables - var samples_accel_i = 0, - accel_data = initDataArray(3), - accel_max_read = [0, 0, 0], - accel_offset = [0, 0, 0], - accel_offset_established = false; - - let $rmsHelper = $(".acc-rms"), - $currentHelper = $(".current-current"), - $voltageHelper = $(".current-voltage"); - - // timer initialization - helper.interval.killAll(['motor_and_status_pull', 'global_data_refresh', 'msp-load-update']); - helper.mspBalancedInterval.flush(); - - helper.interval.add('IMU_pull', function () { - - /* - * Enable balancer - */ - if (helper.mspQueue.shouldDrop()) { - update_accel_graph(); - return; - } - - MSP.send_message(MSPCodes.MSP_RAW_IMU, false, false, update_accel_graph); - }, 25, true); - - helper.interval.add('ANALOG_pull', function () { - $currentHelper.html(ANALOG.amperage.toFixed(2)); - $voltageHelper.html(ANALOG.voltage.toFixed(2)); - }, 100, true); - - function update_accel_graph() { - - if (!accel_offset_established) { - for (var i = 0; i < 3; i++) { - accel_offset[i] = SENSOR_DATA.accelerometer[i] * -1; - } - - accel_offset_established = true; - } - - var accel_with_offset = [ - accel_offset[0] + SENSOR_DATA.accelerometer[0], - accel_offset[1] + SENSOR_DATA.accelerometer[1], - accel_offset[2] + SENSOR_DATA.accelerometer[2] - ]; - - samples_accel_i = addSampleToData(accel_data, samples_accel_i, accel_with_offset); - - // Compute RMS of acceleration in displayed period of time - // This is particularly useful for motor balancing as it - // eliminates the need for external tools - var sum = 0.0; - for (var j = 0; j < accel_data.length; j++) - for (var k = 0; k < accel_data[j].length; k++) - sum += accel_data[j][k][1] * accel_data[j][k][1]; - - let rms = Math.sqrt(sum / (accel_data[0].length + accel_data[1].length + accel_data[2].length)); - $rmsHelper.text(rms.toFixed(4)); - - for (var i = 0; i < 3; i++) { - if (Math.abs(accel_with_offset[i]) > Math.abs(accel_max_read[i])) accel_max_read[i] = accel_with_offset[i]; - } - } - - let motors_wrapper = $('.motors .bar-wrapper'), - servos_wrapper = $('.servos .bar-wrapper'), - $motorTitles = $('.motor-titles'), - $motorSliders = $('.motor-sliders'), - $motorValues = $('.motor-values'); - for (let i = 0; i < MOTOR_RULES.getNumberOfConfiguredMotors(); i++) { const motorNumber = i + 1; motors_wrapper.append('\ @@ -651,79 +700,7 @@ TABS.outputs.initialize = function (callback) { } } - $motorsEnableTestMode.change(); - - function getPeriodicMotorOutput() { - - if (helper.mspQueue.shouldDrop()) { - getPeriodicServoOutput(); - return; - } - - MSP.send_message(MSPCodes.MSP_MOTOR, false, false, getPeriodicServoOutput); - } - - function getPeriodicServoOutput() { - if (helper.mspQueue.shouldDrop()) { - update_ui(); - return; - } - - MSP.send_message(MSPCodes.MSP_SERVO, false, false, update_ui); - } - - var full_block_scale = MISC.maxthrottle - MISC.mincommand; - - function update_ui() { - var previousArmState = self.armed, - block_height = $('div.m-block:first').height(), - data, - margin_top, - height, - color, - i; - - for (i = 0; i < MOTOR_DATA.length; i++) { - data = MOTOR_DATA[i] - MISC.mincommand; - margin_top = block_height - (data * (block_height / full_block_scale)).clamp(0, block_height); - height = (data * (block_height / full_block_scale)).clamp(0, block_height); - color = parseInt(data * 0.009); - - $('.motor-' + i + ' .label', motors_wrapper).text(getMotorOutputValue(MOTOR_DATA[i])); - $('.motor-' + i + ' .indicator', motors_wrapper).css({ 'margin-top': margin_top + 'px', 'height': height + 'px', 'background-color': '#37a8db' + color + ')' }); - } - - // servo indicators are still using old (not flexible block scale), it will be changed in the future accordingly - for (i = 0; i < SERVO_DATA.length; i++) { - data = SERVO_DATA[i] - 1000; - margin_top = block_height - (data * (block_height / 1000)).clamp(0, block_height); - height = (data * (block_height / 1000)).clamp(0, block_height); - color = parseInt(data * 0.009); - - $('.servo-' + i + ' .label', servos_wrapper).text(SERVO_DATA[i]); - $('.servo-' + i + ' .indicator', servos_wrapper).css({ 'margin-top': margin_top + 'px', 'height': height + 'px', 'background-color': '#37a8db' + color + ')' }); - } - //keep the following here so at least we get a visual cue of our motor setup - update_arm_status(); - if (!self.allowTestMode) return; - - if (self.armed) { - $motorsEnableTestMode.prop('disabled', true); - $motorsEnableTestMode.prop('checked', false); - } else { - if (self.allowTestMode) { - $motorsEnableTestMode.prop('disabled', false); - } - } - - if (previousArmState != self.armed) { - console.log('arm state change detected'); - $motorsEnableTestMode.change(); - } - } - - // enable Status and Motor data pulling - helper.interval.add('motor_and_status_pull', getPeriodicMotorOutput, 100, true); + $motorsEnableTestMode.change(); } function finalize() { diff --git a/tabs/programming.js b/tabs/programming.js index 6c22c85b..669a3d6a 100644 --- a/tabs/programming.js +++ b/tabs/programming.js @@ -58,7 +58,7 @@ TABS.programming.initialize = function (callback, scrollPosition) { GUI.log(chrome.i18n.getMessage('programmingEepromSaved')); }); - helper.mspBalancedInterval.add('logic_conditions_pull', 100, 1, function () { + helper.mspBalancedInterval.add('logic_conditions_pull', 100, 3, function () { statusChainer.execute(); });