Last active
November 8, 2024 11:16
-
-
Save cs8425/51893e2f90812aa3831558503597fa1a to your computer and use it in GitHub Desktop.
read 'USB Simulator Cable' as joystick and send to betaflight SITL via MSP over tcp.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
'use strict'; | |
const net = require('net') | |
var ip = '127.0.0.1' | |
var port = 5762 // MSP port on UART2 | |
// via MSP: | |
const MSP = require('./node-msp') // https://github.com/cs8425/node-msp | |
var msp = new MSP() | |
var init = 0; | |
var RC_MAP = ''; | |
var channelMSPIndexes = { | |
A: 3, | |
E: 2, | |
R: 1, | |
T: 0, | |
'1': 4, | |
'2': 5, | |
'3': 6, | |
'4': 7, | |
} | |
msp.on('frame', function(err, frame){ | |
if(err) return | |
// console.log((new Date()).getTime(), 'frame', JSON.stringify(frame)) | |
// var obj = msp.parseFrame(frame) | |
// console.log((new Date()).getTime(), 'data', obj) | |
}) | |
msp.on('data', function(obj){ | |
// console.log((new Date()).getTime(), 'data', obj.code, obj) | |
if(obj.code == msp.Codes.MSP_RX_MAP){ | |
RC_MAP = obj.RC_MAP; | |
for(var i=0; i<RC_MAP.length; i++){ | |
// console.log(RC_MAP[i], i) | |
channelMSPIndexes[RC_MAP[i]] = i | |
} | |
console.log('RC_MAP', RC_MAP) | |
console.log('channelMSPIndexes', channelMSPIndexes) | |
init = 1; | |
} | |
}) | |
var client = net.connect(port, ip, function(){ | |
console.log('connected to FC!', msp) | |
msp.setSender(function(data){ | |
//console.log('_write', data) | |
client.write(data) | |
}) | |
// msp.pull_FC_info() | |
msp.send_message(msp.Codes.MSP_RX_MAP, false, false); | |
}) | |
client.on('error', function(err){ | |
console.log('FC err', err) | |
js.close() | |
init = 0 | |
}) | |
client.on('data', function(data){ | |
//console.log(data) | |
msp.readbytes(data) | |
}) | |
client.on('end', function(){ | |
console.log('disconnected from server') | |
}) | |
//////////////////////// | |
// from: https://gist.github.com/creationix/1695870 | |
/////////////////////// | |
var FS = require('fs'); | |
var EventEmitter = require('events').EventEmitter; | |
// http://www.mjmwired.net/kernel/Documentation/input/joystick-api.txt | |
function parse(buffer) { | |
var event = { | |
time: buffer.readUInt32LE(0), | |
number: buffer[7], | |
value: buffer.readInt16LE(4) | |
} | |
if (buffer[6] & 0x80) event.init = true; | |
if (buffer[6] & 0x01) event.type = "button"; | |
if (buffer[6] & 0x02) event.type = "axis"; | |
return event; | |
} | |
// Expose as a nice JavaScript API | |
function Joystick(id) { | |
this.onOpen = this.onOpen.bind(this); | |
this.onRead = this.onRead.bind(this); | |
this.buffer = new Buffer(8); | |
FS.open("/dev/input/js" + id, "r", this.onOpen); | |
} | |
Joystick.prototype = Object.create(EventEmitter.prototype, { | |
constructor: {value: Joystick} | |
}); | |
Joystick.prototype.onOpen = function (err, fd) { | |
if (err) return this.emit("error", err); | |
this.fd = fd; | |
this.startRead(); | |
}; | |
Joystick.prototype.startRead = function () { | |
FS.read(this.fd, this.buffer, 0, 8, null, this.onRead); | |
}; | |
Joystick.prototype.onRead = function (err, bytesRead) { | |
if (err) return this.emit("error", err); | |
var event = parse(this.buffer); | |
this.emit(event.type, event); | |
if (this.fd) this.startRead(); | |
}; | |
Joystick.prototype.close = function (callback) { | |
FS.close(this.fd, callback); | |
this.fd = undefined; | |
}; | |
/////////////////// | |
// define joystick channel | |
var joy2RC = { | |
0: 'A', | |
1: 'E', | |
2: '1', | |
3: 'T', | |
4: 'R', | |
5: '2', | |
} | |
var joyBtn2RC = { | |
0: '1', | |
1: '4', | |
2: '3', | |
3: '3', | |
4: '3', | |
5: '3', | |
} | |
var channelValues = [1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500]; | |
var js = new Joystick(0); | |
js.on('button', function(data){ | |
var ch = data.number | |
var val = data.value | |
val = 800 * val + 1100 | |
channelValues[channelMSPIndexes[joyBtn2RC[ch]]] = val; | |
}); | |
js.on('axis', function(data){ | |
var ch = data.number | |
var val = data.value | |
val = (1000 * val / 65535) + 1500 | |
channelValues[channelMSPIndexes[joy2RC[ch]]] = val; | |
console.log(channelValues) | |
// msp.send_message(msp.Codes.MSP_SET_RAW_RC, channelValues); | |
}); | |
function sendMSP() { | |
if(init){ | |
var buf = Buffer.alloc(channelValues.length*2) | |
for(var i=0; i<channelValues.length; i++){ | |
buf.writeUInt16LE(channelValues[i] ,2*i) | |
} | |
msp.send_message(msp.Codes.MSP_SET_RAW_RC, buf) | |
} | |
var t = setTimeout(sendMSP, 20); | |
} | |
sendMSP() | |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// from https://github.com/betaflight/betaflight-configurator | |
'use strict'; | |
//MSPCodes needs to be re-integrated inside MSP object | |
var MSPCodes = { | |
MSP_API_VERSION: 1, | |
MSP_FC_VARIANT: 2, | |
MSP_FC_VERSION: 3, | |
MSP_BOARD_INFO: 4, | |
MSP_BUILD_INFO: 5, | |
MSP_NAME: 10, | |
MSP_SET_NAME: 11, | |
MSP_EXT_CMD: 20, | |
MSP_CHANNEL_FORWARDING: 32, | |
MSP_SET_CHANNEL_FORWARDING: 33, | |
MSP_MODE_RANGES: 34, | |
MSP_SET_MODE_RANGE: 35, | |
MSP_CURRENT_METER_CONFIG: 40, | |
MSP_SET_CURRENT_METER_CONFIG: 41, | |
MSP_RX_CONFIG: 44, | |
MSP_SET_RX_CONFIG: 45, | |
MSP_LED_COLORS: 46, | |
MSP_SET_LED_COLORS: 47, | |
MSP_LED_STRIP_CONFIG: 48, | |
MSP_SET_LED_STRIP_CONFIG: 49, | |
MSP_ADJUSTMENT_RANGES: 52, | |
MSP_SET_ADJUSTMENT_RANGE: 53, | |
MSP_CF_SERIAL_CONFIG: 54, | |
MSP_SET_CF_SERIAL_CONFIG: 55, | |
MSP_VOLTAGE_METER_CONFIG: 56, | |
MSP_SET_VOLTAGE_METER_CONFIG: 57, | |
MSP_SONAR: 58, | |
MSP_PID_CONTROLLER: 59, | |
MSP_SET_PID_CONTROLLER: 60, | |
MSP_ARMING_CONFIG: 61, | |
MSP_SET_ARMING_CONFIG: 62, | |
MSP_RX_MAP: 64, | |
MSP_SET_RX_MAP: 65, | |
MSP_BF_CONFIG: 66, | |
MSP_SET_BF_CONFIG: 67, | |
MSP_SET_REBOOT: 68, | |
MSP_BF_BUILD_INFO: 69, // Not used | |
MSP_DATAFLASH_SUMMARY: 70, | |
MSP_DATAFLASH_READ: 71, | |
MSP_DATAFLASH_ERASE: 72, | |
MSP_LOOP_TIME: 73, | |
MSP_SET_LOOP_TIME: 74, | |
MSP_FAILSAFE_CONFIG: 75, | |
MSP_SET_FAILSAFE_CONFIG: 76, | |
MSP_RXFAIL_CONFIG: 77, | |
MSP_SET_RXFAIL_CONFIG: 78, | |
MSP_SDCARD_SUMMARY: 79, | |
MSP_BLACKBOX_CONFIG: 80, | |
MSP_SET_BLACKBOX_CONFIG: 81, | |
MSP_TRANSPONDER_CONFIG: 82, | |
MSP_SET_TRANSPONDER_CONFIG: 83, | |
MSP_OSD_CONFIG: 84, | |
MSP_SET_OSD_CONFIG: 85, | |
MSP_OSD_CHAR_READ: 86, | |
MSP_OSD_CHAR_WRITE: 87, | |
MSP_VTX_CONFIG: 88, | |
MSP_SET_VTX_CONFIG: 89, | |
MSP_ADVANCED_CONFIG: 90, | |
MSP_SET_ADVANCED_CONFIG: 91, | |
MSP_FILTER_CONFIG: 92, | |
MSP_SET_FILTER_CONFIG: 93, | |
MSP_PID_ADVANCED: 94, | |
MSP_SET_PID_ADVANCED: 95, | |
MSP_SENSOR_CONFIG: 96, | |
MSP_SET_SENSOR_CONFIG: 97, | |
// MSP_SPECIAL_PARAMETERS: 98, removed | |
//MSP_SET_SPECIAL_PARAMETERS: 99, removed | |
MSP_IDENT: 100, // Not used | |
MSP_STATUS: 101, | |
MSP_RAW_IMU: 102, | |
MSP_SERVO: 103, | |
MSP_MOTOR: 104, | |
MSP_RC: 105, | |
MSP_RAW_GPS: 106, | |
MSP_COMP_GPS: 107, | |
MSP_ATTITUDE: 108, | |
MSP_ALTITUDE: 109, | |
MSP_ANALOG: 110, | |
MSP_RC_TUNING: 111, | |
MSP_PID: 112, | |
MSP_BOX: 113, // Not used | |
MSP_MISC: 114, | |
MSP_MOTOR_PINS: 115, // Not used | |
MSP_BOXNAMES: 116, | |
MSP_PIDNAMES: 117, | |
MSP_WP: 118, // Not used | |
MSP_BOXIDS: 119, | |
MSP_SERVO_CONFIGURATIONS: 120, | |
MSP_3D: 124, | |
MSP_RC_DEADBAND: 125, | |
MSP_SENSOR_ALIGNMENT: 126, | |
MSP_LED_STRIP_MODECOLOR: 127, | |
MSP_STATUS_EX: 150, | |
MSP_UID: 160, | |
MSP_GPS_SV_INFO: 164, | |
MSP_DISPLAYPORT: 182, | |
MSP_SET_RAW_RC: 200, | |
MSP_SET_RAW_GPS: 201, // Not used | |
MSP_SET_PID: 202, | |
MSP_SET_BOX: 203, | |
MSP_SET_RC_TUNING: 204, | |
MSP_ACC_CALIBRATION: 205, | |
MSP_MAG_CALIBRATION: 206, | |
MSP_SET_MISC: 207, | |
MSP_RESET_CONF: 208, | |
MSP_SET_WP: 209, // Not used | |
MSP_SELECT_SETTING: 210, | |
MSP_SET_HEAD: 211, // Not used | |
MSP_SET_SERVO_CONFIGURATION: 212, | |
MSP_SET_MOTOR: 214, | |
MSP_SET_3D: 217, | |
MSP_SET_RC_DEADBAND: 218, | |
MSP_SET_RESET_CURR_PID: 219, | |
MSP_SET_SENSOR_ALIGNMENT: 220, | |
MSP_SET_LED_STRIP_MODECOLOR: 221, | |
MSP_SET_ACC_TRIM: 239, | |
MSP_ACC_TRIM: 240, | |
MSP_SERVO_MIX_RULES: 241, | |
MSP_SET_SERVO_MIX_RULE: 242, // Not used | |
MSP_EEPROM_WRITE: 250, | |
MSP_DEBUGMSG: 253, // Not used | |
MSP_DEBUG: 254 | |
}; | |
module.exports = MSPCodes |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// copy & modify from https://github.com/betaflight/betaflight-configurator | |
'use strict'; | |
var util = require('util'); | |
var EventEmitter = require('events').EventEmitter; | |
var MSPCodes = require('./MSPCodes'); | |
var MSP = function () { | |
var self = this | |
self.state = 0 | |
self.message_direction = 1 // 1, '>' from FC; 0, '<' to FC | |
self.message_length_expected = 0 | |
self.message_length_received = 0 | |
self.message_checksum = 0 | |
self.messageIsJumboFrame = false | |
self.crcError = false | |
self.message_buffer = null | |
self.message_buffer_uint8_view = null | |
self.packet_error = 0 | |
self.unsupported = 0 | |
self.last_received_timestamp = null | |
self.JUMBO_FRAME_SIZE_LIMIT = 255 | |
self.timeout = 1000 | |
self.sender = null | |
self.sendFIFO = [] | |
self.sending = false | |
// self.on('data', read) | |
}; | |
util.inherits(MSP, EventEmitter); | |
module.exports = MSP; | |
MSP.prototype.Codes = MSPCodes | |
MSP.prototype.setSender = function(func){ | |
var self = this | |
if(func){ | |
self.sender = func | |
}else{ | |
self.sender = null | |
} | |
} | |
MSP.prototype.sendRawFrame = function(rawFrame){ | |
var self = this | |
self.sendFIFO.push(rawFrame) | |
if(!self.sending){ | |
// process.nextTick(self.sendWorker, self) | |
setImmediate(self.sendWorker, self) | |
} | |
} | |
MSP.prototype.sendWorker = function(self){ | |
// var self = this | |
if(self.sendFIFO.length){ | |
self.sender(self.sendFIFO.shift()) | |
} | |
if(self.sendFIFO.length){ | |
// process.nextTick(self.sendWorker, self) | |
setImmediate(self.sendWorker, self) | |
} | |
} | |
MSP.prototype.readbytes = function (data){ | |
var self = this | |
for (var i = 0; i < data.length; i++) { | |
switch (self.state) { | |
case 0: // sync char 1 | |
if (data[i] == 36) { // $ | |
self.state++; | |
} | |
break; | |
case 1: // sync char 2 | |
if (data[i] == 77) { // M | |
self.state++; | |
} else { // restart and try again | |
self.state = 0; | |
} | |
break; | |
case 2: // direction (should be >) | |
self.unsupported = 0; | |
if (data[i] == 62) { // > from FC | |
self.message_direction = 1; | |
} else if (data[i] == 60) { // < to FC | |
self.message_direction = 0; | |
} else if (data[i] == 33) { // ! | |
// FC reports unsupported message error | |
self.unsupported = 1; | |
} | |
self.state++; | |
break; | |
case 3: | |
self.message_length_expected = data[i]; | |
if (self.message_length_expected === self.JUMBO_FRAME_SIZE_LIMIT) { | |
self.messageIsJumboFrame = true; | |
} | |
self.message_checksum = data[i]; | |
self.state++; | |
break; | |
case 4: | |
self.code = data[i]; | |
self.message_checksum ^= data[i]; | |
if (self.message_length_expected > 0) { | |
// process payload | |
if (self.messageIsJumboFrame) { | |
self.state++; | |
} else { | |
self.state = self.state + 3; | |
} | |
} else { | |
// no payload | |
self.state += 5; | |
self.message_buffer = new ArrayBuffer(0); | |
self.message_buffer_uint8_view = new Uint8Array(self.message_buffer); | |
} | |
break; | |
case 5: | |
self.message_length_expected = data[i]; | |
self.message_checksum ^= data[i]; | |
self.state++; | |
break; | |
case 6: | |
self.message_length_expected = self.message_length_expected + 256 * data[i]; | |
self.message_checksum ^= data[i]; | |
self.state++; | |
break; | |
case 7: | |
// setup arraybuffer | |
self.message_buffer = new ArrayBuffer(self.message_length_expected); | |
self.message_buffer_uint8_view = new Uint8Array(self.message_buffer); | |
self.state++; | |
case 8: // payload | |
self.message_buffer_uint8_view[self.message_length_received] = data[i]; | |
self.message_checksum ^= data[i]; | |
self.message_length_received++; | |
if (self.message_length_received >= self.message_length_expected) { | |
self.state++; | |
} | |
break; | |
case 9: | |
var buf = null | |
if (self.message_checksum == data[i]) { | |
// message received, store buffer | |
buf = Buffer.from(self.message_buffer, 0, self.message_length_expected) | |
} else { | |
console.log('code: ' + self.code + ' - crc failed'); | |
self.packet_error++; | |
self.crcError = true; | |
} | |
// Reset variables | |
self.message_length_received = 0; | |
self.state = 0; | |
self.messageIsJumboFrame = false; | |
//self.notify(); | |
var frame = { | |
crcError: self.crcError, | |
dir: self.message_direction, | |
code: self.code, | |
data: buf | |
} | |
self.emit('frame', self.crcError, frame) | |
// send raw frame if is a custom command | |
if(self.code == MSPCodes.MSP_EXT_CMD){ | |
self.emit('extcmd', frame) | |
} | |
var obj = self.parseFrame(frame) | |
self.emit('data', obj) | |
self.crcError = false; | |
break; | |
default: | |
console.log('Unknown state detected: ' + self.state); | |
} | |
} | |
} | |
MSP.prototype.create_message = function (code, data, dir) { | |
var self = this | |
var bufferOut, bufView; | |
var dircode = 0 | |
if(dir){ | |
dircode = '>'.charCodeAt(0) // 62 '>' from FC | |
}else{ | |
dircode = '<'.charCodeAt(0) // 60 '<' to FC | |
} | |
// always reserve 6 bytes for protocol overhead ! | |
if (data) { | |
var size = data.length + 6, | |
checksum = 0; | |
bufferOut = new ArrayBuffer(size); | |
bufView = new Uint8Array(bufferOut); | |
bufView[0] = 36; // $ | |
bufView[1] = 77; // M | |
bufView[2] = dircode; // < or > | |
bufView[3] = data.length; | |
bufView[4] = code; | |
checksum = bufView[3] ^ bufView[4]; | |
for (var i = 0; i < data.length; i++) { | |
bufView[i + 5] = data[i]; | |
checksum ^= bufView[i + 5]; | |
} | |
bufView[5 + data.length] = checksum; | |
} else { | |
bufferOut = new ArrayBuffer(6); | |
bufView = new Uint8Array(bufferOut); | |
bufView[0] = 36; // $ | |
bufView[1] = 77; // M | |
bufView[2] = dircode; // < or > | |
bufView[3] = 0; // data length | |
bufView[4] = code; // code | |
bufView[5] = bufView[3] ^ bufView[4]; // checksum | |
} | |
return Buffer.from(bufferOut); | |
} | |
MSP.prototype.send_message = function (code, data, callback) { | |
var self = this | |
var buf = self.create_message(code, data) | |
self.sendRawFrame(buf) | |
} | |
MSP.prototype.pull_FC_info = function (callback) { | |
var self = this | |
var pulllist = [ | |
MSPCodes.MSP_API_VERSION, | |
MSPCodes.MSP_FC_VARIANT, | |
MSPCodes.MSP_FC_VERSION, | |
MSPCodes.MSP_BOARD_INFO, | |
MSPCodes.MSP_BUILD_INFO, | |
MSPCodes.MSP_NAME, | |
MSPCodes.MSP_UID, | |
MSPCodes.MSP_RX_MAP, | |
MSPCodes.MSP_BOXNAMES, | |
MSPCodes.MSP_BOXIDS, | |
// MSPCodes.MSP_PIDNAMES, | |
// MSPCodes.MSP_PID, | |
] | |
for(var i=0; i<pulllist.length; i++){ | |
self.send_message(pulllist[i], null) | |
} | |
} | |
MSP.prototype.parseFrame = function (frame, dir){ | |
var self = this | |
var dir = frame.dir | |
var code = frame.code | |
var data = frame.data | |
var crcError = frame.crcError | |
var obj = {} | |
if(crcError){ | |
return obj | |
} | |
if(dir == 0){ // to FC | |
return obj | |
} | |
obj.code = code | |
switch (code) { | |
case MSPCodes.MSP_STATUS: | |
obj.cycleTime = data.readU16() | |
obj.i2cError = data.readU16() | |
obj.activeSensors = data.readU16() | |
obj.mode = data.readU32() | |
obj.profile = data.readU8() | |
break | |
case MSPCodes.MSP_STATUS_EX: | |
obj.cycleTime = data.readU16() | |
obj.i2cError = data.readU16() | |
obj.activeSensors = data.readU16() | |
obj.mode = data.readU32() | |
obj.profile = data.readU8() | |
obj.cpuload = data.readU16() | |
obj.numProfiles = data.readU8() | |
obj.rateProfile = data.readU8() | |
break | |
case MSPCodes.MSP_RAW_IMU: | |
// 512 for mpu6050, 256 for mma | |
// currently we are unable to differentiate between the sensor types, so we are goign with 512 | |
obj.accelerometer = [0,0,0] | |
obj.accelerometer[0] = data.read16() / 512 | |
obj.accelerometer[1] = data.read16() / 512 | |
obj.accelerometer[2] = data.read16() / 512 | |
// properly scaled | |
obj.gyroscope = [0,0,0] | |
obj.gyroscope[0] = data.read16() * (4 / 16.4) | |
obj.gyroscope[1] = data.read16() * (4 / 16.4) | |
obj.gyroscope[2] = data.read16() * (4 / 16.4) | |
// no clue about scaling factor | |
obj.magnetometer = [0,0,0] | |
obj.magnetometer[0] = data.read16() / 1090 | |
obj.magnetometer[1] = data.read16() / 1090 | |
obj.magnetometer[2] = data.read16() / 1090 | |
break | |
case MSPCodes.MSP_SERVO: | |
obj.servo = [] | |
var servoCount = data.byteLength / 2 | |
var servo = obj.servo | |
for (var i = 0; i < servoCount; i++) { | |
servo.push(data.readU16()) | |
} | |
break | |
case MSPCodes.MSP_MOTOR: | |
obj.motor = [] | |
var motorCount = data.byteLength / 2 | |
var motor = obj.motor | |
for (var i = 0; i < motorCount; i++) { | |
motor.push(data.readU16()) | |
} | |
break | |
case MSPCodes.MSP_RC: | |
obj.active_channels = data.byteLength / 2 | |
obj.channels = [] | |
var channels = obj.channels | |
for (var i = 0; i < obj.active_channels; i++) { | |
channels.push(data.readU16()) | |
} | |
break | |
case MSPCodes.MSP_RAW_GPS: | |
obj.fix = data.readU8() | |
obj.numSat = data.readU8() | |
obj.lat = data.read32() | |
obj.lon = data.read32() | |
obj.alt = data.readU16() | |
obj.speed = data.readU16() | |
obj.ground_course = data.readU16() | |
break | |
case MSPCodes.MSP_COMP_GPS: | |
break | |
case MSPCodes.MSP_ATTITUDE: | |
obj.x = data.read16() / 10.0 // x | |
obj.y = data.read16() / 10.0 // y | |
obj.z = data.read16() // z | |
break | |
case MSPCodes.MSP_ALTITUDE: | |
obj.altitude = parseFloat((data.read32() / 100.0).toFixed(2)) // correct scale factor | |
break | |
case MSPCodes.MSP_DEBUG: | |
obj.debug = [] | |
var debug = obj.debug | |
for (var i = 0; i < 4; i++) debug.push(data.read16()) | |
break | |
// get once | |
case MSPCodes.MSP_API_VERSION: | |
obj.mspProtocolVersion = data.readU8() | |
obj.apiVersion = data.readU8() + '.' + data.readU8() + '.0' | |
break | |
case MSPCodes.MSP_FC_VARIANT: | |
var identifier = '' | |
for (var i = 0; i < 4; i++) { | |
identifier += String.fromCharCode(data.readU8()) | |
} | |
obj.flightControllerIdentifier = identifier | |
break | |
case MSPCodes.MSP_FC_VERSION: | |
obj.flightControllerVersion = data.readU8() + '.' + data.readU8() + '.' + data.readU8() | |
break | |
case MSPCodes.MSP_BUILD_INFO: | |
var dateLength = 11 | |
var buff = []; | |
for (var i = 0; i < dateLength; i++) { | |
buff.push(data.readU8()); | |
} | |
buff.push(32); // ascii space | |
var timeLength = 8; | |
for (var i = 0; i < timeLength; i++) { | |
buff.push(data.readU8()); | |
} | |
obj.buildInfo = String.fromCharCode.apply(null, buff); | |
break; | |
case MSPCodes.MSP_BOARD_INFO: | |
var identifier = '' | |
for (var i = 0; i < 4; i++) { | |
identifier += String.fromCharCode(data.readU8()); | |
} | |
obj.boardIdentifier = identifier | |
obj.boardVersion = data.readU16() | |
break | |
case MSPCodes.MSP_NAME: | |
obj.name = '' | |
if(data.length !== 0){ | |
var char | |
while ((char = data.readU8()) !== null) { | |
obj.name += String.fromCharCode(char) | |
} | |
} | |
break | |
case MSPCodes.MSP_UID: | |
obj.uid = data.readU32().toString(16) | |
obj.uid += '-' + data.readU32().toString(16) | |
obj.uid += '-' + data.readU32().toString(16) | |
break | |
case MSPCodes.MSP_RX_MAP: | |
var RC_MAP = []; // empty the array as new data is coming in | |
for (var i = 0; i < data.byteLength; i++) { | |
RC_MAP.push(data.readU8()) | |
} | |
var RC_MAP_Letters = ['A', 'E', 'R', 'T', '1', '2', '3', '4']; | |
var strBuffer = []; | |
for (var i = 0; i < RC_MAP.length; i++) { | |
strBuffer[RC_MAP[i]] = RC_MAP_Letters[i]; | |
} | |
obj.RC_MAP = strBuffer.join('') | |
break | |
case MSPCodes.MSP_BOXNAMES: | |
obj.AUX_CONFIG = [] // empty the array as new data is coming in | |
var buff = []; | |
for (var i = 0; i < data.byteLength; i++) { | |
var char = data.readU8() | |
if (char == 0x3B) { // ; (delimeter char) | |
obj.AUX_CONFIG.push(String.fromCharCode.apply(null, buff)) // convert bytes into ASCII and save as strings | |
// empty buffer | |
buff = [] | |
} else { | |
buff.push(char) | |
} | |
} | |
break | |
case MSPCodes.MSP_BOXIDS: | |
obj.AUX_CONFIG_IDS = [] // empty the array as new data is coming in | |
for (var i = 0; i < data.byteLength; i++) { | |
obj.AUX_CONFIG_IDS.push(data.readU8()) | |
} | |
break | |
case MSPCodes.MSP_PIDNAMES: | |
obj.PID_names = [] // empty the array as new data is coming in | |
var buff = [] | |
for (var i = 0; i < data.byteLength; i++) { | |
var char = data.readU8() | |
if (char == 0x3B) { // ; (delimeter char) | |
obj.PID_names.push(String.fromCharCode.apply(null, buff)) // convert bytes into ASCII and save as strings | |
// empty buffer | |
buff = [] | |
} else { | |
buff.push(char) | |
} | |
} | |
break | |
// got ack only | |
case MSPCodes.MSP_SET_RAW_RC: | |
break | |
case MSPCodes.MSP_SET_PID: | |
console.log('PID settings saved') | |
break | |
case MSPCodes.MSP_ACC_CALIBRATION: | |
console.log('Accel calibration executed') | |
break | |
case MSPCodes.MSP_MAG_CALIBRATION: | |
console.log('Mag calibration executed') | |
break | |
case MSPCodes.MSP_SET_RX_MAP: | |
console.log('RCMAP saved') | |
break | |
// skip | |
case MSPCodes.MSP_EXT_CMD: | |
case MSPCodes.MSP_ANALOG: | |
break | |
default: | |
console.log('code not found', code, data) | |
} | |
return obj | |
} | |
Buffer.prototype.__mspoffset = 0 | |
Buffer.prototype.readU32 = function (){ | |
var u32 = this.readUInt32LE(this.__mspoffset) | |
this.__mspoffset += 4 | |
return u32 | |
} | |
Buffer.prototype.read32 = function (){ | |
var i32 = this.readInt32LE(this.__mspoffset) | |
this.__mspoffset += 4 | |
return i32 | |
} | |
Buffer.prototype.readU16 = function (){ | |
var u16 = this.readUInt16LE(this.__mspoffset) | |
this.__mspoffset += 2 | |
return u16 | |
} | |
Buffer.prototype.read16 = function (){ | |
var i16 = this.readInt16LE(this.__mspoffset) | |
this.__mspoffset += 2 | |
return i16 | |
} | |
Buffer.prototype.readU8 = function (){ | |
var u8 = this.readUInt8(this.__mspoffset) | |
this.__mspoffset += 1 | |
return u8 | |
} | |
Buffer.prototype.read8 = function (){ | |
var i8 = this.readInt8(this.__mspoffset) | |
this.__mspoffset += 1 | |
return i8 | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment