Merge pull request #759 from iNavFlight/de_opflow_calibration

[OPFLOW] Initial cut on optic flow calibration

Author: digitalentity

_locales/en/messages.json

@@ -463,6 +463,12 @@
     "initialSetupMagCalibEnded": {
         "message": "Magnetometer calibration <span style=\"color: #37a8db\">finished</span>"
     },
+    "initialSetupOpflowCalibStarted": {
+        "message": "Optic flow calibration started"
+    },
+    "initialSetupOpflowCalibEnded": {
+        "message": "Optic flow calibration <span style=\"color: #37a8db\">finished</span>"
+    },
     "initialSetupSettingsRestored": {
         "message": "Settings restored to <strong>default</strong>"
     },
@@ -2068,6 +2074,15 @@
     "calibrationHead4": {
         "message": "Compass Calibration"
     },
+    "calibrationHead5": {
+        "message": "Optic Flow Calibration"
+    },
+    "OpflowCalText": {
+        "message": "After pressing the button you have 30 seconds to hold the copter in the air and tilt it to sides without moving it horizontally. Note that optic flow sensor needs to observe the surface at all times."
+    },
+    "OpflowCalBtn": {
+        "message": "Calibrate Optic Flow sensor"
+    },
     "accZero": {
         "message": "Acc Zero"
     },

js/fc.js

@@ -400,6 +400,9 @@ var FC = {
                 X: null,
                 Y: null,
                 Z: null
+            },
+            opflow: {
+                Scale: null
             }
         };
 

js/msp/MSPCodes.js

@@ -209,5 +209,7 @@ var MSPCodes = {
     MSP2_INAV_SET_LOGIC_CONDITIONS:     0x2023,
 
     MSP2_PID:                           0x2030,
-    MSP2_SET_PID:                       0x2031
+    MSP2_SET_PID:                       0x2031,
+
+    MSP2_INAV_OPFLOW_CALIBRATION:       0x2032
 };

js/msp/MSPHelper.js

