Added configuration of KISS ESC Telemetry

10.3.x-maintenance
Bas Delfos 2016-11-18 00:34:31 +01:00
parent 0123388a7f
commit 8aa40f1a2c
4 changed files with 144 additions and 135 deletions

View File

@ -527,6 +527,9 @@
"featureBLACKBOXTip": { "featureBLACKBOXTip": {
"message": "Configure via the BlackBox tab after enabling." "message": "Configure via the BlackBox tab after enabling."
}, },
"featureESC_TELEMETRY": {
"message": "KISS ESC 24A telemetry support"
},
"featureCHANNEL_FORWARDING": { "featureCHANNEL_FORWARDING": {
"message": "Forward aux channels to servo outputs" "message": "Forward aux channels to servo outputs"
}, },
@ -777,6 +780,9 @@
"portsFunction_TELEMETRY_SMARTPORT": { "portsFunction_TELEMETRY_SMARTPORT": {
"message": "SmartPort" "message": "SmartPort"
}, },
"portsFunction_TELEMETRY_ESC": {
"message": "ESC telemetry"
},
"portsFunction_RX_SERIAL": { "portsFunction_RX_SERIAL": {
"message": "Serial RX" "message": "Serial RX"
}, },
@ -1114,7 +1120,7 @@
"adjustmentsFunction21": { "adjustmentsFunction21": {
"message": "RC Rate Yaw" "message": "RC Rate Yaw"
}, },
"adjustmentsSave": { "adjustmentsSave": {
"message": "Save" "message": "Save"
}, },
@ -1857,25 +1863,25 @@
"message": "Blackbox logging device" "message": "Blackbox logging device"
}, },
"onboardLoggingRateOfLogging": { "onboardLoggingRateOfLogging": {
"message": "Blackbox logging rate" "message": "Blackbox logging rate"
}, },
"onboardLoggingSerialLogger": { "onboardLoggingSerialLogger": {
"message": "Outboard serial logging device" "message": "Outboard serial logging device"
}, },
"onboardLoggingFlashLogger": { "onboardLoggingFlashLogger": {
"message": "Onboard dataflash chip" "message": "Onboard dataflash chip"
}, },
"onboardLoggingEraseInProgress": { "onboardLoggingEraseInProgress": {
"message": "Erase in progress, please wait..." "message": "Erase in progress, please wait..."
}, },
"onboardLoggingOnboardSDCard": { "onboardLoggingOnboardSDCard": {
"message": "Onboard SD card" "message": "Onboard SD card"
} }

View File

