diff --git a/_locales/en/messages.json b/_locales/en/messages.json
index f590c8d3..487d5d8f 100755
--- a/_locales/en/messages.json
+++ b/_locales/en/messages.json
@@ -514,6 +514,21 @@
"configurationBatteryMultiwiiCurrent": {
"message": "Enable support for legacy Multiwii MSP current output"
},
+ "configuration3d": {
+ "message": "3D"
+ },
+ "configuration3dDeadbandLow": {
+ "message": "3D Deadband Low"
+ },
+ "configuration3dDeadbandHigh": {
+ "message": "3D Deadband High"
+ },
+ "configuration3dNeutral": {
+ "message": "3D Neutral"
+ },
+ "configuration3dDeadbandThrottle": {
+ "message": "3D Deadband Throttle"
+ },
"configurationSystem": {
"message": "System configuration"
},
diff --git a/js/data_storage.js b/js/data_storage.js
index b5582bdf..2d75ed18 100755
--- a/js/data_storage.js
+++ b/js/data_storage.js
@@ -165,6 +165,13 @@ var MISC = {
vbatwarningcellvoltage: 0
};
+var _3D = {
+ deadband3d_low: 0,
+ deadband3d_high: 0,
+ neutral3d: 0,
+ deadband3d_throttle: 0
+};
+
var DATAFLASH = {
ready: false,
sectors: 0,
diff --git a/js/msp.js b/js/msp.js
index 1cdc18c0..cfde7dc0 100644
--- a/js/msp.js
+++ b/js/msp.js
@@ -52,6 +52,7 @@ var MSP_codes = {
MSP_WP: 118,
MSP_BOXIDS: 119,
MSP_SERVO_CONFIGURATIONS: 120,
+ MSP_3D: 124,
MSP_SET_RAW_RC: 200,
MSP_SET_RAW_GPS: 201,
@@ -67,6 +68,7 @@ var MSP_codes = {
MSP_SET_HEAD: 211,
MSP_SET_SERVO_CONFIGURATION: 212,
MSP_SET_MOTOR: 214,
+ MSP_SET_3D: 217,
// MSP_BIND: 240,
@@ -409,6 +411,16 @@ var MSP = {
MISC.vbatmaxcellvoltage = data.getUint8(offset++, 1) / 10; // 10-50
MISC.vbatwarningcellvoltage = data.getUint8(offset++, 1) / 10; // 10-50
break;
+ case MSP_codes.MSP_3D:
+ var offset = 0;
+ _3D.deadband3d_low = data.getUint16(offset, 1);
+ offset += 2;
+ _3D.deadband3d_high = data.getUint16(offset, 1);
+ offset += 2;
+ _3D.neutral3d = data.getUint16(offset, 1);
+ offset += 2;
+ _3D.deadband3d_throttle = data.getUint16(offset, 1);
+ break;
case MSP_codes.MSP_MOTOR_PINS:
console.log(data);
break;
@@ -1129,6 +1141,17 @@ MSP.crunch = function (code) {
}
}
break;
+
+ case MSP_codes.MSP_SET_3D:
+ buffer.push(lowByte(_3D.deadband3d_low));
+ buffer.push(highByte(_3D.deadband3d_low));
+ buffer.push(lowByte(_3D.deadband3d_high));
+ buffer.push(highByte(_3D.deadband3d_high));
+ buffer.push(lowByte(_3D.neutral3d));
+ buffer.push(highByte(_3D.neutral3d));
+ buffer.push(lowByte(_3D.deadband3d_throttle));
+ buffer.push(highByte(_3D.deadband3d_throttle));
+ break;
default:
return false;
diff --git a/tabs/configuration.html b/tabs/configuration.html
index e6760af3..d89f130e 100644
--- a/tabs/configuration.html
+++ b/tabs/configuration.html
@@ -393,6 +393,35 @@
+
diff --git a/tabs/configuration.js b/tabs/configuration.js
index f0cbd979..d6623288 100644
--- a/tabs/configuration.js
+++ b/tabs/configuration.js
@@ -27,7 +27,11 @@ TABS.configuration.initialize = function (callback, scrollPosition) {
}
function load_misc() {
- MSP.send_message(MSP_codes.MSP_MISC, false, false, load_acc_trim);
+ MSP.send_message(MSP_codes.MSP_MISC, false, false, load_3d);
+ }
+
+ function load_3d() {
+ MSP.send_message(MSP_codes.MSP_3D, false, false, load_acc_trim);
}
function load_acc_trim() {
@@ -313,6 +317,11 @@ TABS.configuration.initialize = function (callback, scrollPosition) {
$('input[name="currentoffset"]').val(BF_CONFIG.currentoffset);
$('input[name="multiwiicurrentoutput"]').prop('checked', MISC.multiwiicurrentoutput);
+ //fill 3D
+ $('input[name="3ddeadbandlow"]').val(_3D.deadband3d_low);
+ $('input[name="3ddeadbandhigh"]').val(_3D.deadband3d_high);
+ $('input[name="3dneutral"]').val(_3D.neutral3d);
+ $('input[name="3ddeadbandthrottle"]').val(_3D.deadband3d_throttle);
// UI hooks
$('input[name="looptime"]').change(function() {
@@ -389,6 +398,11 @@ TABS.configuration.initialize = function (callback, scrollPosition) {
BF_CONFIG.currentoffset = parseInt($('input[name="currentoffset"]').val());
MISC.multiwiicurrentoutput = ~~$('input[name="multiwiicurrentoutput"]').is(':checked'); // ~~ boolean to decimal conversion
+ _3D.deadband3d_low = parseInt($('input[name="3ddeadbandlow"]').val());
+ _3D.deadband3d_high = parseInt($('input[name="3ddeadbandhigh"]').val());
+ _3D.neutral3d = parseInt($('input[name="3dneutral"]').val());
+ _3D.deadband3d_throttle = ($('input[name="3ddeadbandthrottle"]').val());
+
function save_serial_config() {
if (semver.lt(CONFIG.apiVersion, "1.6.0")) {
MSP.send_message(MSP_codes.MSP_SET_CF_SERIAL_CONFIG, MSP.crunch(MSP_codes.MSP_SET_CF_SERIAL_CONFIG), false, save_misc);
@@ -398,7 +412,11 @@ TABS.configuration.initialize = function (callback, scrollPosition) {
}
function save_misc() {
- MSP.send_message(MSP_codes.MSP_SET_MISC, MSP.crunch(MSP_codes.MSP_SET_MISC), false, save_acc_trim);
+ MSP.send_message(MSP_codes.MSP_SET_MISC, MSP.crunch(MSP_codes.MSP_SET_MISC), false, save_3d);
+ }
+
+ function save_3d() {
+ MSP.send_message(MSP_codes.MSP_SET_3D, MSP.crunch(MSP_codes.MSP_SET_3D), false, save_acc_trim);
}
function save_acc_trim() {
diff --git a/tabs/motors.js b/tabs/motors.js
index 74c99079..b3ba6d7b 100644
--- a/tabs/motors.js
+++ b/tabs/motors.js
@@ -12,7 +12,15 @@ TABS.motors.initialize = function (callback) {
}
function get_arm_status() {
- MSP.send_message(MSP_codes.MSP_STATUS, false, false, get_motor_data);
+ MSP.send_message(MSP_codes.MSP_STATUS, false, false, load_config);
+ }
+
+ function load_config() {
+ MSP.send_message(MSP_codes.MSP_BF_CONFIG, false, false, load_3d);
+ }
+
+ function load_3d() {
+ MSP.send_message(MSP_codes.MSP_3D, false, false, get_motor_data);
}
function update_arm_status() {
@@ -287,8 +295,13 @@ TABS.motors.initialize = function (callback) {
$('div.sliders input').prop('min', MISC.mincommand);
$('div.sliders input').prop('max', MISC.maxthrottle);
- $('div.sliders input').val(MISC.mincommand);
$('div.values li:not(:last)').text(MISC.mincommand);
+
+ if(bit_check(BF_CONFIG.features,12)){
+ $('div.sliders input').val(_3D.neutral3d);
+ }else{
+ $('div.sliders input').val(MISC.mincommand);
+ }
// UI hooks
var buffering_set_motor = [],
@@ -341,7 +354,11 @@ TABS.motors.initialize = function (callback) {
$('div.sliders input').prop('disabled', true);
// change all values to default
- $('div.sliders input').val(MISC.mincommand);
+ if (! bit_check(BF_CONFIG.features,12)) {
+ $('div.sliders input').val(MISC.mincommand);
+ } else {
+ $('div.sliders input').val(_3D.neutral3d);
+ }
// trigger change event so values are sent to mcu
$('div.sliders input').trigger('input');
@@ -351,11 +368,18 @@ TABS.motors.initialize = function (callback) {
// check if motors are already spinning
var motors_running = false;
- for (var i = 0; i < MOTOR_DATA.length; i++) {
- if (MOTOR_DATA[i] > MISC.mincommand) {
- motors_running = true;
- break;
- }
+ for (var i = 0; i < number_of_valid_outputs; i++) {
+ if( ! bit_check(BF_CONFIG.features,12) ){
+ if (MOTOR_DATA[i] > MISC.mincommand) {
+ motors_running = true;
+ break;
+ }
+ }else{
+ if( (MOTOR_DATA[i] < _3D.deadband3d_low) || (MOTOR_DATA[i] > _3D.deadband3d_high) ){
+ motors_running = true;
+ break;
+ }
+ }
}
if (motors_running) {