@@ -642,6 +642,9 @@ var mspHelper = (function (gui) {
             case MSPCodes.MSP_MAG_CALIBRATION:
                 console.log('Mag calibration executed');
                 break;
+            case MSPCodes.MSP2_INAV_OPFLOW_CALIBRATION:
+                console.log('Optic flow calibration executed');
+                break;
             case MSPCodes.MSP_SET_MISC:
                 console.log('MISC Configuration saved');
                 break;
@@ -1293,6 +1296,11 @@ var mspHelper = (function (gui) {
                 CALIBRATION_DATA.magZero.X = data.getInt16(13, true);
                 CALIBRATION_DATA.magZero.Y = data.getInt16(15, true);
                 CALIBRATION_DATA.magZero.Z = data.getInt16(17, true);
+
+                if (semver.gte(CONFIG.flightControllerVersion, "2.2.0")) {
+                    CALIBRATION_DATA.opflow.Scale = (data.getInt16(19, true) / 256.0);
+                }
+
                 break;
 
             case MSPCodes.MSP_SET_CALIBRATION_DATA:
@@ -1945,6 +1953,11 @@ var mspHelper = (function (gui) {
 
                 buffer.push(lowByte(CALIBRATION_DATA.magZero.Z));
                 buffer.push(highByte(CALIBRATION_DATA.magZero.Z));
+
+                if (semver.gte(CONFIG.flightControllerVersion, "2.2.0")) {
+                    buffer.push(lowByte(Math.round(CALIBRATION_DATA.opflow.Scale * 256)));
+                    buffer.push(highByte(Math.round(CALIBRATION_DATA.opflow.Scale * 256)));
+                }
                 break;
 
             case MSPCodes.MSP_SET_POSITION_ESTIMATION_CONFIG:

tabs/calibration.html

@@ -115,6 +115,25 @@
                         </table>
                     </div>
                 </div>
+                <div class="gui_box grey">
+                    <div class="gui_box_titlebar">
+                        <div class="spacer_box_title" data-i18n="calibrationHead5"></div>
+                    </div>
+                    <div class="spacer_box">
+                        <span data-i18n="OpflowCalText">text</span>
+                        <div class="default_btn">
+                            <div id="opflow_btn">
+                                <a class="calibrateopflow" href="#" data-i18n="OpflowCalBtn"></a>
+                            </div>
+                        </div>
+                        <table id="opflow-calibrated-data" class="cf_table">
+                            <tr>
+                                <td><label for="OpflowScale"><span>Scale</span></label></td>
+                                <td><input type="number" name="OpflowScale" min="0" max="10000"></td>
+                            </tr>
+                        </table>
+                    </div>
+                </div>
             </div>
         </div>
     </div>
@@ -155,3 +174,10 @@ <h1 class="modal__title modal__title--center" data-i18n="accCalibrationProcessin
         <div id="modal-compass-countdown" class="modal__text"></div>
     </div>
 </div>
+
+<div id="modal-opflow-processing" class="is-hidden">
+    <div class="modal__content">
+        <h1 class="modal__title modal__title--center" data-i18n="accCalibrationProcessing"></h1>
+        <div id="modal-opflow-countdown" class="modal__text"></div>
+    </div>
+</div>

tabs/calibration.js

@@ -61,7 +61,8 @@ TABS.calibration.initialize = function (callback) {
     loadChainer.execute();
 
     saveChainer.setChain([
-        mspHelper.saveCalibrationData
+        mspHelper.saveCalibrationData,
+        mspHelper.saveToEeprom
     ]);
     saveChainer.setExitPoint(reboot);
 
@@ -105,6 +106,7 @@ TABS.calibration.initialize = function (callback) {
             $('[name=accZero' + item + ']').val(CALIBRATION_DATA.accZero[item]);
             $('[name=Mag' + item + ']').val(CALIBRATION_DATA.magZero[item]);
         });
+        $('[name=OpflowScale]').val(CALIBRATION_DATA.opflow.Scale);
         updateCalibrationSteps();
     }
 
@@ -176,6 +178,7 @@ TABS.calibration.initialize = function (callback) {
 
     function processHtml() {
         $('#calibrateButtonSave').on('click', function () {
+            CALIBRATION_DATA.opflow.Scale = parseFloat($('[name=OpflowScale]').val());
             saveChainer.execute();
         });
 
@@ -184,6 +187,11 @@ TABS.calibration.initialize = function (callback) {
             $('#mag_btn, #mag-calibrated-data').css('pointer-events', 'none').css('opacity', '0.4');
         }
 
+        if (SENSOR_CONFIG.opflow === 0) {
+            //Comment for test
+            $('#opflow_btn, #opflow-calibrated-data').css('pointer-events', 'none').css('opacity', '0.4');
+        }
+
         $('#mag_btn').on('click', function () {
             MSP.send_message(MSPCodes.MSP_MAG_CALIBRATION, false, false, function () {
                 GUI.log(chrome.i18n.getMessage('initialSetupMagCalibStarted'));
@@ -217,6 +225,39 @@ TABS.calibration.initialize = function (callback) {
             }, 1000);
         });
 
+        $('#opflow_btn').on('click', function () {
+            MSP.send_message(MSPCodes.MSP2_INAV_OPFLOW_CALIBRATION, false, false, function () {
+                GUI.log(chrome.i18n.getMessage('initialSetupOpflowCalibStarted'));
+            });
+
+            var button = $(this);
+
+            $(button).addClass('disabled');
+
+            modalProcessing = new jBox('Modal', {
+                width: 400,
+                height: 100,
+                animation: false,
+                closeOnClick: false,
+                closeOnEsc: false,
+                content: $('#modal-opflow-processing')
+            }).open();
+
+            var countdown = 30;
+            helper.interval.add('opflow_calibration_interval', function () {
+                countdown--;
+                $('#modal-opflow-countdown').text(countdown);
+                if (countdown === 0) {
+                    $(button).removeClass('disabled');
+
+                    modalProcessing.close();
+                    GUI.log(chrome.i18n.getMessage('initialSetupOpflowCalibEnded'));
+                    MSP.send_message(MSPCodes.MSP_CALIBRATION_DATA, false, false, updateSensorData);
+                    helper.interval.remove('opflow_calibration_interval');
+                }
+            }, 1000);
+        });
+
         $('#modal-start-button').click(function () {
             modalStart.close();
             TABS.calibration.model.next();