-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
diff --git a/tabs/magnetometer.js b/tabs/magnetometer.js
index ec68ed30..77e3391c 100644
--- a/tabs/magnetometer.js
+++ b/tabs/magnetometer.js
@@ -1,5 +1,5 @@
'use strict';
-/*global chrome,GUI,BOARD_ALIGNMENT,TABS,nwdialog,$*/
+/*global chrome,GUI,BOARD_ALIGNMENT,TABS,nwdialog,helper,$*/
TABS.magnetometer = {};
@@ -18,6 +18,12 @@ TABS.magnetometer.initialize = function (callback) {
yaw: 0
};
+ self.boardAlignmentConfig = {
+ pitch: 0,
+ roll: 0,
+ yaw: 0
+ };
+
self.pageElements = {};
self.isSavePreset = true;
self.showMagnetometer = true;
@@ -29,6 +35,12 @@ TABS.magnetometer.initialize = function (callback) {
var loadChain = [
mspHelper.loadMixerConfig,
mspHelper.loadBoardAlignment,
+ function (callback) {
+ self.boardAlignmentConfig.pitch = Math.round(BOARD_ALIGNMENT.pitch / 10);
+ self.boardAlignmentConfig.roll = Math.round(BOARD_ALIGNMENT.roll / 10);
+ self.boardAlignmentConfig.yaw = Math.round(BOARD_ALIGNMENT.yaw / 10);
+ callback();
+ },
mspHelper.loadSensorAlignment,
// Pitch and roll must be inverted
function (callback) {
@@ -38,7 +50,7 @@ TABS.magnetometer.initialize = function (callback) {
},
function (callback) {
mspHelper.getSetting("align_mag_pitch").then(function (data) {
- self.alignmentConfig.pitch = (parseInt(data.value, 10) / 10) - 180;
+ self.alignmentConfig.pitch = parseInt(data.value, 10) / 10;
}).then(callback)
},
function (callback) {
@@ -52,12 +64,27 @@ TABS.magnetometer.initialize = function (callback) {
loadChainer.setExitPoint(load_html);
loadChainer.execute();
+ function areAnglesZero() {
+ return self.alignmentConfig.pitch === 0 && self.alignmentConfig.roll === 0 && self.alignmentConfig.yaw === 0
+ }
+
+ function isBoardAlignmentZero() {
+ return (self.boardAlignmentConfig.pitch == 0 ) && (self.boardAlignmentConfig.roll == 0 ) && (self.boardAlignmentConfig.yaw == 0);
+ }
+
//========================
// Save chain
// =======================
var saveChainer = new MSPChainerClass();
var saveChain = [
+ function (callback) {
+ BOARD_ALIGNMENT.pitch = self.boardAlignmentConfig.pitch * 10;
+ BOARD_ALIGNMENT.roll = self.boardAlignmentConfig.roll * 10;
+ BOARD_ALIGNMENT.yaw = self.boardAlignmentConfig.yaw * 10;
+ callback();
+ },
+ mspHelper.saveBoardAlignment,
// Magnetometer alignment
function (callback) {
let orientation_mag_e = $('select.magalign');
@@ -66,7 +93,7 @@ TABS.magnetometer.initialize = function (callback) {
},
mspHelper.saveSensorAlignment,
// Pitch/Roll/Yaw
- // Pitch and roll must be inverted
+ // Pitch and roll must be inverted - ???
function (callback) {
if (self.isSavePreset)
mspHelper.setSetting("align_mag_roll", 0, callback);
@@ -77,14 +104,19 @@ TABS.magnetometer.initialize = function (callback) {
if (self.isSavePreset)
mspHelper.setSetting("align_mag_pitch", 0, callback);
else
- mspHelper.setSetting("align_mag_pitch", (180 + self.alignmentConfig.pitch) * 10, callback);
+ mspHelper.setSetting("align_mag_pitch", self.alignmentConfig.pitch * 10, callback);
},
function (callback) {
if (self.isSavePreset)
mspHelper.setSetting("align_mag_yaw", 0, callback);
- else
- mspHelper.setSetting("align_mag_yaw", self.alignmentConfig.yaw * 10, callback);
+ else {
+ var fix = 0;
+ if ( areAnglesZero() ) {
+ fix = 1; //if all angles are 0, then we have to save yaw = 1 (0.1 deg) to enforce usage of angles, not a usage of preset
+ }
+ mspHelper.setSetting("align_mag_yaw", self.alignmentConfig.yaw * 10 + fix, callback);
+ }
},
mspHelper.saveToEeprom
];
@@ -118,35 +150,98 @@ TABS.magnetometer.initialize = function (callback) {
return arr;
}
+ function toUpperRange(input, max) {
+ while (input > max) input -= 360;
+ while (input + 360 <= max) input += 360;
+ return input;
+ }
+
/*
Returns pitch, roll and yaw in degree by the id of a preset.
Degree are the ones used in the slider
*/
function getAxisDegreeWithPreset(selectedPreset) {
+ //pitch, roll, yaw
switch (selectedPreset) {
case 1: //CW0_DEG = 1
- return [180, 0, 0];
- case 2: //CW90_DEG = 2
- return [180, 0, 90];
- case 3: //CW180_DEG = 3
- return [180, 0, 180];
- case 4: //CW270_DEG = 4
- return [180, 0, 270];
- case 5: //CW0_DEG_FLIP = 5
return [0, 0, 0];
- case 6: //CW90_DEG_FLIP = 5
+ case 2: //CW90_DEG = 2
return [0, 0, 90];
- case 7: //CW180_DEG_FLIP = 5
+ case 3: //CW180_DEG = 3
return [0, 0, 180];
+ case 4: //CW270_DEG = 4
+ return [0, 0, 270];
+ case 5: //CW0_DEG_FLIP = 5
+ return [180, 0, 0];
+ case 6: //CW90_DEG_FLIP = 5
+ return [180, 0, 90];
+ case 7: //CW180_DEG_FLIP = 5
+ return [180, 0, 180];
case 0: //ALIGN_DEFAULT = 0
case 8: //CW270_DEG_FLIP = 5
default://If not recognized, returns defualt
- return [0, 0, 270];
+ return [180, 0, 270];
}
}
- function isUsingAPreset() {
- return self.alignmentConfig.pitch === -180 && self.alignmentConfig.roll === 0 && self.alignmentConfig.yaw === 0
+ function getAxisDegreeWithPresetAndBoardOrientation(selectedPreset) {
+ var degree = getAxisDegreeWithPreset(selectedPreset);
+
+ if (isBoardAlignmentZero()) {
+ return degree;
+ }
+
+ //degree[0] - pitch
+ //degree[1] - roll
+ //degree[2] - yaw
+ //-(pitch-180), -180 - yaw, roll
+ var magRotation = new THREE.Euler(-THREE.Math.degToRad(degree[0]-180), THREE.Math.degToRad(-180 - degree[2]), THREE.Math.degToRad(degree[1]), 'YXZ');
+ var matrix = (new THREE.Matrix4()).makeRotationFromEuler(magRotation);
+
+ var boardRotation = new THREE.Euler( THREE.Math.degToRad( -self.boardAlignmentConfig.pitch ), THREE.Math.degToRad( -self.boardAlignmentConfig.yaw ), THREE.Math.degToRad( -self.boardAlignmentConfig.roll ), 'YXZ');
+ var matrix1 = (new THREE.Matrix4()).makeRotationFromEuler(boardRotation);
+
+ matrix.premultiply(matrix1);
+
+ var euler = new THREE.Euler();
+ euler.setFromRotationMatrix(matrix, 'YXZ');
+
+ var pitch = toUpperRange( Math.round( THREE.Math.radToDeg(-euler.x)) + 180, 180 );
+ var yaw = toUpperRange( Math.round( -180 - THREE.Math.radToDeg(euler.y)), 359 );
+ var roll = toUpperRange( Math.round( THREE.Math.radToDeg(euler.z)), 180 );
+
+ return [pitch, roll, yaw];
+ }
+
+ function updateMagOrientationWithPreset() {
+ if (self.isSavePreset) {
+ const degrees = getAxisDegreeWithPresetAndBoardOrientation(SENSOR_ALIGNMENT.align_mag);
+ presetUpdated(degrees);
+ }
+ }
+
+ function updateBoardRollAxis(value) {
+ self.boardAlignmentConfig.roll = Number(value);
+ self.pageElements.board_roll_slider.val(self.boardAlignmentConfig.roll);
+ self.pageElements.orientation_board_roll.val(self.boardAlignmentConfig.roll);
+ updateMagOrientationWithPreset();
+ self.render3D();
+ }
+
+ function updateBoardPitchAxis(value) {
+ self.boardAlignmentConfig.pitch = Number(value);
+ self.pageElements.board_pitch_slider.val(self.boardAlignmentConfig.pitch);
+ self.pageElements.orientation_board_pitch.val(self.boardAlignmentConfig.pitch);
+ updateMagOrientationWithPreset();
+ self.render3D();
+ }
+
+ function updateBoardYawAxis(value) {
+ self.boardAlignmentConfig.yaw = Number(value);
+ self.pageElements.board_yaw_slider.val(self.boardAlignmentConfig.yaw);
+ self.pageElements.orientation_board_yaw.val(self.boardAlignmentConfig.yaw);
+ updateMagOrientationWithPreset();
+ self.render3D();
}
//Called when roll values change
@@ -173,10 +268,22 @@ TABS.magnetometer.initialize = function (callback) {
self.render3D();
}
- //Called when a preset is selected
- function presetUpdated(degrees) {
+ function enableSavePreset() {
self.isSavePreset = true;
self.pageElements.orientation_mag_e.css("opacity", 1);
+ self.pageElements.orientation_mag_e.css("text-decoration", "");
+ }
+
+ function disableSavePreset() {
+ self.isSavePreset = false;
+ self.pageElements.orientation_mag_e.css("opacity", 0.5);
+ self.pageElements.orientation_mag_e.css("text-decoration", "line-through");
+ }
+
+
+ //Called when a preset is selected
+ function presetUpdated(degrees) {
+ enableSavePreset();
updatePitchAxis(degrees[0]);
updateRollAxis(degrees[1]);
updateYawAxis(degrees[2]);
@@ -184,12 +291,21 @@ TABS.magnetometer.initialize = function (callback) {
function process_html() {
+
localize();
// initialize 3D
self.initialize3D();
let alignments = FC.getSensorAlignments();
+
+ self.pageElements.orientation_board_roll = $('#boardAlignRoll');
+ self.pageElements.orientation_board_pitch = $('#boardAlignPitch');
+ self.pageElements.orientation_board_yaw = $('#boardAlignYaw');
+ self.pageElements.board_roll_slider = $('#board_roll_slider');
+ self.pageElements.board_pitch_slider = $('#board_pitch_slider');
+ self.pageElements.board_yaw_slider = $('#board_yaw_slider');
+
self.pageElements.orientation_mag_e = $('select.magalign');
self.pageElements.orientation_mag_roll = $('#alignRoll');
self.pageElements.orientation_mag_pitch = $('#alignPitch');
@@ -198,24 +314,97 @@ TABS.magnetometer.initialize = function (callback) {
self.pageElements.pitch_slider = $('#pitch_slider');
self.pageElements.yaw_slider = $('#yaw_slider');
+ self.roll_e = $('dd.roll'),
+ self.pitch_e = $('dd.pitch'),
+ self.heading_e = $('dd.heading');
+
for (i = 0; i < alignments.length; i++) {
self.pageElements.orientation_mag_e.append('');
}
self.pageElements.orientation_mag_e.val(SENSOR_ALIGNMENT.align_mag);
- if (isUsingAPreset()) {
+ if (areAnglesZero()) {
//If using a preset, checking if custom values are equal to 0
//Update the slider, but don't save the value until they will be not modified.
- const degrees = getAxisDegreeWithPreset(SENSOR_ALIGNMENT.align_mag);
+ const degrees = getAxisDegreeWithPresetAndBoardOrientation(SENSOR_ALIGNMENT.align_mag);
presetUpdated(degrees);
}
else {
updateRollAxis(self.alignmentConfig.roll);
updatePitchAxis(self.alignmentConfig.pitch);
updateYawAxis(self.alignmentConfig.yaw);
- self.pageElements.orientation_mag_e.css("opacity", 0.5);
+ disableSavePreset();
}
+
+ self.pageElements.orientation_board_roll.change(function () {
+ updateBoardRollAxis(clamp(this, -180, 360));
+ });
+
+ self.pageElements.orientation_board_pitch.change(function () {
+ updateBoardPitchAxis(clamp(this, -180, 360));
+ });
+
+ self.pageElements.orientation_board_yaw.change(function () {
+ updateBoardYawAxis(clamp(this, -180, 360));
+ });
+
+ self.pageElements.board_roll_slider.noUiSlider({
+ start: [self.boardAlignmentConfig.roll],
+ range: {
+ 'min': [-180],
+ 'max': [360]
+ },
+ step: 1,
+ });
+ self.pageElements.board_roll_slider.noUiSlider_pips({
+ mode: 'values',
+ values: generateRange(-180, 360, 45),
+ density: 4,
+ stepped: true
+ });
+
+ self.pageElements.board_pitch_slider.noUiSlider({
+ start: [self.boardAlignmentConfig.pitch],
+ range: {
+ 'min': [-180],
+ 'max': [360]
+ },
+ step: 1,
+ });
+ self.pageElements.board_pitch_slider.noUiSlider_pips({
+ mode: 'values',
+ values: generateRange(-180, 360, 45),
+ density: 4,
+ stepped: true
+ });
+
+ self.pageElements.board_yaw_slider.noUiSlider({
+ start: [self.boardAlignmentConfig.yaw],
+ range: {
+ 'min': [-180],
+ 'max': [360]
+ },
+ step: 1,
+ });
+ self.pageElements.board_yaw_slider.noUiSlider_pips({
+ mode: 'values',
+ values: generateRange(-180, 360, 45),
+ density: 4,
+ stepped: true
+ });
+
+
+ self.pageElements.board_pitch_slider.Link('lower').to((e) => {
+ updateBoardPitchAxis(e);
+ });
+ self.pageElements.board_roll_slider.Link('lower').to((e) => {
+ updateBoardRollAxis(e);
+ });
+ self.pageElements.board_yaw_slider.Link('lower').to((e) => {
+ updateBoardYawAxis(e);
+ });
+
const elementToShow = $("#element_to_show");
elementToShow.change(function () {
const value = parseInt($(this).val());
@@ -229,28 +418,28 @@ TABS.magnetometer.initialize = function (callback) {
self.pageElements.orientation_mag_e.change(function () {
SENSOR_ALIGNMENT.align_mag = parseInt($(this).val());
- const degrees = getAxisDegreeWithPreset(SENSOR_ALIGNMENT.align_mag);
+ const degrees = getAxisDegreeWithPresetAndBoardOrientation(SENSOR_ALIGNMENT.align_mag);
+ presetUpdated(degrees);
+ });
+
+ self.pageElements.orientation_mag_e.on('mousedown', function () {
+ const degrees = getAxisDegreeWithPresetAndBoardOrientation(SENSOR_ALIGNMENT.align_mag);
presetUpdated(degrees);
});
self.pageElements.orientation_mag_roll.change(function () {
- self.isSavePreset = false;
- self.pageElements.orientation_mag_e.css("opacity", 0.5);
- updateRollAxis(clamp(this, -180, 180));
+ disableSavePreset();
+ updateRollAxis(clamp(this, -180, 360));
});
self.pageElements.orientation_mag_pitch.change(function () {
- self.isSavePreset = false;
- self.pageElements.orientation_mag_e.css("opacity", 0.5);
- updatePitchAxis(clamp(this, -180, 180));
-
+ disableSavePreset();
+ updatePitchAxis(clamp(this, -180, 360));
});
self.pageElements.orientation_mag_yaw.change(function () {
- self.isSavePreset = false;
- self.pageElements.orientation_mag_e.css("opacity", 0.5);
+ disableSavePreset();
updateYawAxis(clamp(this, -180, 360));
-
});
$('a.save').click(function () {
@@ -261,13 +450,13 @@ TABS.magnetometer.initialize = function (callback) {
start: [self.alignmentConfig.roll],
range: {
'min': [-180],
- 'max': [180]
+ 'max': [360]
},
step: 1,
});
self.pageElements.roll_slider.noUiSlider_pips({
mode: 'values',
- values: generateRange(-180, 180, 15),
+ values: generateRange(-180, 360, 45),
density: 4,
stepped: true
});
@@ -276,13 +465,13 @@ TABS.magnetometer.initialize = function (callback) {
start: [self.alignmentConfig.pitch],
range: {
'min': [-180],
- 'max': [180]
+ 'max': [360]
},
step: 1,
});
self.pageElements.pitch_slider.noUiSlider_pips({
mode: 'values',
- values: generateRange(-180, 180, 15),
+ values: generateRange(-180, 360, 45),
density: 4,
stepped: true
});
@@ -293,7 +482,7 @@ TABS.magnetometer.initialize = function (callback) {
'min': [-180],
'max': [360]
},
- step: 45,
+ step: 1,
});
self.pageElements.yaw_slider.noUiSlider_pips({
mode: 'values',
@@ -302,6 +491,7 @@ TABS.magnetometer.initialize = function (callback) {
stepped: true
});
+
self.pageElements.pitch_slider.Link('lower').to((e) => {
updatePitchAxis(e);
});
@@ -312,19 +502,31 @@ TABS.magnetometer.initialize = function (callback) {
updateYawAxis(e);
});
- self.pageElements.pitch_slider.change((e) => {
- self.isSavePreset = false;
- self.pageElements.orientation_mag_e.css("opacity", 0.5);
+ self.pageElements.pitch_slider.on('slide', (e) => {
+ disableSavePreset();
});
- self.pageElements.roll_slider.change((e) => {
- self.isSavePreset = false;
- self.pageElements.orientation_mag_e.css("opacity", 0.5);
+ self.pageElements.roll_slider.on('slide', (e) => {
+ disableSavePreset();
});
- self.pageElements.yaw_slider.change((e) => {
- self.isSavePreset = false;
- self.pageElements.orientation_mag_e.css("opacity", 0.5);
+ self.pageElements.yaw_slider.on('slide', (e) => {
+ disableSavePreset();
});
+ function get_fast_data() {
+ if (helper.mspQueue.shouldDrop()) {
+ return;
+ }
+
+ MSP.send_message(MSPCodes.MSP_ATTITUDE, false, false, function () {
+ self.roll_e.text(chrome.i18n.getMessage('initialSetupAttitude', [SENSOR_DATA.kinematics[0]]));
+ self.pitch_e.text(chrome.i18n.getMessage('initialSetupAttitude', [SENSOR_DATA.kinematics[1]]));
+ self.heading_e.text(chrome.i18n.getMessage('initialSetupAttitude', [SENSOR_DATA.kinematics[2]]));
+ self.render3D();
+ });
+ }
+
+ helper.mspBalancedInterval.add('setup_data_pull_fast', 40, 1, get_fast_data);
+
GUI.content_ready(callback);
}
@@ -395,14 +597,17 @@ TABS.magnetometer.initialize3D = function () {
xyz.visible = !self.showMagnetometer;
fc.visible = true;
- var magRotation = new THREE.Euler(-THREE.Math.degToRad(self.alignmentConfig.pitch), THREE.Math.degToRad(-180 - self.alignmentConfig.yaw), THREE.Math.degToRad(self.alignmentConfig.roll), 'YXZ');
+ var magRotation = new THREE.Euler(-THREE.Math.degToRad(self.alignmentConfig.pitch-180), THREE.Math.degToRad(-180 - self.alignmentConfig.yaw), THREE.Math.degToRad(self.alignmentConfig.roll), 'YXZ');
var matrix = (new THREE.Matrix4()).makeRotationFromEuler(magRotation);
- var boardRotation = new THREE.Euler( THREE.Math.degToRad( -BOARD_ALIGNMENT.pitch / 10.0 ), THREE.Math.degToRad( -BOARD_ALIGNMENT.yaw / 10.0 ), THREE.Math.degToRad( -BOARD_ALIGNMENT.roll / 10.0 ), 'YXZ');
+ var boardRotation = new THREE.Euler( THREE.Math.degToRad( -self.boardAlignmentConfig.pitch ), THREE.Math.degToRad( -self.boardAlignmentConfig.yaw ), THREE.Math.degToRad( -self.boardAlignmentConfig.roll ), 'YXZ');
var matrix1 = (new THREE.Matrix4()).makeRotationFromEuler(boardRotation);
-
- matrix.multiply(matrix1);
+/*
+ if ( self.isSavePreset ) {
+ matrix.premultiply(matrix1); //preset specifies orientation relative to FC, align_max_xxx specify absolute orientation
+ }
+*/
gps.rotation.setFromRotationMatrix(matrix);
xyz.rotation.setFromRotationMatrix(matrix);
fc.rotation.setFromRotationMatrix(matrix1);