@ -21,7 +21,8 @@ var Features = function (config) {
{bit: 15, group: 'rssi', name: 'RSSI_ADC'}, {bit: 15, group: 'rssi', name: 'RSSI_ADC'},
{bit: 16, group: 'other', name: 'LED_STRIP'}, {bit: 16, group: 'other', name: 'LED_STRIP'},
{bit: 17, group: 'other', name: 'DISPLAY'}, {bit: 17, group: 'other', name: 'DISPLAY'},
{bit: 19, group: 'other', name: 'BLACKBOX', haveTip: true} {bit: 19, group: 'other', name: 'BLACKBOX', haveTip: true},
{bit: 27, group: 'other', name: 'ESC_TELEMETRY'}
]; ];
if (semver.gte(config.apiVersion, "1.12.0")) { if (semver.gte(config.apiVersion, "1.12.0")) {

View File

@ -20,13 +20,14 @@ function MspHelper () {
'RX_SERIAL': 6, 'RX_SERIAL': 6,
'BLACKBOX': 7, 'BLACKBOX': 7,
'TELEMETRY_MAVLINK': 8, 'TELEMETRY_MAVLINK': 8,
'TELEMETRY_ESC': 10
}; };
} }
MspHelper.prototype.process_data = function(dataHandler) { MspHelper.prototype.process_data = function(dataHandler) {
var self = this; var self = this;
var data = dataHandler.dataView; // DataView (allowing us to view arrayBuffer as struct/union) var data = dataHandler.dataView; // DataView (allowing us to view arrayBuffer as struct/union)
var code = dataHandler.code; var code = dataHandler.code;
if (!dataHandler.unsupported) switch (code) { if (!dataHandler.unsupported) switch (code) {
case MSPCodes.MSP_STATUS: case MSPCodes.MSP_STATUS:
CONFIG.cycleTime = data.readU16(); CONFIG.cycleTime = data.readU16();
@ -60,7 +61,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
$('span.cycle-time').text(CONFIG.cycleTime); $('span.cycle-time').text(CONFIG.cycleTime);
$('span.cpu-load').text(chrome.i18n.getMessage('statusbar_cpu_load', [CONFIG.cpuload])); $('span.cpu-load').text(chrome.i18n.getMessage('statusbar_cpu_load', [CONFIG.cpuload]));
break; break;
case MSPCodes.MSP_RAW_IMU: case MSPCodes.MSP_RAW_IMU:
// 512 for mpu6050, 256 for mma // 512 for mpu6050, 256 for mma
// currently we are unable to differentiate between the sensor types, so we are goign with 512 // currently we are unable to differentiate between the sensor types, so we are goign with 512
@ -192,11 +193,11 @@ MspHelper.prototype.process_data = function(dataHandler) {
MISC.gps_ubx_sbas = data.readU8(); MISC.gps_ubx_sbas = data.readU8();
MISC.multiwiicurrentoutput = data.readU8(); MISC.multiwiicurrentoutput = data.readU8();
MISC.rssi_channel = data.readU8(); MISC.rssi_channel = data.readU8();
MISC.placeholder2 = data.readU8(); MISC.placeholder2 = data.readU8();
if (semver.lt(CONFIG.apiVersion, "1.18.0")) if (semver.lt(CONFIG.apiVersion, "1.18.0"))
MISC.mag_declination = data.read16() / 10; // -1800-1800 MISC.mag_declination = data.read16() / 10; // -1800-1800
else else
MISC.mag_declination = data.read16() / 100; // -18000-18000 MISC.mag_declination = data.read16() / 100; // -18000-18000
MISC.vbatscale = data.readU8(); // 10-200 MISC.vbatscale = data.readU8(); // 10-200
MISC.vbatmincellvoltage = data.readU8() / 10; // 10-50 MISC.vbatmincellvoltage = data.readU8() / 10; // 10-50
MISC.vbatmaxcellvoltage = data.readU8() / 10; // 10-50 MISC.vbatmaxcellvoltage = data.readU8() / 10; // 10-50
@ -289,12 +290,12 @@ MspHelper.prototype.process_data = function(dataHandler) {
SERVO_CONFIG.push(arr); SERVO_CONFIG.push(arr);
} }
} }
if (semver.eq(CONFIG.apiVersion, '1.10.0')) { if (semver.eq(CONFIG.apiVersion, '1.10.0')) {
// drop two unused servo configurations due to MSP rx buffer to small) // drop two unused servo configurations due to MSP rx buffer to small)
while (SERVO_CONFIG.length > 8) { while (SERVO_CONFIG.length > 8) {
SERVO_CONFIG.pop(); SERVO_CONFIG.pop();
} }
} }
} }
break; break;
@ -302,7 +303,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
RC_deadband.deadband = data.readU8(); RC_deadband.deadband = data.readU8();
RC_deadband.yaw_deadband = data.readU8(); RC_deadband.yaw_deadband = data.readU8();
RC_deadband.alt_hold_deadband = data.readU8(); RC_deadband.alt_hold_deadband = data.readU8();
if (semver.gte(CONFIG.apiVersion, "1.17.0")) { if (semver.gte(CONFIG.apiVersion, "1.17.0")) {
_3D.deadband3d_throttle = data.readU16(); _3D.deadband3d_throttle = data.readU16();
} }
@ -403,7 +404,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
break; break;
case MSPCodes.MSP_API_VERSION: case MSPCodes.MSP_API_VERSION:
CONFIG.mspProtocolVersion = data.readU8(); CONFIG.mspProtocolVersion = data.readU8();
CONFIG.apiVersion = data.readU8() + '.' + data.readU8() + '.0'; CONFIG.apiVersion = data.readU8() + '.' + data.readU8() + '.0';
break; break;
@ -426,7 +427,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
buff.push(data.readU8()); buff.push(data.readU8());
} }
buff.push(32); // ascii space buff.push(32); // ascii space
var timeLength = 8; var timeLength = 8;
for (var i = 0; i < timeLength; i++) { for (var i = 0; i < timeLength; i++) {
buff.push(data.readU8()); buff.push(data.readU8());
@ -464,7 +465,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
identifier: data.readU8(), identifier: data.readU8(),
scenario: data.readU8() scenario: data.readU8()
} }
SERIAL_CONFIG.ports.push(serialPort); SERIAL_CONFIG.ports.push(serialPort);
} }
SERIAL_CONFIG.mspBaudRate = data.readU32(); SERIAL_CONFIG.mspBaudRate = data.readU32();
SERIAL_CONFIG.cliBaudRate = data.readU32(); SERIAL_CONFIG.cliBaudRate = data.readU32();
@ -474,7 +475,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
SERIAL_CONFIG.ports = []; SERIAL_CONFIG.ports = [];
var bytesPerPort = 1 + 2 + (1 * 4); var bytesPerPort = 1 + 2 + (1 * 4);
var serialPortCount = data.byteLength / bytesPerPort; var serialPortCount = data.byteLength / bytesPerPort;
for (var i = 0; i < serialPortCount; i++) { for (var i = 0; i < serialPortCount; i++) {
var serialPort = { var serialPort = {
identifier: data.readU8(), identifier: data.readU8(),
@ -484,7 +485,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
telemetry_baudrate: self.BAUD_RATES[data.readU8()], telemetry_baudrate: self.BAUD_RATES[data.readU8()],
blackbox_baudrate: self.BAUD_RATES[data.readU8()] blackbox_baudrate: self.BAUD_RATES[data.readU8()]
} }
SERIAL_CONFIG.ports.push(serialPort); SERIAL_CONFIG.ports.push(serialPort);
} }
} }
@ -498,7 +499,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
MODE_RANGES = []; // empty the array as new data is coming in MODE_RANGES = []; // empty the array as new data is coming in
var modeRangeCount = data.byteLength / 4; // 4 bytes per item. var modeRangeCount = data.byteLength / 4; // 4 bytes per item.
for (var i = 0; i < modeRangeCount; i++) { for (var i = 0; i < modeRangeCount; i++) {
var modeRange = { var modeRange = {
id: data.readU8(), id: data.readU8(),
@ -516,7 +517,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
ADJUSTMENT_RANGES = []; // empty the array as new data is coming in ADJUSTMENT_RANGES = []; // empty the array as new data is coming in
var adjustmentRangeCount = data.byteLength / 6; // 6 bytes per item. var adjustmentRangeCount = data.byteLength / 6; // 6 bytes per item.
for (var i = 0; i < adjustmentRangeCount; i++) { for (var i = 0; i < adjustmentRangeCount; i++) {
var adjustmentRange = { var adjustmentRange = {
slotIndex: data.readU8(), slotIndex: data.readU8(),
@ -644,25 +645,25 @@ MspHelper.prototype.process_data = function(dataHandler) {
SENSOR_CONFIG.baro_hardware = data.readU8(); SENSOR_CONFIG.baro_hardware = data.readU8();
SENSOR_CONFIG.mag_hardware = data.readU8(); SENSOR_CONFIG.mag_hardware = data.readU8();
break; break;
case MSPCodes.MSP_LED_STRIP_CONFIG: case MSPCodes.MSP_LED_STRIP_CONFIG:
LED_STRIP = []; LED_STRIP = [];
var ledDirectionLetters = ['n', 'e', 's', 'w', 'u', 'd']; // in LSB bit order var ledDirectionLetters = ['n', 'e', 's', 'w', 'u', 'd']; // in LSB bit order
var ledFunctionLetters = ['i', 'w', 'f', 'a', 't', 'r', 'c', 'g', 's', 'b', 'l']; // in LSB bit order var ledFunctionLetters = ['i', 'w', 'f', 'a', 't', 'r', 'c', 'g', 's', 'b', 'l']; // in LSB bit order
var ledBaseFunctionLetters = ['c', 'f', 'a', 'l', 's', 'g', 'r']; // in LSB bit var ledBaseFunctionLetters = ['c', 'f', 'a', 'l', 's', 'g', 'r']; // in LSB bit
var ledOverlayLetters = ['t', 'o', 'b', 'n', 'i', 'w']; // in LSB bit var ledOverlayLetters = ['t', 'o', 'b', 'n', 'i', 'w']; // in LSB bit
var ledCount = data.byteLength / 7; // v1.4.0 and below incorrectly reported 4 bytes per led. var ledCount = data.byteLength / 7; // v1.4.0 and below incorrectly reported 4 bytes per led.
if (semver.gte(CONFIG.apiVersion, "1.20.0")) if (semver.gte(CONFIG.apiVersion, "1.20.0"))
ledCount = data.byteLength / 4; ledCount = data.byteLength / 4;
for (var i = 0; i < ledCount; i++) { for (var i = 0; i < ledCount; i++) {
if (semver.lt(CONFIG.apiVersion, "1.20.0")) { if (semver.lt(CONFIG.apiVersion, "1.20.0")) {
var directionMask = data.readU16(); var directionMask = data.readU16();
var directions = []; var directions = [];
for (var directionLetterIndex = 0; directionLetterIndex < ledDirectionLetters.length; directionLetterIndex++) { for (var directionLetterIndex = 0; directionLetterIndex < ledDirectionLetters.length; directionLetterIndex++) {
if (bit_check(directionMask, directionLetterIndex)) { if (bit_check(directionMask, directionLetterIndex)) {
@ -671,14 +672,14 @@ MspHelper.prototype.process_data = function(dataHandler) {
} }
var functionMask = data.readU16(); var functionMask = data.readU16();
var functions = []; var functions = [];
for (var functionLetterIndex = 0; functionLetterIndex < ledFunctionLetters.length; functionLetterIndex++) { for (var functionLetterIndex = 0; functionLetterIndex < ledFunctionLetters.length; functionLetterIndex++) {
if (bit_check(functionMask, functionLetterIndex)) { if (bit_check(functionMask, functionLetterIndex)) {
functions.push(ledFunctionLetters[functionLetterIndex]); functions.push(ledFunctionLetters[functionLetterIndex]);
} }
} }
var led = { var led = {
directions: directions, directions: directions,
functions: functions, functions: functions,
@ -686,11 +687,11 @@ MspHelper.prototype.process_data = function(dataHandler) {
y: data.readU8(), y: data.readU8(),
color: data.readU8() color: data.readU8()
}; };
LED_STRIP.push(led); LED_STRIP.push(led);
} else { } else {
var mask = data.readU32(); var mask = data.readU32();
var functionId = (mask >> 8) & 0xF; var functionId = (mask >> 8) & 0xF;
var functions = []; var functions = [];
for (var baseFunctionLetterIndex = 0; baseFunctionLetterIndex < ledBaseFunctionLetters.length; baseFunctionLetterIndex++) { for (var baseFunctionLetterIndex = 0; baseFunctionLetterIndex < ledBaseFunctionLetters.length; baseFunctionLetterIndex++) {
@ -699,14 +700,14 @@ MspHelper.prototype.process_data = function(dataHandler) {
break; break;
} }
} }
var overlayMask = (mask >> 12) & 0x3F; var overlayMask = (mask >> 12) & 0x3F;
for (var overlayLetterIndex = 0; overlayLetterIndex < ledOverlayLetters.length; overlayLetterIndex++) { for (var overlayLetterIndex = 0; overlayLetterIndex < ledOverlayLetters.length; overlayLetterIndex++) {
if (bit_check(overlayMask, overlayLetterIndex)) { if (bit_check(overlayMask, overlayLetterIndex)) {
functions.push(ledOverlayLetters[overlayLetterIndex]); functions.push(ledOverlayLetters[overlayLetterIndex]);
} }
} }
var directionMask = (mask >> 22) & 0x3F; var directionMask = (mask >> 22) & 0x3F;
var directions = []; var directions = [];
for (var directionLetterIndex = 0; directionLetterIndex < ledDirectionLetters.length; directionLetterIndex++) { for (var directionLetterIndex = 0; directionLetterIndex < ledDirectionLetters.length; directionLetterIndex++) {
@ -722,7 +723,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
directions: directions, directions: directions,
parameters: (mask >> 28) & 0xF parameters: (mask >> 28) & 0xF
}; };
LED_STRIP.push(led); LED_STRIP.push(led);
} }
} }
@ -731,11 +732,11 @@ MspHelper.prototype.process_data = function(dataHandler) {
console.log('Led strip config saved'); console.log('Led strip config saved');
break; break;
case MSPCodes.MSP_LED_COLORS: case MSPCodes.MSP_LED_COLORS:
LED_COLORS = []; LED_COLORS = [];
var colorCount = data.byteLength / 4; var colorCount = data.byteLength / 4;
for (var i = 0; i < colorCount; i++) { for (var i = 0; i < colorCount; i++) {
var color = { var color = {
@ -745,7 +746,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
}; };
LED_COLORS.push(color); LED_COLORS.push(color);
} }
break; break;
case MSPCodes.MSP_SET_LED_COLORS: case MSPCodes.MSP_SET_LED_COLORS:
console.log('Led strip colors saved'); console.log('Led strip colors saved');
@ -754,9 +755,9 @@ MspHelper.prototype.process_data = function(dataHandler) {
if (semver.gte(CONFIG.apiVersion, "1.19.0")) { if (semver.gte(CONFIG.apiVersion, "1.19.0")) {
LED_MODE_COLORS = []; LED_MODE_COLORS = [];
var colorCount = data.byteLength / 3; var colorCount = data.byteLength / 3;
for (var i = 0; i < colorCount; i++) { for (var i = 0; i < colorCount; i++) {
var mode_color = { var mode_color = {
@ -797,8 +798,8 @@ MspHelper.prototype.process_data = function(dataHandler) {
console.log("Data flash erase begun..."); console.log("Data flash erase begun...");
break; break;
case MSPCodes.MSP_SDCARD_SUMMARY: case MSPCodes.MSP_SDCARD_SUMMARY:
var flags = data.readU8(); var flags = data.readU8();
SDCARD.supported = (flags & 0x01) != 0; SDCARD.supported = (flags & 0x01) != 0;
SDCARD.state = data.readU8(); SDCARD.state = data.readU8();
SDCARD.filesystemLastError = data.readU8(); SDCARD.filesystemLastError = data.readU8();
@ -817,7 +818,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
case MSPCodes.MSP_TRANSPONDER_CONFIG: case MSPCodes.MSP_TRANSPONDER_CONFIG:
TRANSPONDER.supported = (data.readU8() & 1) != 0; TRANSPONDER.supported = (data.readU8() & 1) != 0;
TRANSPONDER.data = []; TRANSPONDER.data = [];
var bytesRemaining = data.byteLength - 1; var bytesRemaining = data.byteLength - 1;
for (var i = 0; i < bytesRemaining; i++) { for (var i = 0; i < bytesRemaining; i++) {
TRANSPONDER.data.push(data.readU8()); TRANSPONDER.data.push(data.readU8());
} }
@ -831,7 +832,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
case MSPCodes.MSP_SET_ADJUSTMENT_RANGE: case MSPCodes.MSP_SET_ADJUSTMENT_RANGE:
console.log('Adjustment range saved'); console.log('Adjustment range saved');
break; break;
case MSPCodes.MSP_PID_CONTROLLER: case MSPCodes.MSP_PID_CONTROLLER:
PID.controller = data.readU8(); PID.controller = data.readU8();
break; break;
@ -855,7 +856,7 @@ MspHelper.prototype.process_data = function(dataHandler) {
break; break;
case MSPCodes.MSP_SET_SENSOR_ALIGNMENT: case MSPCodes.MSP_SET_SENSOR_ALIGNMENT:
console.log('Sensor alignment saved'); console.log('Sensor alignment saved');
break; break;
case MSPCodes.MSP_SET_RX_CONFIG: case MSPCodes.MSP_SET_RX_CONFIG:
console.log('Rx config saved'); console.log('Rx config saved');
break; break;
@ -997,7 +998,7 @@ MspHelper.prototype.crunch = function(code) {
.push8(MISC.placeholder2); .push8(MISC.placeholder2);
if (semver.lt(CONFIG.apiVersion, "1.18.0")) { if (semver.lt(CONFIG.apiVersion, "1.18.0")) {
buffer.push16(Math.round(MISC.mag_declination * 10)); buffer.push16(Math.round(MISC.mag_declination * 10));
} else { } else {
buffer.push16(Math.round(MISC.mag_declination * 100)); buffer.push16(Math.round(MISC.mag_declination * 100));
} }
buffer.push8(MISC.vbatscale) buffer.push8(MISC.vbatscale)
@ -1061,9 +1062,9 @@ MspHelper.prototype.crunch = function(code) {
} else { } else {
for (var i = 0; i < SERIAL_CONFIG.ports.length; i++) { for (var i = 0; i < SERIAL_CONFIG.ports.length; i++) {
var serialPort = SERIAL_CONFIG.ports[i]; var serialPort = SERIAL_CONFIG.ports[i];
buffer.push8(serialPort.identifier); buffer.push8(serialPort.identifier);
var functionMask = self.serialPortFunctionsToMask(serialPort.functions); var functionMask = self.serialPortFunctionsToMask(serialPort.functions);
buffer.push16(functionMask) buffer.push16(functionMask)
.push8(self.BAUD_RATES.indexOf(serialPort.msp_baudrate)) .push8(self.BAUD_RATES.indexOf(serialPort.msp_baudrate))
@ -1081,11 +1082,11 @@ MspHelper.prototype.crunch = function(code) {
if (semver.lt(CONFIG.apiVersion, "1.17.0")) { if (semver.lt(CONFIG.apiVersion, "1.17.0")) {
buffer.push16(_3D.deadband3d_throttle); buffer.push16(_3D.deadband3d_throttle);
} }
break; break;
case MSPCodes.MSP_SET_RC_DEADBAND: case MSPCodes.MSP_SET_RC_DEADBAND:
buffer.push8(RC_deadband.deadband) buffer.push8(RC_deadband.deadband)
.push8(RC_deadband.yaw_deadband) .push8(RC_deadband.yaw_deadband)
.push8(RC_deadband.alt_hold_deadband); .push8(RC_deadband.alt_hold_deadband);
if (semver.gte(CONFIG.apiVersion, "1.17.0")) { if (semver.gte(CONFIG.apiVersion, "1.17.0")) {
buffer.push16(_3D.deadband3d_throttle); buffer.push16(_3D.deadband3d_throttle);
@ -1158,20 +1159,20 @@ MspHelper.prototype.crunch = function(code) {
.push8(SENSOR_CONFIG.baro_hardware) .push8(SENSOR_CONFIG.baro_hardware)
.push8(SENSOR_CONFIG.mag_hardware); .push8(SENSOR_CONFIG.mag_hardware);
break; break;
case MSPCodes.MSP_SET_NAME: case MSPCodes.MSP_SET_NAME:
var MSP_BUFFER_SIZE = 64; var MSP_BUFFER_SIZE = 64;
for (var i = 0; i<CONFIG.name.length && i<MSP_BUFFER_SIZE; i++) { for (var i = 0; i<CONFIG.name.length && i<MSP_BUFFER_SIZE; i++) {
buffer.push8(CONFIG.name.charCodeAt(i)); buffer.push8(CONFIG.name.charCodeAt(i));
} }
break; break;
case MSPCodes.MSP_SET_BLACKBOX_CONFIG: case MSPCodes.MSP_SET_BLACKBOX_CONFIG:
buffer.push8(BLACKBOX.blackboxDevice) buffer.push8(BLACKBOX.blackboxDevice)
.push8(BLACKBOX.blackboxRateNum) .push8(BLACKBOX.blackboxRateNum)
.push8(BLACKBOX.blackboxRateDenom); .push8(BLACKBOX.blackboxRateDenom);
break; break;
default: default:
return false; return false;
} }
@ -1181,16 +1182,16 @@ MspHelper.prototype.crunch = function(code) {
/** /**
* Set raw Rx values over MSP protocol. * Set raw Rx values over MSP protocol.
* *
* Channels is an array of 16-bit unsigned integer channel values to be sent. 8 channels is probably the maximum. * Channels is an array of 16-bit unsigned integer channel values to be sent. 8 channels is probably the maximum.
*/ */
MspHelper.prototype.setRawRx = function(channels) { MspHelper.prototype.setRawRx = function(channels) {
var buffer = []; var buffer = [];
for (var i = 0; i < channels.length; i++) { for (var i = 0; i < channels.length; i++) {
buffer.push16(channels[i]); buffer.push16(channels[i]);
} }
MSP.send_message(MSPCodes.MSP_SET_RAW_RC, buffer, false); MSP.send_message(MSPCodes.MSP_SET_RAW_RC, buffer, false);
} }
@ -1231,8 +1232,8 @@ MspHelper.prototype.dataflashRead = function(address, blockSize, onDataCallback)
}; };
MspHelper.prototype.sendServoConfigurations = function(onCompleteCallback) { MspHelper.prototype.sendServoConfigurations = function(onCompleteCallback) {
var nextFunction = send_next_servo_configuration; var nextFunction = send_next_servo_configuration;
var servoIndex = 0; var servoIndex = 0;
if (SERVO_CONFIG.length == 0) { if (SERVO_CONFIG.length == 0) {
@ -1243,7 +1244,7 @@ MspHelper.prototype.sendServoConfigurations = function(onCompleteCallback) {
function send_next_servo_configuration() { function send_next_servo_configuration() {
var buffer = []; var buffer = [];
if (semver.lt(CONFIG.apiVersion, "1.12.0")) { if (semver.lt(CONFIG.apiVersion, "1.12.0")) {
@ -1258,9 +1259,9 @@ MspHelper.prototype.sendServoConfigurations = function(onCompleteCallback) {
nextFunction = send_channel_forwarding; nextFunction = send_channel_forwarding;
} else { } else {
// send one at a time, with index // send one at a time, with index
var servoConfiguration = SERVO_CONFIG[servoIndex]; var servoConfiguration = SERVO_CONFIG[servoIndex];
buffer.push8(servoIndex) buffer.push8(servoIndex)
.push16(servoConfiguration.min) .push16(servoConfiguration.min)
.push16(servoConfiguration.max) .push16(servoConfiguration.max)
@ -1275,7 +1276,7 @@ MspHelper.prototype.sendServoConfigurations = function(onCompleteCallback) {
} }
buffer.push8(out) buffer.push8(out)
.push32(servoConfiguration.reversedInputSources); .push32(servoConfiguration.reversedInputSources);
// prepare for next iteration // prepare for next iteration
servoIndex++; servoIndex++;
if (servoIndex == SERVO_CONFIG.length) { if (servoIndex == SERVO_CONFIG.length) {
@ -1284,7 +1285,7 @@ MspHelper.prototype.sendServoConfigurations = function(onCompleteCallback) {
} }
MSP.send_message(MSPCodes.MSP_SET_SERVO_CONFIGURATION, buffer, false, nextFunction); MSP.send_message(MSPCodes.MSP_SET_SERVO_CONFIGURATION, buffer, false, nextFunction);
} }
function send_channel_forwarding() { function send_channel_forwarding() {
var buffer = []; var buffer = [];
@ -1303,8 +1304,8 @@ MspHelper.prototype.sendServoConfigurations = function(onCompleteCallback) {
}; };
MspHelper.prototype.sendModeRanges = function(onCompleteCallback) { MspHelper.prototype.sendModeRanges = function(onCompleteCallback) {
var nextFunction = send_next_mode_range; var nextFunction = send_next_mode_range;
var modeRangeIndex = 0; var modeRangeIndex = 0;
if (MODE_RANGES.length == 0) { if (MODE_RANGES.length == 0) {
@ -1314,29 +1315,29 @@ MspHelper.prototype.sendModeRanges = function(onCompleteCallback) {
} }
function send_next_mode_range() { function send_next_mode_range() {
var modeRange = MODE_RANGES[modeRangeIndex]; var modeRange = MODE_RANGES[modeRangeIndex];
var buffer = []; var buffer = [];
buffer.push8(modeRangeIndex) buffer.push8(modeRangeIndex)
.push8(modeRange.id) .push8(modeRange.id)
.push8(modeRange.auxChannelIndex) .push8(modeRange.auxChannelIndex)
.push8((modeRange.range.start - 900) / 25) .push8((modeRange.range.start - 900) / 25)
.push8((modeRange.range.end - 900) / 25); .push8((modeRange.range.end - 900) / 25);
// prepare for next iteration // prepare for next iteration
modeRangeIndex++; modeRangeIndex++;
if (modeRangeIndex == MODE_RANGES.length) { if (modeRangeIndex == MODE_RANGES.length) {
nextFunction = onCompleteCallback; nextFunction = onCompleteCallback;
} }
MSP.send_message(MSPCodes.MSP_SET_MODE_RANGE, buffer, false, nextFunction); MSP.send_message(MSPCodes.MSP_SET_MODE_RANGE, buffer, false, nextFunction);
} }
}; };
MspHelper.prototype.sendAdjustmentRanges = function(onCompleteCallback) { MspHelper.prototype.sendAdjustmentRanges = function(onCompleteCallback) {
var nextFunction = send_next_adjustment_range; var nextFunction = send_next_adjustment_range;
var adjustmentRangeIndex = 0; var adjustmentRangeIndex = 0;
if (ADJUSTMENT_RANGES.length == 0) { if (ADJUSTMENT_RANGES.length == 0) {
@ -1347,9 +1348,9 @@ MspHelper.prototype.sendAdjustmentRanges = function(onCompleteCallback) {
function send_next_adjustment_range() { function send_next_adjustment_range() {
var adjustmentRange = ADJUSTMENT_RANGES[adjustmentRangeIndex]; var adjustmentRange = ADJUSTMENT_RANGES[adjustmentRangeIndex];
var buffer = []; var buffer = [];
buffer.push8(adjustmentRangeIndex) buffer.push8(adjustmentRangeIndex)
.push8(adjustmentRange.slotIndex) .push8(adjustmentRange.slotIndex)
@ -1358,21 +1359,21 @@ MspHelper.prototype.sendAdjustmentRanges = function(onCompleteCallback) {
.push8((adjustmentRange.range.end - 900) / 25) .push8((adjustmentRange.range.end - 900) / 25)
.push8(adjustmentRange.adjustmentFunction) .push8(adjustmentRange.adjustmentFunction)
.push8(adjustmentRange.auxSwitchChannelIndex); .push8(adjustmentRange.auxSwitchChannelIndex);
// prepare for next iteration // prepare for next iteration
adjustmentRangeIndex++; adjustmentRangeIndex++;
if (adjustmentRangeIndex == ADJUSTMENT_RANGES.length) { if (adjustmentRangeIndex == ADJUSTMENT_RANGES.length) {
nextFunction = onCompleteCallback; nextFunction = onCompleteCallback;
} }
MSP.send_message(MSPCodes.MSP_SET_ADJUSTMENT_RANGE, buffer, false, nextFunction); MSP.send_message(MSPCodes.MSP_SET_ADJUSTMENT_RANGE, buffer, false, nextFunction);
} }
}; };
MspHelper.prototype.sendLedStripConfig = function(onCompleteCallback) { MspHelper.prototype.sendLedStripConfig = function(onCompleteCallback) {
var nextFunction = send_next_led_strip_config; var nextFunction = send_next_led_strip_config;
var ledIndex = 0; var ledIndex = 0;
if (LED_STRIP.length == 0) { if (LED_STRIP.length == 0) {
@ -1382,15 +1383,15 @@ MspHelper.prototype.sendLedStripConfig = function(onCompleteCallback) {
} }
function send_next_led_strip_config() { function send_next_led_strip_config() {
var led = LED_STRIP[ledIndex]; var led = LED_STRIP[ledIndex];
var ledDirectionLetters = ['n', 'e', 's', 'w', 'u', 'd']; // in LSB bit order var ledDirectionLetters = ['n', 'e', 's', 'w', 'u', 'd']; // in LSB bit order
var ledFunctionLetters = ['i', 'w', 'f', 'a', 't', 'r', 'c', 'g', 's', 'b', 'l']; // in LSB bit order var ledFunctionLetters = ['i', 'w', 'f', 'a', 't', 'r', 'c', 'g', 's', 'b', 'l']; // in LSB bit order
var ledBaseFunctionLetters = ['c', 'f', 'a', 'l', 's', 'g', 'r']; // in LSB bit var ledBaseFunctionLetters = ['c', 'f', 'a', 'l', 's', 'g', 'r']; // in LSB bit
var ledOverlayLetters = ['t', 'o', 'b', 'n', 'i', 'w']; // in LSB bit var ledOverlayLetters = ['t', 'o', 'b', 'n', 'i', 'w']; // in LSB bit
var buffer = []; var buffer = [];
buffer.push(ledIndex); buffer.push(ledIndex);
if (semver.lt(CONFIG.apiVersion, "1.20.0")) { if (semver.lt(CONFIG.apiVersion, "1.20.0")) {
@ -1402,7 +1403,7 @@ MspHelper.prototype.sendLedStripConfig = function(onCompleteCallback) {
} }
} }
buffer.push16(directionMask); buffer.push16(directionMask);
var functionMask = 0; var functionMask = 0;
for (var functionLetterIndex = 0; functionLetterIndex < led.functions.length; functionLetterIndex++) { for (var functionLetterIndex = 0; functionLetterIndex < led.functions.length; functionLetterIndex++) {
var bitIndex = ledFunctionLetters.indexOf(led.functions[functionLetterIndex]); var bitIndex = ledFunctionLetters.indexOf(led.functions[functionLetterIndex]);
@ -1411,10 +1412,10 @@ MspHelper.prototype.sendLedStripConfig = function(onCompleteCallback) {
} }
} }
buffer.push16(functionMask) buffer.push16(functionMask)
.push8(led.x) .push8(led.x)
.push8(led.y) .push8(led.y)
.push8(led.color); .push8(led.color);
} else { } else {
var mask = 0; var mask = 0;
@ -1429,7 +1430,7 @@ MspHelper.prototype.sendLedStripConfig = function(onCompleteCallback) {
break; break;
} }
} }
for (var overlayLetterIndex = 0; overlayLetterIndex < led.functions.length; overlayLetterIndex++) { for (var overlayLetterIndex = 0; overlayLetterIndex < led.functions.length; overlayLetterIndex++) {
var bitIndex = ledOverlayLetters.indexOf(led.functions[overlayLetterIndex]); var bitIndex = ledOverlayLetters.indexOf(led.functions[overlayLetterIndex]);
if (bitIndex >= 0) { if (bitIndex >= 0) {
@ -1445,19 +1446,19 @@ MspHelper.prototype.sendLedStripConfig = function(onCompleteCallback) {
mask |= bit_set(mask, bitIndex + 22); mask |= bit_set(mask, bitIndex + 22);
} }
} }
mask |= (0 << 28); // parameters mask |= (0 << 28); // parameters
buffer.push32(mask); buffer.push32(mask);
} }
// prepare for next iteration // prepare for next iteration
ledIndex++; ledIndex++;
if (ledIndex == LED_STRIP.length) { if (ledIndex == LED_STRIP.length) {
nextFunction = onCompleteCallback; nextFunction = onCompleteCallback;
} }
MSP.send_message(MSPCodes.MSP_SET_LED_STRIP_CONFIG, buffer, false, nextFunction); MSP.send_message(MSPCodes.MSP_SET_LED_STRIP_CONFIG, buffer, false, nextFunction);
} }
} }
@ -1467,10 +1468,10 @@ MspHelper.prototype.sendLedStripColors = function(onCompleteCallback) {
onCompleteCallback(); onCompleteCallback();
} else { } else {
var buffer = []; var buffer = [];
for (var colorIndex = 0; colorIndex < LED_COLORS.length; colorIndex++) { for (var colorIndex = 0; colorIndex < LED_COLORS.length; colorIndex++) {
var color = LED_COLORS[colorIndex]; var color = LED_COLORS[colorIndex];
buffer.push16(color.h) buffer.push16(color.h)
.push8(color.s) .push8(color.s)
.push8(color.v); .push8(color.v);
@ -1480,21 +1481,21 @@ MspHelper.prototype.sendLedStripColors = function(onCompleteCallback) {
} }
MspHelper.prototype.sendLedStripModeColors = function(onCompleteCallback) { MspHelper.prototype.sendLedStripModeColors = function(onCompleteCallback) {
var nextFunction = send_next_led_strip_mode_color; var nextFunction = send_next_led_strip_mode_color;
var index = 0; var index = 0;
if (LED_MODE_COLORS.length == 0) { if (LED_MODE_COLORS.length == 0) {
onCompleteCallback(); onCompleteCallback();
} else { } else {
send_next_led_strip_mode_color(); send_next_led_strip_mode_color();
} }
function send_next_led_strip_mode_color() { function send_next_led_strip_mode_color() {
var buffer = []; var buffer = [];
var mode_color = LED_MODE_COLORS[index]; var mode_color = LED_MODE_COLORS[index];
buffer.push8(mode_color.mode) buffer.push8(mode_color.mode)
.push8(mode_color.direction) .push8(mode_color.direction)
.push8(mode_color.color); .push8(mode_color.color);
@ -1527,7 +1528,7 @@ MspHelper.prototype.serialPortFunctionMaskToFunctions = function(functionMask) {
MspHelper.prototype.serialPortFunctionsToMask = function(functions) { MspHelper.prototype.serialPortFunctionsToMask = function(functions) {
var self = this; var self = this;
var mask = 0; var mask = 0;
var keys = Object.keys(self.SERIAL_PORT_FUNCTIONS); var keys = Object.keys(self.SERIAL_PORT_FUNCTIONS);
for (var index = 0; index < functions.length; index++) { for (var index = 0; index < functions.length; index++) {
var key = functions[index]; var key = functions[index];
@ -1558,7 +1559,7 @@ MspHelper.prototype.sendRxFailConfig = function(onCompleteCallback) {
buffer.push8(rxFailIndex) buffer.push8(rxFailIndex)
.push8(rxFail.mode) .push8(rxFail.mode)
.push16(rxFail.value); .push16(rxFail.value);
// prepare for next iteration // prepare for next iteration
rxFailIndex++; rxFailIndex++;

View File

@ -13,6 +13,7 @@ TABS.ports.initialize = function (callback, scrollPosition) {
{name: 'TELEMETRY_FRSKY', groups: ['telemetry'], sharableWith: ['msp'], notSharableWith: ['blackbox'], maxPorts: 1}, {name: 'TELEMETRY_FRSKY', groups: ['telemetry'], sharableWith: ['msp'], notSharableWith: ['blackbox'], maxPorts: 1},
{name: 'TELEMETRY_HOTT', groups: ['telemetry'], sharableWith: ['msp'], notSharableWith: ['blackbox'], maxPorts: 1}, {name: 'TELEMETRY_HOTT', groups: ['telemetry'], sharableWith: ['msp'], notSharableWith: ['blackbox'], maxPorts: 1},
{name: 'TELEMETRY_SMARTPORT', groups: ['telemetry'], maxPorts: 1}, {name: 'TELEMETRY_SMARTPORT', groups: ['telemetry'], maxPorts: 1},
{name: 'TELEMETRY_ESC', groups: ['telemetry'], maxPorts: 1},
{name: 'RX_SERIAL', groups: ['rx'], maxPorts: 1}, {name: 'RX_SERIAL', groups: ['rx'], maxPorts: 1},
{name: 'BLACKBOX', groups: ['logging', 'blackbox'], sharableWith: ['msp'], notSharableWith: ['telemetry'], maxPorts: 1}, {name: 'BLACKBOX', groups: ['logging', 'blackbox'], sharableWith: ['msp'], notSharableWith: ['telemetry'], maxPorts: 1},
]; ];
@ -81,28 +82,28 @@ TABS.ports.initialize = function (callback, scrollPosition) {
} }
load_configuration_from_fc(); load_configuration_from_fc();
function load_configuration_from_fc() { function load_configuration_from_fc() {
MSP.send_message(MSPCodes.MSP_CF_SERIAL_CONFIG, false, false, on_configuration_loaded_handler); MSP.send_message(MSPCodes.MSP_CF_SERIAL_CONFIG, false, false, on_configuration_loaded_handler);
function on_configuration_loaded_handler() { function on_configuration_loaded_handler() {
$('#content').load("./tabs/ports.html", on_tab_loaded_handler); $('#content').load("./tabs/ports.html", on_tab_loaded_handler);
board_definition = BOARD.find_board_definition(CONFIG.boardIdentifier); board_definition = BOARD.find_board_definition(CONFIG.boardIdentifier);
console.log('Using board definition', board_definition); console.log('Using board definition', board_definition);
} }
} }
function update_ui() { function update_ui() {
if (semver.lt(CONFIG.apiVersion, "1.6.0")) { if (semver.lt(CONFIG.apiVersion, "1.6.0")) {
$(".tab-ports").removeClass("supported"); $(".tab-ports").removeClass("supported");
return; return;
} }
$(".tab-ports").addClass("supported"); $(".tab-ports").addClass("supported");
var portIdentifierToNameMapping = { var portIdentifierToNameMapping = {
0: 'UART1', 0: 'UART1',
1: 'UART2', 1: 'UART2',
@ -141,13 +142,13 @@ TABS.ports.initialize = function (callback, scrollPosition) {
var ports_e = $('.tab-ports .ports'); var ports_e = $('.tab-ports .ports');
var port_configuration_template_e = $('#tab-ports-templates .portConfiguration'); var port_configuration_template_e = $('#tab-ports-templates .portConfiguration');
for (var portIndex = 0; portIndex < SERIAL_CONFIG.ports.length; portIndex++) { for (var portIndex = 0; portIndex < SERIAL_CONFIG.ports.length; portIndex++) {
var port_configuration_e = port_configuration_template_e.clone(); var port_configuration_e = port_configuration_template_e.clone();
var serialPort = SERIAL_CONFIG.ports[portIndex]; var serialPort = SERIAL_CONFIG.ports[portIndex];
port_configuration_e.data('serialPort', serialPort); port_configuration_e.data('serialPort', serialPort);
var msp_baudrate_e = port_configuration_e.find('select.msp_baudrate'); var msp_baudrate_e = port_configuration_e.find('select.msp_baudrate');
msp_baudrate_e.val(serialPort.msp_baudrate); msp_baudrate_e.val(serialPort.msp_baudrate);
@ -161,24 +162,24 @@ TABS.ports.initialize = function (callback, scrollPosition) {
blackbox_baudrate_e.val(serialPort.blackbox_baudrate); blackbox_baudrate_e.val(serialPort.blackbox_baudrate);
port_configuration_e.find('.identifier').text(portIdentifierToNameMapping[serialPort.identifier]) port_configuration_e.find('.identifier').text(portIdentifierToNameMapping[serialPort.identifier])
port_configuration_e.data('index', portIndex); port_configuration_e.data('index', portIndex);
port_configuration_e.data('port', serialPort); port_configuration_e.data('port', serialPort);
for (var columnIndex = 0; columnIndex < columns.length; columnIndex++) { for (var columnIndex = 0; columnIndex < columns.length; columnIndex++) {
var column = columns[columnIndex]; var column = columns[columnIndex];
var functions_e = $(port_configuration_e).find('.functionsCell-' + column); var functions_e = $(port_configuration_e).find('.functionsCell-' + column);
for (var i = 0; i < functionRules.length; i++) { for (var i = 0; i < functionRules.length; i++) {
var functionRule = functionRules[i]; var functionRule = functionRules[i];
var functionName = functionRule.name; var functionName = functionRule.name;
if (functionRule.groups.indexOf(column) == -1) { if (functionRule.groups.indexOf(column) == -1) {
continue; continue;
} }
var select_e; var select_e;
if (column != 'telemetry') { if (column != 'telemetry') {
var checkboxId = 'functionCheckbox-' + portIndex + '-' + columnIndex + '-' + i; var checkboxId = 'functionCheckbox-' + portIndex + '-' + columnIndex + '-' + i;
@ -188,13 +189,13 @@ TABS.ports.initialize = function (callback, scrollPosition) {
var checkbox_e = functions_e.find('#' + checkboxId); var checkbox_e = functions_e.find('#' + checkboxId);
checkbox_e.prop("checked", true); checkbox_e.prop("checked", true);
} }
} else { } else {
var selectElementName = 'function-' + column; var selectElementName = 'function-' + column;
var selectElementSelector = 'select[name=' + selectElementName + ']'; var selectElementSelector = 'select[name=' + selectElementName + ']';
select_e = functions_e.find(selectElementSelector); select_e = functions_e.find(selectElementSelector);
if (select_e.size() == 0) { if (select_e.size() == 0) {
functions_e.prepend('<span class="function"><select name="' + selectElementName + '" /></span>'); functions_e.prepend('<span class="function"><select name="' + selectElementName + '" /></span>');
select_e = functions_e.find(selectElementSelector); select_e = functions_e.find(selectElementSelector);
@ -213,11 +214,11 @@ TABS.ports.initialize = function (callback, scrollPosition) {
ports_e.find('tbody').append(port_configuration_e); ports_e.find('tbody').append(port_configuration_e);
} }
} }
function on_tab_loaded_handler() { function on_tab_loaded_handler() {
localize(); localize();
update_ui(); update_ui();
$('a.save').click(on_save_handler); $('a.save').click(on_save_handler);
@ -231,25 +232,25 @@ TABS.ports.initialize = function (callback, scrollPosition) {
} }
function on_save_handler() { function on_save_handler() {
// update configuration based on current ui state // update configuration based on current ui state
SERIAL_CONFIG.ports = []; SERIAL_CONFIG.ports = [];
var ports_e = $('.tab-ports .portConfiguration').each(function (portConfiguration_e) { var ports_e = $('.tab-ports .portConfiguration').each(function (portConfiguration_e) {
var portConfiguration_e = this; var portConfiguration_e = this;
var oldSerialPort = $(this).data('serialPort'); var oldSerialPort = $(this).data('serialPort');
var functions = $(portConfiguration_e).find('input:checkbox:checked').map(function() { var functions = $(portConfiguration_e).find('input:checkbox:checked').map(function() {
return this.value; return this.value;
}).get(); }).get();
var telemetryFunction = $(portConfiguration_e).find('select[name=function-telemetry]').val(); var telemetryFunction = $(portConfiguration_e).find('select[name=function-telemetry]').val();
if (telemetryFunction) { if (telemetryFunction) {
functions.push(telemetryFunction); functions.push(telemetryFunction);
} }
var serialPort = { var serialPort = {
functions: functions, functions: functions,
msp_baudrate: $(portConfiguration_e).find('.msp_baudrate').val(), msp_baudrate: $(portConfiguration_e).find('.msp_baudrate').val(),
@ -260,7 +261,7 @@ TABS.ports.initialize = function (callback, scrollPosition) {
}; };
SERIAL_CONFIG.ports.push(serialPort); 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); MSP.send_message(MSPCodes.MSP_SET_CF_SERIAL_CONFIG, mspHelper.crunch(MSPCodes.MSP_SET_CF_SERIAL_CONFIG), false, save_to_eeprom);
function save_to_eeprom() { function save_to_eeprom() {