1
0
Fork 0
mirror of https://github.com/iNavFlight/inav-configurator.git synced 2025-07-15 04:15:28 +03:00
This commit is contained in:
skaman82 2015-10-23 15:13:33 +02:00
parent 3a9c7f3794
commit d61970e177
5 changed files with 251 additions and 221 deletions

299
js/msp.js
View file

@ -51,7 +51,7 @@ var MSP_codes = {
MSP_PIDNAMES: 117,
MSP_WP: 118,
MSP_BOXIDS: 119,
MSP_SERVO_CONF: 120,
MSP_SERVO_CONFIGURATIONS: 120,
MSP_SET_RAW_RC: 200,
MSP_SET_RAW_GPS: 201,
@ -65,10 +65,13 @@ var MSP_codes = {
MSP_SET_WP: 209,
MSP_SELECT_SETTING: 210,
MSP_SET_HEAD: 211,
MSP_SET_SERVO_CONF: 212,
MSP_SET_SERVO_CONFIGURATION: 212,
MSP_SET_MOTOR: 214,
// MSP_BIND: 240,
MSP_SERVO_MIX_RULES: 241,
MSP_SET_SERVO_MIX_RULE: 242,
MSP_EEPROM_WRITE: 250,
@ -80,7 +83,7 @@ var MSP_codes = {
MSP_ACC_TRIM: 240, // get acc angle trim values
MSP_SET_ACC_TRIM: 239, // set acc angle trim values
MSP_GPS_SV_INFO: 164, // get Signal Strength
// Additional private MSP for baseflight configurator (yes thats us \o/)
MSP_RX_MAP: 64, // get channel map (also returns number of channels total)
MSP_SET_RX_MAP: 65, // set rc map, numchannels to set comes from MSP_RX_MAP
@ -102,6 +105,7 @@ var MSP = {
callbacks: [],
packet_error: 0,
unsupported: 0,
ledDirectionLetters: ['n', 'e', 's', 'w', 'u', 'd'], // in LSB bit order
ledFunctionLetters: ['i', 'w', 'f', 'a', 't', 'r', 'c'], // in LSB bit order
@ -138,10 +142,14 @@ var MSP = {
}
break;
case 2: // direction (should be >)
this.unsupported = 0;
if (data[i] == 62) { // >
this.message_direction = 1;
} else { // <
} else if (data[i] == 60) { // <
this.message_direction = 0;
} else if (data[i] == 33) { // !
// FC reports unsupported message error
this.unsupported = 1;
}
this.state++;
@ -202,7 +210,7 @@ var MSP = {
process_data: function (code, message_buffer, message_length) {
var data = new DataView(message_buffer, 0); // DataView (allowing us to view arrayBuffer as struct/union)
switch (code) {
if (!this.unsupported) switch (code) {
case MSP_codes.MSP_IDENT:
console.log('Using deprecated msp command: MSP_IDENT');
// Deprecated
@ -377,7 +385,6 @@ var MSP = {
}
break;
case MSP_codes.MSP_MISC: // 22 bytes
<<<<<<< HEAD
var offset = 0;
MISC.midrc = data.getInt16(offset, 1);
offset += 2;
@ -401,24 +408,6 @@ var MSP = {
MISC.vbatmincellvoltage = data.getUint8(offset++, 1) / 10; // 10-50
MISC.vbatmaxcellvoltage = data.getUint8(offset++, 1) / 10; // 10-50
MISC.vbatwarningcellvoltage = data.getUint8(offset++, 1) / 10; // 10-50
=======
MISC.midrc = data.getInt16(0, 1);
MISC.minthrottle = data.getUint16(2, 1); // 0-2000
MISC.maxthrottle = data.getUint16(4, 1); // 0-2000
MISC.mincommand = data.getUint16(6, 1); // 0-2000
MISC.failsafe_throttle = data.getUint16(8, 1); // 1000-2000
MISC.gps_type = data.getUint8(10);
MISC.gps_baudrate = data.getUint8(11);
MISC.gps_ubx_sbas = data.getInt8(12);
MISC.multiwiicurrentoutput = data.getUint8(13);
MISC.rssi_aux_channel = data.getUint8(14);
MISC.placeholder2 = data.getUint8(15);
MISC.mag_declination = data.getInt16(16, 1) / 10; // -18000-18000
MISC.vbatscale = data.getUint8(18, 1); // 10-200
MISC.vbatmincellvoltage = data.getUint8(19, 1) / 10; // 10-50
MISC.vbatmaxcellvoltage = data.getUint8(20, 1) / 10; // 10-50
MISC.vbatwarningcellvoltage = data.getUint8(21, 1) / 10; // 10-50
>>>>>>> origin/baseflight-configurator-development
break;
case MSP_codes.MSP_MOTOR_PINS:
console.log(data);
@ -463,19 +452,52 @@ var MSP = {
AUX_CONFIG_IDS.push(data.getUint8(i));
}
break;
case MSP_codes.MSP_SERVO_CONF:
case MSP_codes.MSP_SERVO_MIX_RULES:
break;
case MSP_codes.MSP_SERVO_CONFIGURATIONS:
SERVO_CONFIG = []; // empty the array as new data is coming in
if (data.byteLength % 7 == 0) {
for (var i = 0; i < data.byteLength; i += 7) {
var arr = {
'min': data.getInt16(i, 1),
'max': data.getInt16(i + 2, 1),
'middle': data.getInt16(i + 4, 1),
'rate': data.getInt8(i + 6)
};
SERVO_CONFIG.push(arr);
if (semver.gte(CONFIG.apiVersion, "1.12.0")) {
if (data.byteLength % 14 == 0) {
for (var i = 0; i < data.byteLength; i += 14) {
var arr = {
'min': data.getInt16(i + 0, 1),
'max': data.getInt16(i + 2, 1),
'middle': data.getInt16(i + 4, 1),
'rate': data.getInt8(i + 6),
'angleAtMin': data.getInt8(i + 7),
'angleAtMax': data.getInt8(i + 8),
'indexOfChannelToForward': data.getInt8(i + 9),
'reversedInputSources': data.getUint32(i + 10)
};
SERVO_CONFIG.push(arr);
}
}
} else {
if (data.byteLength % 7 == 0) {
for (var i = 0; i < data.byteLength; i += 7) {
var arr = {
'min': data.getInt16(i + 0, 1),
'max': data.getInt16(i + 2, 1),
'middle': data.getInt16(i + 4, 1),
'rate': data.getInt8(i + 6),
'angleAtMin': 90,
'angleAtMax': 90,
'indexOfChannelToForward': undefined,
'reversedInputSources': 0
};
SERVO_CONFIG.push(arr);
}
}
if (semver.eq(CONFIG.apiVersion, '1.10.0')) {
// drop two unused servo configurations due to MSP rx buffer to small)
while (SERVO_CONFIG.length > 8) {
SERVO_CONFIG.pop();
}
}
}
break;
@ -509,7 +531,7 @@ var MSP = {
case MSP_codes.MSP_SELECT_SETTING:
console.log('Profile selected');
break;
case MSP_codes.MSP_SET_SERVO_CONF:
case MSP_codes.MSP_SET_SERVO_CONFIGURATION:
console.log('Servo Configuration saved');
break;
case MSP_codes.MSP_EEPROM_WRITE:
@ -817,6 +839,8 @@ var MSP = {
default:
console.log('Unknown code detected: ' + code);
} else {
console.log('FC reports unsupported message error: ' + code);
}
// trigger callbacks, cleanup/remove callback after trigger
@ -963,43 +987,43 @@ MSP.crunch = function (code) {
case 7:
case 8:
case 9:
buffer.push(parseInt(PIDs[i][0] * 10));
buffer.push(parseInt(PIDs[i][1] * 1000));
buffer.push(Math.round(PIDs[i][0] * 10));
buffer.push(Math.round(PIDs[i][1] * 1000));
buffer.push(parseInt(PIDs[i][2]));
break;
case 4:
buffer.push(parseInt(PIDs[i][0] * 100));
buffer.push(parseInt(PIDs[i][1] * 100));
buffer.push(Math.round(PIDs[i][0] * 100));
buffer.push(Math.round(PIDs[i][1] * 100));
buffer.push(parseInt(PIDs[i][2]));
break;
case 5:
case 6:
buffer.push(parseInt(PIDs[i][0] * 10));
buffer.push(parseInt(PIDs[i][1] * 100));
buffer.push(parseInt(PIDs[i][2] * 1000));
buffer.push(Math.round(PIDs[i][0] * 10));
buffer.push(Math.round(PIDs[i][1] * 100));
buffer.push(Math.round(PIDs[i][2] * 1000));
break;
}
}
break;
case MSP_codes.MSP_SET_RC_TUNING:
buffer.push(parseInt(RC_tuning.RC_RATE * 100));
buffer.push(parseInt(RC_tuning.RC_EXPO * 100));
buffer.push(Math.round(RC_tuning.RC_RATE * 100));
buffer.push(Math.round(RC_tuning.RC_EXPO * 100));
if (semver.lt(CONFIG.apiVersion, "1.7.0")) {
buffer.push(parseInt(RC_tuning.roll_pitch_rate * 100));
buffer.push(Math.round(RC_tuning.roll_pitch_rate * 100));
} else {
buffer.push(parseInt(RC_tuning.roll_rate * 100));
buffer.push(parseInt(RC_tuning.pitch_rate * 100));
buffer.push(Math.round(RC_tuning.roll_rate * 100));
buffer.push(Math.round(RC_tuning.pitch_rate * 100));
}
buffer.push(parseInt(RC_tuning.yaw_rate * 100));
buffer.push(parseInt(RC_tuning.dynamic_THR_PID * 100));
buffer.push(parseInt(RC_tuning.throttle_MID * 100));
buffer.push(parseInt(RC_tuning.throttle_EXPO * 100));
buffer.push(Math.round(RC_tuning.yaw_rate * 100));
buffer.push(Math.round(RC_tuning.dynamic_THR_PID * 100));
buffer.push(Math.round(RC_tuning.throttle_MID * 100));
buffer.push(Math.round(RC_tuning.throttle_EXPO * 100));
if (semver.gte(CONFIG.apiVersion, "1.7.0")) {
buffer.push(lowByte(RC_tuning.dynamic_THR_breakpoint));
buffer.push(highByte(RC_tuning.dynamic_THR_breakpoint));
}
if (semver.gte(CONFIG.apiVersion, "1.10.0")) {
buffer.push(parseInt(RC_tuning.RC_YAW_EXPO * 100));
buffer.push(Math.round(RC_tuning.RC_YAW_EXPO * 100));
}
break;
// Disabled, cleanflight does not use MSP_SET_BOX.
@ -1047,26 +1071,12 @@ MSP.crunch = function (code) {
buffer.push(MISC.multiwiicurrentoutput);
buffer.push(MISC.rssi_channel);
buffer.push(MISC.placeholder2);
buffer.push(lowByte(MISC.mag_declination * 10));
buffer.push(highByte(MISC.mag_declination * 10));
buffer.push(lowByte(Math.round(MISC.mag_declination * 10)));
buffer.push(highByte(Math.round(MISC.mag_declination * 10)));
buffer.push(MISC.vbatscale);
buffer.push(MISC.vbatmincellvoltage * 10);
buffer.push(MISC.vbatmaxcellvoltage * 10);
buffer.push(MISC.vbatwarningcellvoltage * 10);
break;
case MSP_codes.MSP_SET_SERVO_CONF:
for (var i = 0; i < SERVO_CONFIG.length; i++) {
buffer.push(lowByte(SERVO_CONFIG[i].min));
buffer.push(highByte(SERVO_CONFIG[i].min));
buffer.push(lowByte(SERVO_CONFIG[i].max));
buffer.push(highByte(SERVO_CONFIG[i].max));
buffer.push(lowByte(SERVO_CONFIG[i].middle));
buffer.push(highByte(SERVO_CONFIG[i].middle));
buffer.push(lowByte(SERVO_CONFIG[i].rate));
}
buffer.push(Math.round(MISC.vbatmincellvoltage * 10));
buffer.push(Math.round(MISC.vbatmaxcellvoltage * 10));
buffer.push(Math.round(MISC.vbatwarningcellvoltage * 10));
break;
case MSP_codes.MSP_SET_CHANNEL_FORWARDING:
for (var i = 0; i < SERVO_CONFIG.length; i++) {
@ -1127,6 +1137,22 @@ MSP.crunch = function (code) {
return buffer;
};
/**
* 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.
*/
MSP.setRawRx = function(channels) {
var buffer = [];
for (var i = 0; i < channels.length; i++) {
buffer.push(specificByte(channels[i], 0));
buffer.push(specificByte(channels[i], 1));
}
MSP.send_message(MSP_codes.MSP_SET_RAW_RC, buffer, false);
}
/**
* Send a request to read a block of data from the dataflash at the given address and pass that address and a dataview
* of the returned data to the given callback (or null for the data if an error occured).
@ -1147,7 +1173,102 @@ MSP.dataflashRead = function(address, onDataCallback) {
onDataCallback(address, null);
}
});
} ;
};
MSP.sendServoMixRules = function(onCompleteCallback) {
// TODO implement
onCompleteCallback();
};
MSP.sendServoConfigurations = function(onCompleteCallback) {
var nextFunction = send_next_servo_configuration;
var servoIndex = 0;
if (SERVO_CONFIG.length == 0) {
onCompleteCallback();
}
nextFunction();
function send_next_servo_configuration() {
var buffer = [];
if (semver.lt(CONFIG.apiVersion, "1.12.0")) {
// send all in one go
// 1.9.0 had a bug where the MSP input buffer was too small, limit to 8.
for (var i = 0; i < SERVO_CONFIG.length && i < 8; i++) {
buffer.push(lowByte(SERVO_CONFIG[i].min));
buffer.push(highByte(SERVO_CONFIG[i].min));
buffer.push(lowByte(SERVO_CONFIG[i].max));
buffer.push(highByte(SERVO_CONFIG[i].max));
buffer.push(lowByte(SERVO_CONFIG[i].middle));
buffer.push(highByte(SERVO_CONFIG[i].middle));
buffer.push(lowByte(SERVO_CONFIG[i].rate));
}
nextFunction = send_channel_forwarding;
} else {
// send one at a time, with index
var servoConfiguration = SERVO_CONFIG[servoIndex];
buffer.push(servoIndex);
buffer.push(lowByte(servoConfiguration.min));
buffer.push(highByte(servoConfiguration.min));
buffer.push(lowByte(servoConfiguration.max));
buffer.push(highByte(servoConfiguration.max));
buffer.push(lowByte(servoConfiguration.middle));
buffer.push(highByte(servoConfiguration.middle));
buffer.push(lowByte(servoConfiguration.rate));
buffer.push(servoConfiguration.angleAtMin);
buffer.push(servoConfiguration.angleAtMax);
var out = servoConfiguration.indexOfChannelToForward;
if (out == undefined) {
out = 255; // Cleanflight defines "CHANNEL_FORWARDING_DISABLED" as "(uint8_t)0xFF"
}
buffer.push(out);
buffer.push(specificByte(servoConfiguration.reversedInputSources, 0));
buffer.push(specificByte(servoConfiguration.reversedInputSources, 1));
buffer.push(specificByte(servoConfiguration.reversedInputSources, 2));
buffer.push(specificByte(servoConfiguration.reversedInputSources, 3));
// prepare for next iteration
servoIndex++;
if (servoIndex == SERVO_CONFIG.length) {
nextFunction = onCompleteCallback;
}
}
MSP.send_message(MSP_codes.MSP_SET_SERVO_CONFIGURATION, buffer, false, nextFunction);
}
function send_channel_forwarding() {
var buffer = [];
for (var i = 0; i < SERVO_CONFIG.length; i++) {
var out = SERVO_CONFIG[i].indexOfChannelToForward;
if (out == undefined) {
out = 255; // Cleanflight defines "CHANNEL_FORWARDING_DISABLED" as "(uint8_t)0xFF"
}
buffer.push(out);
}
nextFunction = onCompleteCallback;
MSP.send_message(MSP_codes.MSP_SET_CHANNEL_FORWARDING, buffer, false, nextFunction);
}
};
MSP.sendModeRanges = function(onCompleteCallback) {
var nextFunction = send_next_mode_range;
@ -1165,12 +1286,12 @@ MSP.sendModeRanges = function(onCompleteCallback) {
var modeRange = MODE_RANGES[modeRangeIndex];
var AUX_val_buffer_out = [];
AUX_val_buffer_out.push(modeRangeIndex);
AUX_val_buffer_out.push(modeRange.id);
AUX_val_buffer_out.push(modeRange.auxChannelIndex);
AUX_val_buffer_out.push((modeRange.range.start - 900) / 25);
AUX_val_buffer_out.push((modeRange.range.end - 900) / 25);
var buffer = [];
buffer.push(modeRangeIndex);
buffer.push(modeRange.id);
buffer.push(modeRange.auxChannelIndex);
buffer.push((modeRange.range.start - 900) / 25);
buffer.push((modeRange.range.end - 900) / 25);
// prepare for next iteration
modeRangeIndex++;
@ -1178,7 +1299,7 @@ MSP.sendModeRanges = function(onCompleteCallback) {
nextFunction = onCompleteCallback;
}
MSP.send_message(MSP_codes.MSP_SET_MODE_RANGE, AUX_val_buffer_out, false, nextFunction);
MSP.send_message(MSP_codes.MSP_SET_MODE_RANGE, buffer, false, nextFunction);
}
};
@ -1198,14 +1319,14 @@ MSP.sendAdjustmentRanges = function(onCompleteCallback) {
var adjustmentRange = ADJUSTMENT_RANGES[adjustmentRangeIndex];
var ADJUSTMENT_val_buffer_out = [];
ADJUSTMENT_val_buffer_out.push(adjustmentRangeIndex);
ADJUSTMENT_val_buffer_out.push(adjustmentRange.slotIndex);
ADJUSTMENT_val_buffer_out.push(adjustmentRange.auxChannelIndex);
ADJUSTMENT_val_buffer_out.push((adjustmentRange.range.start - 900) / 25);
ADJUSTMENT_val_buffer_out.push((adjustmentRange.range.end - 900) / 25);
ADJUSTMENT_val_buffer_out.push(adjustmentRange.adjustmentFunction);
ADJUSTMENT_val_buffer_out.push(adjustmentRange.auxSwitchChannelIndex);
var buffer = [];
buffer.push(adjustmentRangeIndex);
buffer.push(adjustmentRange.slotIndex);
buffer.push(adjustmentRange.auxChannelIndex);
buffer.push((adjustmentRange.range.start - 900) / 25);
buffer.push((adjustmentRange.range.end - 900) / 25);
buffer.push(adjustmentRange.adjustmentFunction);
buffer.push(adjustmentRange.auxSwitchChannelIndex);
// prepare for next iteration
adjustmentRangeIndex++;
@ -1213,7 +1334,7 @@ MSP.sendAdjustmentRanges = function(onCompleteCallback) {
nextFunction = onCompleteCallback;
}
MSP.send_message(MSP_codes.MSP_SET_ADJUSTMENT_RANGE, ADJUSTMENT_val_buffer_out, false, nextFunction);
MSP.send_message(MSP_codes.MSP_SET_ADJUSTMENT_RANGE, buffer, false, nextFunction);
}
};