diff --git a/_locales/en/messages.json b/_locales/en/messages.json index 803adb37..68a4d78a 100755 --- a/_locales/en/messages.json +++ b/_locales/en/messages.json @@ -68,6 +68,9 @@ "tabSetup": { "message": "Setup" }, + "tabCalibration": { + "message": "Calibration" + }, "tabConfiguration": { "message": "Configuration" }, @@ -1891,6 +1894,42 @@ "armingCheckFail": { "message": "
" }, + "calibrationHead1": { + "message": "Accelerometer Calibration" + }, + "calibrationHead2": { + "message": "Accelerometer Values" + }, + "calibrationHead3": { + "message": "Level Calibration" + }, + "calibrationHead4": { + "message": "Compass Calibration" + }, + "accZero": { + "message": "Acc Zero" + }, + "accGain": { + "message": "Acc Gain" + }, + "NoteCalibration": { + "message": "Note: If the flightcontroller is mounted in another angle or upside down, do the calibration steps with the flightcontroller pointing as shown in the pictures, not the quad (otherwise calibration won´t work)." + }, + "AccBtn": { + "message": "Calibrate Accelerometer" + }, + "MagCalText": { + "message": "After pressing the button you have 30 seconds to hold the copter in the air and rotate it so that each side (front, back, left, right, top and bottom) points down towards the earth." + }, + "MagBtn": { + "message": "Calibrate Compass" + }, + "LevCalText": { + "message": "Please put some Text here…" + }, + "LevBtn": { + "message": "Level calibration" + }, "tabPresets": { "message": "Presets" }, @@ -1915,6 +1954,24 @@ "presetApplyDescription": { "message": "Preset overwrites selected configuration values including mixer, filtering, PIDs and other. Settings like: flight modes, radio settings, failsafe and OSD are not changed. Applied values should NOT treated as final values, but entry points for final tuning.
Always check new configuration before flying!" }, + "OK": { + "message": "OK" + }, + "accCalibrationStartTitle": { + "message": "Accelerometer Calibration" + }, + "accCalibrationStartBody": { + "message": "Place flight controller in a position showed in image, then press Calibrate button again. Repeat for each of 6 position. Keep it stable during calibration." + }, + "accCalibrationStopTitle": { + "message": "Calibration finished" + }, + "accCalibrationStopBody": { + "message": "Accelerometer calibration finished, check if values have been saved." + }, + "accCalibrationProcessing": { + "message": "Processing..." + }, "tabAdvancedTuning": { "message": "Advanced tuning" }, diff --git a/images/icons/cf_icon_cal_grey.svg b/images/icons/cf_icon_cal_grey.svg new file mode 100644 index 00000000..84cc0969 --- /dev/null +++ b/images/icons/cf_icon_cal_grey.svg @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/images/icons/cf_icon_cal_white.svg b/images/icons/cf_icon_cal_white.svg new file mode 100644 index 00000000..38a408eb --- /dev/null +++ b/images/icons/cf_icon_cal_white.svg @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/images/icons/check.svg b/images/icons/check.svg new file mode 100644 index 00000000..9fbb4c77 --- /dev/null +++ b/images/icons/check.svg @@ -0,0 +1,11 @@ + + + + + + diff --git a/images/icons/pos1.svg b/images/icons/pos1.svg new file mode 100644 index 00000000..98f60115 --- /dev/null +++ b/images/icons/pos1.svg @@ -0,0 +1,21 @@ + + + + + + + diff --git a/images/icons/pos1_grey.svg b/images/icons/pos1_grey.svg new file mode 100644 index 00000000..38911397 --- /dev/null +++ b/images/icons/pos1_grey.svg @@ -0,0 +1,20 @@ + + + + + + + diff --git a/images/icons/pos2.svg b/images/icons/pos2.svg new file mode 100644 index 00000000..ac88e27c --- /dev/null +++ b/images/icons/pos2.svg @@ -0,0 +1,21 @@ + + + + + + + diff --git a/images/icons/pos2_grey.svg b/images/icons/pos2_grey.svg new file mode 100644 index 00000000..afc37942 --- /dev/null +++ b/images/icons/pos2_grey.svg @@ -0,0 +1,20 @@ + + + + + + + diff --git a/images/icons/pos3.svg b/images/icons/pos3.svg new file mode 100644 index 00000000..6c4cfe6e --- /dev/null +++ b/images/icons/pos3.svg @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/images/icons/pos3_grey.svg b/images/icons/pos3_grey.svg new file mode 100644 index 00000000..1b2b1d4d --- /dev/null +++ b/images/icons/pos3_grey.svg @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/images/icons/pos4.svg b/images/icons/pos4.svg new file mode 100644 index 00000000..247a45c7 --- /dev/null +++ b/images/icons/pos4.svg @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/images/icons/pos4_grey.svg b/images/icons/pos4_grey.svg new file mode 100644 index 00000000..2a34455f --- /dev/null +++ b/images/icons/pos4_grey.svg @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/images/icons/pos5.svg b/images/icons/pos5.svg new file mode 100644 index 00000000..efbbc317 --- /dev/null +++ b/images/icons/pos5.svg @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/images/icons/pos5_grey.svg b/images/icons/pos5_grey.svg new file mode 100644 index 00000000..90173c48 --- /dev/null +++ b/images/icons/pos5_grey.svg @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/images/icons/pos6.svg b/images/icons/pos6.svg new file mode 100644 index 00000000..da14b3aa --- /dev/null +++ b/images/icons/pos6.svg @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + diff --git a/images/icons/pos6_grey.svg b/images/icons/pos6_grey.svg new file mode 100644 index 00000000..e8be72c5 --- /dev/null +++ b/images/icons/pos6_grey.svg @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + diff --git a/js/fc.js b/js/fc.js index 6289d7dc..987d624b 100644 --- a/js/fc.js +++ b/js/fc.js @@ -44,6 +44,7 @@ var CONFIG, SENSOR_STATUS, SENSOR_CONFIG, NAV_POSHOLD, + CALIBRATION_DATA, POSITION_ESTIMATOR, RTH_AND_LAND_CONFIG, FW_CONFIG; @@ -284,6 +285,32 @@ var FC = { hoverThrottle: null }; + CALIBRATION_DATA = { + acc: { + Pos0: null, + Pos1: null, + Pos2: null, + Pos3: null, + Pos4: null, + Pos5: null + }, + accZero: { + X: null, + Y: null, + Z: null + }, + accGain: { + X: null, + Y: null, + Z: null + }, + magZero: { + X: null, + Y: null, + Z: null + } + }; + RTH_AND_LAND_CONFIG = { minRthDistance: null, rthClimbFirst: null, diff --git a/js/gui.js b/js/gui.js index cd6a8327..3028577d 100644 --- a/js/gui.js +++ b/js/gui.js @@ -34,6 +34,7 @@ var GUI_control = function () { 'receiver', 'sensors', 'servos', + 'calibration', 'setup', 'osd', 'profiles', diff --git a/js/msp/MSPCodes.js b/js/msp/MSPCodes.js index 10d831f3..a09228d3 100644 --- a/js/msp/MSPCodes.js +++ b/js/msp/MSPCodes.js @@ -12,6 +12,8 @@ var MSPCodes = { MSP_NAV_POSHOLD: 12, MSP_SET_NAV_POSHOLD: 13, + MSP_CALIBRATION_DATA: 14, + MSP_SET_CALIBRATION_DATA: 15, MSP_POSITION_ESTIMATION_CONFIG: 16, MSP_SET_POSITION_ESTIMATION_CONFIG: 17, diff --git a/js/msp/MSPHelper.js b/js/msp/MSPHelper.js index 5d6e9da9..39112ea1 100644 --- a/js/msp/MSPHelper.js +++ b/js/msp/MSPHelper.js @@ -982,6 +982,30 @@ var mspHelper = (function (gui) { console.log('NAV_POSHOLD saved'); break; + case MSPCodes.MSP_CALIBRATION_DATA: + var callibrations = data.getUint8(0); + CALIBRATION_DATA.acc.Pos0 = (1 & (callibrations >> 0)); + CALIBRATION_DATA.acc.Pos1 = (1 & (callibrations >> 1)); + CALIBRATION_DATA.acc.Pos2 = (1 & (callibrations >> 2)); + CALIBRATION_DATA.acc.Pos3 = (1 & (callibrations >> 3)); + CALIBRATION_DATA.acc.Pos4 = (1 & (callibrations >> 4)); + CALIBRATION_DATA.acc.Pos5 = (1 & (callibrations >> 5)); + + CALIBRATION_DATA.accZero.X = data.getInt16(1, true); + CALIBRATION_DATA.accZero.Y = data.getInt16(3, true); + CALIBRATION_DATA.accZero.Z = data.getInt16(5, true); + CALIBRATION_DATA.accGain.X = data.getInt16(7, true); + CALIBRATION_DATA.accGain.Y = data.getInt16(9, true); + CALIBRATION_DATA.accGain.Z = data.getInt16(11, true); + CALIBRATION_DATA.magZero.X = data.getInt16(13, true); + CALIBRATION_DATA.magZero.Y = data.getInt16(15, true); + CALIBRATION_DATA.magZero.Z = data.getInt16(17, true); + break; + + case MSPCodes.MSP_SET_CALIBRATION_DATA: + console.log('Calibration data saved'); + break; + case MSPCodes.MSP_POSITION_ESTIMATION_CONFIG: POSITION_ESTIMATOR.w_z_baro_p = data.getUint16(0, true) / 100; POSITION_ESTIMATOR.w_z_gps_p = data.getUint16(2, true) / 100; @@ -1388,6 +1412,36 @@ var mspHelper = (function (gui) { buffer.push(highByte(NAV_POSHOLD.hoverThrottle)); break; + case MSPCodes.MSP_SET_CALIBRATION_DATA: + + buffer.push(lowByte(CALIBRATION_DATA.accZero.X)); + buffer.push(highByte(CALIBRATION_DATA.accZero.X)); + + buffer.push(lowByte(CALIBRATION_DATA.accZero.Y)); + buffer.push(highByte(CALIBRATION_DATA.accZero.Y)); + + buffer.push(lowByte(CALIBRATION_DATA.accZero.Z)); + buffer.push(highByte(CALIBRATION_DATA.accZero.Z)); + + buffer.push(lowByte(CALIBRATION_DATA.accGain.X)); + buffer.push(highByte(CALIBRATION_DATA.accGain.X)); + + buffer.push(lowByte(CALIBRATION_DATA.accGain.Y)); + buffer.push(highByte(CALIBRATION_DATA.accGain.Y)); + + buffer.push(lowByte(CALIBRATION_DATA.accGain.Z)); + buffer.push(highByte(CALIBRATION_DATA.accGain.Z)); + + buffer.push(lowByte(CALIBRATION_DATA.magZero.X)); + buffer.push(highByte(CALIBRATION_DATA.magZero.X)); + + buffer.push(lowByte(CALIBRATION_DATA.magZero.Y)); + buffer.push(highByte(CALIBRATION_DATA.magZero.Y)); + + buffer.push(lowByte(CALIBRATION_DATA.magZero.Z)); + buffer.push(highByte(CALIBRATION_DATA.magZero.Z)); + break; + case MSPCodes.MSP_SET_POSITION_ESTIMATION_CONFIG: buffer.push(lowByte(POSITION_ESTIMATOR.w_z_baro_p * 100)); buffer.push(highByte(POSITION_ESTIMATOR.w_z_baro_p * 100)); @@ -2223,6 +2277,22 @@ var mspHelper = (function (gui) { } }; + self.loadCalibrationData = function (callback) { + if (semver.gte(CONFIG.flightControllerVersion, "1.6.0")) { + MSP.send_message(MSPCodes.MSP_CALIBRATION_DATA, false, false, callback); + } else { + callback(); + } + }; + + self.saveCalibrationData = function (callback) { + if (semver.gte(CONFIG.flightControllerVersion, "1.6.0")) { + MSP.send_message(MSPCodes.MSP_SET_CALIBRATION_DATA, mspHelper.crunch(MSPCodes.MSP_SET_CALIBRATION_DATA), false, callback); + } else { + callback(); + } + }; + self.loadRthAndLandConfig = function (callback) { if (semver.gte(CONFIG.flightControllerVersion, "1.7.1")) { MSP.send_message(MSPCodes.MSP_RTH_AND_LAND_CONFIG, false, false, callback); diff --git a/main.css b/main.css index 6d226eae..3dfbb971 100644 --- a/main.css +++ b/main.css @@ -866,6 +866,18 @@ li.active .ic_transponder { background-image: url("../images/icons/cf_icon_transponder_white.svg"); } +.ic_calibration { + background-image: url(../images/icons/cf_icon_cal_grey.svg); +} + +.ic_calibration:hover { + background-image: url(../images/icons/cf_icon_cal_white.svg); +} + +li.active .ic_calibration { + background-image: url(../images/icons/cf_icon_cal_white.svg); +} + /* SPARE Tab-Icons */ .ic_failsafe { background-image: url("../images/icons/cf_icon_failsafe_grey.svg"); @@ -1502,7 +1514,7 @@ dialog { /* fixing padding for all Tabs*/ .tab-setup, .tab-landing, .tab-adjustments, .tab-auxiliary, .tab-cli, .tab-configuration, .tab-failsafe, .tab-onboard_logging, .tab-firmware_flasher, .tab-gps, .tab-help, .tab-led-strip, .tab-logging, .tab-modes, .tab-motors, .tab-pid_tuning, -.tab-ports, .tab-receiver, .tab-sensors, .tab-servos, .tab-osd { +.tab-ports, .tab-receiver, .tab-sensors, .tab-servos, .tab-osd, .tab-calibration { height: 100%; position: relative; } diff --git a/main.html b/main.html index 457d9e7e..f989e0c4 100755 --- a/main.html +++ b/main.html @@ -164,6 +164,7 @@