diff --git a/js/msp.js b/js/msp.js index b18730ce..baace5e6 100644 --- a/js/msp.js +++ b/js/msp.js @@ -73,6 +73,475 @@ var MSP = { callbacks: [], packet_error: 0, + read: function (readInfo) { + var data = new Uint8Array(readInfo.data); + + for (var i = 0; i < data.length; i++) { + switch (this.state) { + case 0: // sync char 1 + if (data[i] == 36) { // $ + this.state++; + } + break; + case 1: // sync char 2 + if (data[i] == 77) { // M + this.state++; + } else { // restart and try again + this.state = 0; + } + break; + case 2: // direction (should be >) + if (data[i] == 62) { // > + this.message_direction = 1; + } else { // < + this.message_direction = 0; + } + + this.state++; + break; + case 3: + this.message_length_expected = data[i]; + + this.message_checksum = data[i]; + + // setup arraybuffer + this.message_buffer = new ArrayBuffer(this.message_length_expected); + this.message_buffer_uint8_view = new Uint8Array(this.message_buffer); + + this.state++; + break; + case 4: + this.code = data[i]; + this.message_checksum ^= data[i]; + + if (this.message_length_expected > 0) { + // process payload + this.state++; + } else { + // no payload + this.state += 2; + } + break; + case 5: // payload + this.message_buffer_uint8_view[this.message_length_received] = data[i]; + this.message_checksum ^= data[i]; + this.message_length_received++; + + if (this.message_length_received >= this.message_length_expected) { + this.state++; + } + break; + case 6: + if (this.message_checksum == data[i]) { + // message received, process + this.process_data(this.code, this.message_buffer, this.message_length_expected); + } else { + console.log('code: ' + this.code + ' - crc failed'); + + this.packet_error++; + $('span.packet-error').html(this.packet_error); + } + + // Reset variables + this.message_length_received = 0; + this.state = 0; + break; + } + } + }, + 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) { + case MSP_codes.MSP_IDENT: + CONFIG.version = parseFloat((data.getUint8(0) / 100).toFixed(2)); + CONFIG.multiType = data.getUint8(1); + CONFIG.msp_version = data.getUint8(2); + CONFIG.capability = data.getUint32(3, 1); + break; + case MSP_codes.MSP_STATUS: + CONFIG.cycleTime = data.getUint16(0, 1); + CONFIG.i2cError = data.getUint16(2, 1); + CONFIG.activeSensors = data.getUint16(4, 1); + CONFIG.mode = data.getUint32(6, 1); + CONFIG.profile = data.getUint8(10); + + sensor_status(CONFIG.activeSensors); + $('span.i2c-error').text(CONFIG.i2cError); + $('span.cycle-time').text(CONFIG.cycleTime); + break; + case MSP_codes.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 + SENSOR_DATA.accelerometer[0] = data.getInt16(0, 1) / 512; + SENSOR_DATA.accelerometer[1] = data.getInt16(2, 1) / 512; + SENSOR_DATA.accelerometer[2] = data.getInt16(4, 1) / 512; + + // properly scaled + SENSOR_DATA.gyroscope[0] = data.getInt16(6, 1) * (4 / 16.4); + SENSOR_DATA.gyroscope[1] = data.getInt16(8, 1) * (4 / 16.4); + SENSOR_DATA.gyroscope[2] = data.getInt16(10, 1) * (4 / 16.4); + + // no clue about scaling factor + SENSOR_DATA.magnetometer[0] = data.getInt16(12, 1) / 1090; + SENSOR_DATA.magnetometer[1] = data.getInt16(14, 1) / 1090; + SENSOR_DATA.magnetometer[2] = data.getInt16(16, 1) / 1090; + break; + case MSP_codes.MSP_SERVO: + var needle = 0; + for (var i = 0; i < 8; i++) { + SERVO_DATA[i] = data.getUint16(needle, 1); + + needle += 2; + } + break; + case MSP_codes.MSP_MOTOR: + var needle = 0; + for (var i = 0; i < 8; i++) { + MOTOR_DATA[i] = data.getUint16(needle, 1); + + needle += 2; + } + break; + case MSP_codes.MSP_RC: + RC.active_channels = message_length / 2; + + for (var i = 0; i < RC.active_channels; i++) { + RC.channels[i] = data.getUint16((i * 2), 1); + } + break; + case MSP_codes.MSP_RAW_GPS: + GPS_DATA.fix = data.getUint8(0); + GPS_DATA.numSat = data.getUint8(1); + GPS_DATA.lat = data.getInt32(2, 1); + GPS_DATA.lon = data.getInt32(6, 1); + GPS_DATA.alt = data.getUint16(10, 1); + GPS_DATA.speed = data.getUint16(12, 1); + GPS_DATA.ground_course = data.getUint16(14, 1); + break; + case MSP_codes.MSP_COMP_GPS: + GPS_DATA.distanceToHome = data.getUint16(0, 1); + GPS_DATA.directionToHome = data.getUint16(2, 1); + GPS_DATA.update = data.getUint8(4); + break; + case MSP_codes.MSP_ATTITUDE: + SENSOR_DATA.kinematics[0] = data.getInt16(0, 1) / 10.0; // x + SENSOR_DATA.kinematics[1] = data.getInt16(2, 1) / 10.0; // y + SENSOR_DATA.kinematics[2] = data.getInt16(4, 1); // z + break; + case MSP_codes.MSP_ALTITUDE: + SENSOR_DATA.altitude = parseFloat((data.getInt32(0, 1) / 100.0).toFixed(2)); // correct scale factor + break; + case MSP_codes.MSP_ANALOG: + ANALOG.voltage = data.getUint8(0) / 10.0; + ANALOG.mAhdrawn = data.getUint16(1, 1); + ANALOG.rssi = data.getUint16(3, 1); // 0-1023 + ANALOG.amperage = data.getUint16(5, 1) / 100; // A + break; + case MSP_codes.MSP_RC_TUNING: + RC_tuning.RC_RATE = parseFloat((data.getUint8(0) / 100).toFixed(2)); + RC_tuning.RC_EXPO = parseFloat((data.getUint8(1) / 100).toFixed(2)); + RC_tuning.roll_pitch_rate = parseFloat((data.getUint8(2) / 100).toFixed(2)); + RC_tuning.yaw_rate = parseFloat((data.getUint8(3) / 100).toFixed(2)); + RC_tuning.dynamic_THR_PID = parseFloat((data.getUint8(4) / 100).toFixed(2)); + RC_tuning.throttle_MID = parseFloat((data.getUint8(5) / 100).toFixed(2)); + RC_tuning.throttle_EXPO = parseFloat((data.getUint8(6) / 100).toFixed(2)); + break; + case MSP_codes.MSP_PID: + // PID data arrived, we need to scale it and save to appropriate bank / array + for (var i = 0, needle = 0; i < (message_length / 3); i++, needle += 3) { + // main for loop selecting the pid section + switch (i) { + case 0: + case 1: + case 2: + case 3: + case 7: + case 8: + case 9: + PIDs[i][0] = data.getUint8(needle) / 10; + PIDs[i][1] = data.getUint8(needle + 1) / 1000; + PIDs[i][2] = data.getUint8(needle + 2); + break; + case 4: + PIDs[i][0] = data.getUint8(needle) / 100; + PIDs[i][1] = data.getUint8(needle + 1) / 100; + PIDs[i][2] = data.getUint8(needle + 2) / 1000; + break; + case 5: + case 6: + PIDs[i][0] = data.getUint8(needle) / 10; + PIDs[i][1] = data.getUint8(needle + 1) / 100; + PIDs[i][2] = data.getUint8(needle + 2) / 1000; + break; + } + } + break; + case MSP_codes.MSP_BOX: + AUX_CONFIG_values = []; // empty the array as new data is coming in + + // fill in current data + for (var i = 0; i < data.byteLength; i += 2) { // + 2 because uint16_t = 2 bytes + AUX_CONFIG_values.push(data.getUint16(i, 1)); + } + break; + case MSP_codes.MSP_MISC: // 22 bytes + 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.getUint8(12); + MISC.placeholder1 = data.getUint8(13); + MISC.placeholder2 = data.getUint16(14, 1); + MISC.mag_declination = data.getInt16(16, 1); // -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.placeholder3 = data.getUint8(21, 1); + break; + case MSP_codes.MSP_MOTOR_PINS: + console.log(data); + break; + case MSP_codes.MSP_BOXNAMES: + AUX_CONFIG = []; // empty the array as new data is coming in + + var buff = []; + for (var i = 0; i < data.byteLength; i++) { + if (data.getUint8(i) == 0x3B) { // ; (delimeter char) + AUX_CONFIG.push(String.fromCharCode.apply(null, buff)); // convert bytes into ASCII and save as strings + + // empty buffer + buff = []; + } else { + buff.push(data.getUint8(i)); + } + } + break; + case MSP_codes.MSP_PIDNAMES: + PID_names = []; // empty the array as new data is coming in + + var buff = []; + for (var i = 0; i < data.byteLength; i++) { + if (data.getUint8(i) == 0x3B) { // ; (delimeter char) + PID_names.push(String.fromCharCode.apply(null, buff)); // convert bytes into ASCII and save as strings + + // empty buffer + buff = []; + } else { + buff.push(data.getUint8(i)); + } + } + break; + case MSP_codes.MSP_WP: + console.log(data); + break; + case MSP_codes.MSP_BOXIDS: + AUX_CONFIG_IDS = []; // empty the array as new data is coming in + + for (var i = 0; i < data.byteLength; i++) { + AUX_CONFIG_IDS.push(data.getUint8(i)); + } + break; + case MSP_codes.MSP_SERVO_CONF: + SERVO_CONFIG = []; // empty the array as new data is coming in + + for (var i = 0; i < 56; 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); + } + break; + case MSP_codes.MSP_SET_RAW_RC: + break; + case MSP_codes.MSP_SET_RAW_GPS: + break; + case MSP_codes.MSP_SET_PID: + console.log('PID settings saved'); + break; + case MSP_codes.MSP_SET_BOX: + console.log('AUX Configuration saved'); + break; + case MSP_codes.MSP_SET_RC_TUNING: + console.log('RC Tuning saved'); + break; + case MSP_codes.MSP_ACC_CALIBRATION: + console.log('Accel calibration executed'); + break; + case MSP_codes.MSP_MAG_CALIBRATION: + console.log('Mag calibration executed'); + break; + case MSP_codes.MSP_SET_MISC: + console.log('MISC Configuration saved'); + break; + case MSP_codes.MSP_RESET_CONF: + console.log('Settings Reset'); + break; + case MSP_codes.MSP_SELECT_SETTING: + console.log('Profile selected'); + break; + case MSP_codes.MSP_SET_SERVO_CONF: + console.log('Servo Configuration saved'); + break; + case MSP_codes.MSP_EEPROM_WRITE: + console.log('Settings Saved in EEPROM'); + break; + case MSP_codes.MSP_DEBUGMSG: + break; + case MSP_codes.MSP_DEBUG: + for (var i = 0; i < 4; i++) + SENSOR_DATA.debug[i] = data.getInt16((2 * i), 1); + break; + case MSP_codes.MSP_SET_MOTOR: + console.log('Motor Speeds Updated'); + break; + // Additional baseflight commands that are not compatible with MultiWii + case MSP_codes.MSP_UID: + CONFIG.uid[0] = data.getUint32(0, 1); + CONFIG.uid[1] = data.getUint32(4, 1); + CONFIG.uid[2] = data.getUint32(8, 1); + break; + case MSP_codes.MSP_ACC_TRIM: + CONFIG.accelerometerTrims[0] = data.getInt16(0, 1); // pitch + CONFIG.accelerometerTrims[1] = data.getInt16(2, 1); // roll + break; + case MSP_codes.MSP_SET_ACC_TRIM: + console.log('Accelerometer trimms saved.'); + break; + case MSP_codes.MSP_GPSSVINFO: + if (data.byteLength > 0) { + var numCh = data.getUint8(0); + + var needle = 1; + for (var i = 0; i < numCh; i++) { + GPS_DATA.chn[i] = data.getUint8(needle); + GPS_DATA.svid[i] = data.getUint8(needle + 1); + GPS_DATA.quality[i] = data.getUint8(needle + 2); + GPS_DATA.cno[i] = data.getUint8(needle + 3); + + needle += 4; + } + } + break; + // Additional private MSP for baseflight configurator + case MSP_codes.MSP_RCMAP: + RC_MAP = []; // empty the array as new data is coming in + + for (var i = 0; i < data.byteLength; i++) { + RC_MAP.push(data.getUint8(i)); + } + break; + case MSP_codes.MSP_SET_RCMAP: + console.log('RCMAP Updated'); + break; + case MSP_codes.MSP_CONFIG: + BF_CONFIG.mixerConfiguration = data.getUint8(0); + BF_CONFIG.features = data.getUint32(1, 1); + BF_CONFIG.serialrx_type = data.getUint8(5); + BF_CONFIG.board_align_roll = data.getInt16(6, 1); + BF_CONFIG.board_align_pitch = data.getInt16(8, 1); + BF_CONFIG.board_align_yaw = data.getInt16(10, 1); + BF_CONFIG.currentscale = data.getUint16(12, 1); + BF_CONFIG.currentoffset = data.getUint16(14, 1); + break; + case MSP_codes.MSP_SET_CONFIG: + break; + case MSP_codes.MSP_SET_REBOOT: + console.log('Reboot request accepted'); + break; + + default: + console.log('Unknown code detected: ' + code); + } + + // trigger callbacks, cleanup/remove callback after trigger + for (var i = this.callbacks.length - 1; i >= 0; i--) { // itterating in reverse because we use .splice which modifies array length + if (this.callbacks[i].code == code) { + // save callback reference + var callback = this.callbacks[i].callback; + + // remove timeout + clearInterval(this.callbacks[i].timer); + + // remove object from array + this.callbacks.splice(i, 1); + + // fire callback + if (callback) callback({'command': code, 'data': data, 'length': message_length}); + } + } + }, + send_message: function (code, data, callback_sent, callback_msp) { + var bufferOut, + bufView; + + // 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] = 60; // < + 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] = 60; // < + bufView[3] = 0; // data length + bufView[4] = code; // code + bufView[5] = bufView[3] ^ bufView[4]; // checksum + } + + // utilizing callback/timeout system for all commands + for (var i = 0; i < MSP.callbacks.length; i++) { + if (MSP.callbacks[i].code == code) { + // request already exist + return false; // skips the code below + } + } + + var obj = {'code': code, 'callback': (callback_msp) ? callback_msp : false}; + obj.timer = setInterval(function() { + console.log('MSP data request timed-out: ' + code); + + serial.send(bufferOut, function(sendInfo) {}); + }, 1000); // we should be able to define timeout in the future + + MSP.callbacks.push(obj); + + serial.send(bufferOut, function(sendInfo) { + if (sendInfo.bytesSent > 0) { + if (callback_sent) callback_sent(); + } + }); + + return true; + }, callbacks_cleanup: function () { for (var i = 0; i < this.callbacks.length; i++) { clearInterval(this.callbacks[i].timer); @@ -80,7 +549,6 @@ var MSP = { this.callbacks = []; }, - disconnect_cleanup: function () { this.state = 0; // reset packet state for "clean" initial entry (this is only required if user hot-disconnects) this.packet_error = 0; // reset CRC packet error counter for next session @@ -89,476 +557,6 @@ var MSP = { } }; -MSP.read = function (readInfo) { - var data = new Uint8Array(readInfo.data); - - for (var i = 0; i < data.length; i++) { - switch (this.state) { - case 0: // sync char 1 - if (data[i] == 36) { // $ - this.state++; - } - break; - case 1: // sync char 2 - if (data[i] == 77) { // M - this.state++; - } else { // restart and try again - this.state = 0; - } - break; - case 2: // direction (should be >) - if (data[i] == 62) { // > - this.message_direction = 1; - } else { // < - this.message_direction = 0; - } - - this.state++; - break; - case 3: - this.message_length_expected = data[i]; - - this.message_checksum = data[i]; - - // setup arraybuffer - this.message_buffer = new ArrayBuffer(this.message_length_expected); - this.message_buffer_uint8_view = new Uint8Array(this.message_buffer); - - this.state++; - break; - case 4: - this.code = data[i]; - this.message_checksum ^= data[i]; - - if (this.message_length_expected != 0) { // standard message - this.state++; - } else { // MSP_ACC_CALIBRATION, etc... - this.state += 2; - } - break; - case 5: // payload - this.message_buffer_uint8_view[this.message_length_received] = data[i]; - this.message_checksum ^= data[i]; - this.message_length_received++; - - if (this.message_length_received >= this.message_length_expected) { - this.state++; - } - break; - case 6: - if (this.message_checksum == data[i]) { - // message received, process - this.process_data(this.code, this.message_buffer, this.message_length_expected); - } else { - console.log('code: ' + this.code + ' - crc failed'); - - this.packet_error++; - $('span.packet-error').html(this.packet_error); - } - - // Reset variables - this.message_length_received = 0; - this.state = 0; - break; - } - } -}; - -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) { - case MSP_codes.MSP_IDENT: - CONFIG.version = parseFloat((data.getUint8(0) / 100).toFixed(2)); - CONFIG.multiType = data.getUint8(1); - CONFIG.msp_version = data.getUint8(2); - CONFIG.capability = data.getUint32(3, 1); - break; - case MSP_codes.MSP_STATUS: - CONFIG.cycleTime = data.getUint16(0, 1); - CONFIG.i2cError = data.getUint16(2, 1); - CONFIG.activeSensors = data.getUint16(4, 1); - CONFIG.mode = data.getUint32(6, 1); - CONFIG.profile = data.getUint8(10); - - sensor_status(CONFIG.activeSensors); - $('span.i2c-error').text(CONFIG.i2cError); - $('span.cycle-time').text(CONFIG.cycleTime); - break; - case MSP_codes.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 - SENSOR_DATA.accelerometer[0] = data.getInt16(0, 1) / 512; - SENSOR_DATA.accelerometer[1] = data.getInt16(2, 1) / 512; - SENSOR_DATA.accelerometer[2] = data.getInt16(4, 1) / 512; - - // properly scaled - SENSOR_DATA.gyroscope[0] = data.getInt16(6, 1) * (4 / 16.4); - SENSOR_DATA.gyroscope[1] = data.getInt16(8, 1) * (4 / 16.4); - SENSOR_DATA.gyroscope[2] = data.getInt16(10, 1) * (4 / 16.4); - - // no clue about scaling factor - SENSOR_DATA.magnetometer[0] = data.getInt16(12, 1) / 1090; - SENSOR_DATA.magnetometer[1] = data.getInt16(14, 1) / 1090; - SENSOR_DATA.magnetometer[2] = data.getInt16(16, 1) / 1090; - break; - case MSP_codes.MSP_SERVO: - var needle = 0; - for (var i = 0; i < 8; i++) { - SERVO_DATA[i] = data.getUint16(needle, 1); - - needle += 2; - } - break; - case MSP_codes.MSP_MOTOR: - var needle = 0; - for (var i = 0; i < 8; i++) { - MOTOR_DATA[i] = data.getUint16(needle, 1); - - needle += 2; - } - break; - case MSP_codes.MSP_RC: - RC.active_channels = message_length / 2; - - for (var i = 0; i < RC.active_channels; i++) { - RC.channels[i] = data.getUint16((i * 2), 1); - } - break; - case MSP_codes.MSP_RAW_GPS: - GPS_DATA.fix = data.getUint8(0); - GPS_DATA.numSat = data.getUint8(1); - GPS_DATA.lat = data.getInt32(2, 1); - GPS_DATA.lon = data.getInt32(6, 1); - GPS_DATA.alt = data.getUint16(10, 1); - GPS_DATA.speed = data.getUint16(12, 1); - GPS_DATA.ground_course = data.getUint16(14, 1); - break; - case MSP_codes.MSP_COMP_GPS: - GPS_DATA.distanceToHome = data.getUint16(0, 1); - GPS_DATA.directionToHome = data.getUint16(2, 1); - GPS_DATA.update = data.getUint8(4); - break; - case MSP_codes.MSP_ATTITUDE: - SENSOR_DATA.kinematics[0] = data.getInt16(0, 1) / 10.0; // x - SENSOR_DATA.kinematics[1] = data.getInt16(2, 1) / 10.0; // y - SENSOR_DATA.kinematics[2] = data.getInt16(4, 1); // z - break; - case MSP_codes.MSP_ALTITUDE: - SENSOR_DATA.altitude = parseFloat((data.getInt32(0, 1) / 100.0).toFixed(2)); // correct scale factor - break; - case MSP_codes.MSP_ANALOG: - ANALOG.voltage = data.getUint8(0) / 10.0; - ANALOG.mAhdrawn = data.getUint16(1, 1); - ANALOG.rssi = data.getUint16(3, 1); // 0-1023 - ANALOG.amperage = data.getUint16(5, 1) / 100; // A - break; - case MSP_codes.MSP_RC_TUNING: - RC_tuning.RC_RATE = parseFloat((data.getUint8(0) / 100).toFixed(2)); - RC_tuning.RC_EXPO = parseFloat((data.getUint8(1) / 100).toFixed(2)); - RC_tuning.roll_pitch_rate = parseFloat((data.getUint8(2) / 100).toFixed(2)); - RC_tuning.yaw_rate = parseFloat((data.getUint8(3) / 100).toFixed(2)); - RC_tuning.dynamic_THR_PID = parseFloat((data.getUint8(4) / 100).toFixed(2)); - RC_tuning.throttle_MID = parseFloat((data.getUint8(5) / 100).toFixed(2)); - RC_tuning.throttle_EXPO = parseFloat((data.getUint8(6) / 100).toFixed(2)); - break; - case MSP_codes.MSP_PID: - // PID data arrived, we need to scale it and save to appropriate bank / array - for (var i = 0, needle = 0; i < (message_length / 3); i++, needle += 3) { - // main for loop selecting the pid section - switch (i) { - case 0: - case 1: - case 2: - case 3: - case 7: - case 8: - case 9: - PIDs[i][0] = data.getUint8(needle) / 10; - PIDs[i][1] = data.getUint8(needle + 1) / 1000; - PIDs[i][2] = data.getUint8(needle + 2); - break; - case 4: - PIDs[i][0] = data.getUint8(needle) / 100; - PIDs[i][1] = data.getUint8(needle + 1) / 100; - PIDs[i][2] = data.getUint8(needle + 2) / 1000; - break; - case 5: - case 6: - PIDs[i][0] = data.getUint8(needle) / 10; - PIDs[i][1] = data.getUint8(needle + 1) / 100; - PIDs[i][2] = data.getUint8(needle + 2) / 1000; - break; - } - } - break; - case MSP_codes.MSP_BOX: - AUX_CONFIG_values = []; // empty the array as new data is coming in - - // fill in current data - for (var i = 0; i < data.byteLength; i += 2) { // + 2 because uint16_t = 2 bytes - AUX_CONFIG_values.push(data.getUint16(i, 1)); - } - break; - case MSP_codes.MSP_MISC: // 22 bytes - 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.getUint8(12); - MISC.placeholder1 = data.getUint8(13); - MISC.placeholder2 = data.getUint16(14, 1); - MISC.mag_declination = data.getInt16(16, 1); // -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.placeholder3 = data.getUint8(21, 1); - break; - case MSP_codes.MSP_MOTOR_PINS: - console.log(data); - break; - case MSP_codes.MSP_BOXNAMES: - AUX_CONFIG = []; // empty the array as new data is coming in - - var buff = []; - for (var i = 0; i < data.byteLength; i++) { - if (data.getUint8(i) == 0x3B) { // ; (delimeter char) - AUX_CONFIG.push(String.fromCharCode.apply(null, buff)); // convert bytes into ASCII and save as strings - - // empty buffer - buff = []; - } else { - buff.push(data.getUint8(i)); - } - } - break; - case MSP_codes.MSP_PIDNAMES: - PID_names = []; // empty the array as new data is coming in - - var buff = []; - for (var i = 0; i < data.byteLength; i++) { - if (data.getUint8(i) == 0x3B) { // ; (delimeter char) - PID_names.push(String.fromCharCode.apply(null, buff)); // convert bytes into ASCII and save as strings - - // empty buffer - buff = []; - } else { - buff.push(data.getUint8(i)); - } - } - break; - case MSP_codes.MSP_WP: - console.log(data); - break; - case MSP_codes.MSP_BOXIDS: - AUX_CONFIG_IDS = []; // empty the array as new data is coming in - - for (var i = 0; i < data.byteLength; i++) { - AUX_CONFIG_IDS.push(data.getUint8(i)); - } - break; - case MSP_codes.MSP_SERVO_CONF: - SERVO_CONFIG = []; // empty the array as new data is coming in - - for (var i = 0; i < 56; 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); - } - break; - case MSP_codes.MSP_SET_RAW_RC: - break; - case MSP_codes.MSP_SET_RAW_GPS: - break; - case MSP_codes.MSP_SET_PID: - console.log('PID settings saved'); - break; - case MSP_codes.MSP_SET_BOX: - console.log('AUX Configuration saved'); - break; - case MSP_codes.MSP_SET_RC_TUNING: - console.log('RC Tuning saved'); - break; - case MSP_codes.MSP_ACC_CALIBRATION: - console.log('Accel calibration executed'); - break; - case MSP_codes.MSP_MAG_CALIBRATION: - console.log('Mag calibration executed'); - break; - case MSP_codes.MSP_SET_MISC: - console.log('MISC Configuration saved'); - break; - case MSP_codes.MSP_RESET_CONF: - console.log('Settings Reset'); - break; - case MSP_codes.MSP_SELECT_SETTING: - console.log('Profile selected'); - break; - case MSP_codes.MSP_SET_SERVO_CONF: - console.log('Servo Configuration saved'); - break; - case MSP_codes.MSP_EEPROM_WRITE: - console.log('Settings Saved in EEPROM'); - break; - case MSP_codes.MSP_DEBUGMSG: - break; - case MSP_codes.MSP_DEBUG: - for (var i = 0; i < 4; i++) - SENSOR_DATA.debug[i] = data.getInt16((2 * i), 1); - break; - case MSP_codes.MSP_SET_MOTOR: - console.log('Motor Speeds Updated'); - break; - // Additional baseflight commands that are not compatible with MultiWii - case MSP_codes.MSP_UID: - CONFIG.uid[0] = data.getUint32(0, 1); - CONFIG.uid[1] = data.getUint32(4, 1); - CONFIG.uid[2] = data.getUint32(8, 1); - break; - case MSP_codes.MSP_ACC_TRIM: - CONFIG.accelerometerTrims[0] = data.getInt16(0, 1); // pitch - CONFIG.accelerometerTrims[1] = data.getInt16(2, 1); // roll - break; - case MSP_codes.MSP_SET_ACC_TRIM: - console.log('Accelerometer trimms saved.'); - break; - case MSP_codes.MSP_GPSSVINFO: - if (data.byteLength > 0) { - var numCh = data.getUint8(0); - - var needle = 1; - for (var i = 0; i < numCh; i++) { - GPS_DATA.chn[i] = data.getUint8(needle); - GPS_DATA.svid[i] = data.getUint8(needle + 1); - GPS_DATA.quality[i] = data.getUint8(needle + 2); - GPS_DATA.cno[i] = data.getUint8(needle + 3); - - needle += 4; - } - } - break; - // Additional private MSP for baseflight configurator - case MSP_codes.MSP_RCMAP: - RC_MAP = []; // empty the array as new data is coming in - - for (var i = 0; i < data.byteLength; i++) { - RC_MAP.push(data.getUint8(i)); - } - break; - case MSP_codes.MSP_SET_RCMAP: - console.log('RCMAP Updated'); - break; - case MSP_codes.MSP_CONFIG: - BF_CONFIG.mixerConfiguration = data.getUint8(0); - BF_CONFIG.features = data.getUint32(1, 1); - BF_CONFIG.serialrx_type = data.getUint8(5); - BF_CONFIG.board_align_roll = data.getInt16(6, 1); - BF_CONFIG.board_align_pitch = data.getInt16(8, 1); - BF_CONFIG.board_align_yaw = data.getInt16(10, 1); - BF_CONFIG.currentscale = data.getUint16(12, 1); - BF_CONFIG.currentoffset = data.getUint16(14, 1); - break; - case MSP_codes.MSP_SET_CONFIG: - break; - case MSP_codes.MSP_SET_REBOOT: - console.log('Reboot request accepted'); - break; - - default: - console.log('Unknown code detected: ' + code); - } - - // trigger callbacks, cleanup/remove callback after trigger - for (var i = this.callbacks.length - 1; i >= 0; i--) { // itterating in reverse because we use .splice which modifies array length - if (this.callbacks[i].code == code) { - // save callback reference - var callback = this.callbacks[i].callback; - - // remove timeout - clearInterval(this.callbacks[i].timer); - - // remove object from array - this.callbacks.splice(i, 1); - - // fire callback - if (callback) callback({'command': code, 'data': data, 'length': message_length}); - } - } -}; - -MSP.send_message = function(code, data, callback_sent, callback_msp) { - var bufferOut; - var bufView; - - // always reserve 6 bytes for protocol overhead ! - if (data) { - var size = data.length + 6; - var checksum = 0; - - bufferOut = new ArrayBuffer(size); - bufView = new Uint8Array(bufferOut); - - bufView[0] = 36; // $ - bufView[1] = 77; // M - bufView[2] = 60; // < - 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] = 60; // < - bufView[3] = 0; // data length - bufView[4] = code; // code - bufView[5] = bufView[3] ^ bufView[4]; // checksum - } - - // utilizing callback/timeout system for all commands - for (var i = 0; i < MSP.callbacks.length; i++) { - if (MSP.callbacks[i].code == code) { - // request already exist - return false; // skips the code below - } - } - - var obj = {'code': code, 'callback': (callback_msp) ? callback_msp : false}; - obj.timer = setInterval(function() { - console.log('MSP data request timed-out: ' + code); - - serial.send(bufferOut, function(sendInfo) {}); - }, 1000); // we should be able to define timeout in the future - - MSP.callbacks.push(obj); - - serial.send(bufferOut, function(sendInfo) { - if (sendInfo.bytesSent > 0) { - if (callback_sent) callback_sent(); - } - }); - - return true; -}; - MSP.crunch = function (code) { var buffer = [];