Filtering configuration

_locales/en/messages.json

@@ -1750,5 +1750,32 @@
     },
     "yawPLimitHelp": {
         "message": "Limiter for yaw P term. Increasing it improves yaw authority but can cause instability on ROLL and PITCH."
+    },
+    "tabFiltering": {
+        "message": "Filtering"
+    },
+    "gyroLpfCutoffFrequency": {
+        "message": "Gyro LPF cutoff frequency"
+    },
+    "gyroLpfCutoffFrequencyHelp": {
+        "message": "Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. Too high value can cause motor and ESC overheating."
+    },
+    "accLpfCutoffFrequency": {
+        "message": "Accelerometer LPF cutoff frequency"
+    },
+    "accLpfCutoffFrequencyHelp": {
+        "message": "Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value."
+    },
+    "dtermLpfCutoffFrequency": {
+        "message": "D-term LPF cutoff frequency"
+    },
+    "dtermLpfCutoffFrequencyHelp": {
+        "message": "Lowpass cutoff filter for Dterm for all PID controllers"
+    },
+    "yawLpfCutoffFrequency": {
+        "message": "Yaw LPF cutoff frequency"
+    },
+    "yawLpfCutoffFrequencyHelp": {
+        "message": "Yaw P-term LPF cutoff frequency"
     }
 }

js/fc.js

@@ -40,6 +40,7 @@ var RXFAIL_CONFIG;
 var ADVANCED_CONFIG;
 var INAV_PID_CONFIG;
 var PID_ADVANCED;
+var FILTER_CONFIG;
 
 var FC = {
     isRatesInDps: function () {
@@ -212,6 +213,12 @@ var FC = {
             gyroSync: null
         };
 
+        FILTER_CONFIG = {
+            gyroSoftLpfHz: null,
+            dtermLpfHz: null,
+            yawLpfHz: null
+        }
+
         PID_ADVANCED = {
             rollPitchItermIgnoreRate: null,
             yawItermIgnoreRate: null,

js/msp.js

@@ -1102,6 +1102,24 @@ var MSP = {
                 console.log("Advanced config saved");
                 break;
 
+            case MSP_codes.MSP_FILTER_CONFIG:
+                FILTER_CONFIG.gyroSoftLpfHz     = data.getUint8(0, true);
+                FILTER_CONFIG.dtermLpfHz        = data.getUint16(1, true);
+                FILTER_CONFIG.yawLpfHz          = data.getUint16(3, true);
+                /*
+                sbufWriteU16(dst, 1); //masterConfig.gyro_soft_notch_hz_1
+                sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_1
+                sbufWriteU16(dst, 1); //BF: currentProfile->pidProfile.dterm_notch_hz
+                sbufWriteU16(dst, 1); //currentProfile->pidProfile.dterm_notch_cutoff
+                sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_hz_2
+                sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
+                */
+                break;
+
+            case MSP_codes.MSP_SET_FILTER_CONFIG:
+                console.log("Filter config saved");
+                break;
+
             case MSP_codes.MSP_PID_ADVANCED:
                 PID_ADVANCED.rollPitchItermIgnoreRate           = data.getUint16(0, true);
                 PID_ADVANCED.yawItermIgnoreRate                 = data.getUint16(2, true);
@@ -1552,6 +1570,34 @@ MSP.crunch = function (code) {
             buffer.push(0); //reserved
             break;
 
+        case MSP_codes.MSP_SET_FILTER_CONFIG:
+            buffer.push(FILTER_CONFIG.gyroSoftLpfHz);
+
+            buffer.push(lowByte(FILTER_CONFIG.dtermLpfHz));
+            buffer.push(highByte(FILTER_CONFIG.dtermLpfHz));
+
+            buffer.push(lowByte(FILTER_CONFIG.yawLpfHz));
+            buffer.push(highByte(FILTER_CONFIG.yawLpfHz));
+
+            buffer.push(0);
+            buffer.push(0);
+
+            buffer.push(0);
+            buffer.push(0);
+
+            buffer.push(0);
+            buffer.push(0);
+
+            buffer.push(0);
+            buffer.push(0);
+
+            buffer.push(0);
+            buffer.push(0);
+
+            buffer.push(0);
+            buffer.push(0);
+            break;
+
         case MSP_codes.MSP_SET_PID_ADVANCED:
             buffer.push(lowByte(PID_ADVANCED.rollPitchItermIgnoreRate));
             buffer.push(highByte(PID_ADVANCED.rollPitchItermIgnoreRate));

tabs/pid_tuning.css

@@ -1,23 +1,23 @@
-.rate-tpa.rate-tpa--inav input[type="number"] {
+/*.rate-tpa.rate-tpa--inav input[type="number"],*/
+.rate-tpa_input {
     margin: 4px;
     width: auto;
     border: 1px solid silver;
     border-radius: 3px;
     display: inline-block;
+    line-height: 20px;
+    text-align: right;
 }
 
-.rate-tpa.rate-tpa--inav td {
+.rate-tpa.rate-tpa--inav td,
+.rate-tpa.rate-tpa--filtering td {
     background-color: #DEDEDE;
     width: auto;
     border-bottom: 1px solid #ccc;
 }
 
-.tab-pid_tuning .rate-tpa.rate-tpa--inav th:first-child {
+.tab-pid_tuning .rate-tpa.rate-tpa--filtering th {
-    border-top-left-radius: 0;
+    width: 18em;
-}
-
-.tab-pid_tuning .rate-tpa.rate-tpa--inav .roll {
-    border-bottom-left-radius: 0;
 }
 
 .tab-pid_tuning .rate-tpa th {
@@ -66,16 +66,12 @@
     width: calc(100% - 10px);
 }
 
-.tab-pid_tuning .rate-tpa th:nth-child(2) {
+.tab-pid_tuning .rate-tpa tr:first-child th {
-    border-top-left-radius: 3px;
+    border-top-left-radius: 5px;
 }
 
-.tab-pid_tuning .rate-tpa th:first-child {
+.tab-pid_tuning .rate-tpa tr:first-child td {
-    border-top-left-radius: 3px;
+    border-top-right-radius: 5px;
-}
-
-.tab-pid_tuning .rate-tpa th:last-child {
-    border-top-right-radius: 3px;
 }
 
 .tab-pid_tuning .rate-tpa {
@@ -145,7 +141,7 @@
     padding-left: 0px;
 }
 
-.tab-pid_tuning table input {
+.pid-section input {
     display: block;
     width: calc(100% - 0px);
     height: 20px;
@@ -357,10 +353,6 @@
     border-top-left-radius: 0px;
 }
 
-.tab-pid_tuning .roll {
-    border-bottom-left-radius: 3px;
-}
-
 .tab-pid_tuning .pidTuningLevel {
     float: left;
 }

tabs/pid_tuning.html

@@ -13,13 +13,11 @@
                 <div class="default_btn resetbt">
                     <a href="#" id="resetPIDs">Reset PID Controller</a>
                 </div>
-
-
             </div>
         </div>
         <form name="pid-tuning" id="pid-tuning">
             <div class="clear-both"></div>
-            <div class="cf_column half">
+            <div class="cf_column half pid-section">
                 <div class="gui_box grey">
                     <table class="pid_titlebar">
                         <tr>
@@ -153,25 +151,25 @@
                             <tr>
                                 <th class="roll" i18n="pidTuningRollRate"></th>
                                 <td class="roll">
-                                    <input type="number" name="roll" step="10" min="60" max="1800" /> degrees per second
+                                    <input type="number" name="roll" class="rate-tpa_input" step="10" min="60" max="1800" /> degrees per second
                                 </td>
                             </tr>
                             <tr>
                                 <th class="pitch" i18n="pidTuningPitchRate"></th>
                                 <td class="pitch">
-                                    <input type="number" name="pitch" step="10" min="60" max="1800" /> degrees per second
+                                    <input type="number" name="pitch" class="rate-tpa_input" step="10" min="60" max="1800" /> degrees per second
                                 </td>
                             </tr>
                             <tr>
                                 <th class="yaw" i18n="pidTuningYawRate"></th>
                                 <td class="yaw">
-                                    <input type="number" name="yaw" step="10" min="20" max="1800" /> degrees per second
+                                    <input type="number" name="yaw" class="rate-tpa_input" step="10" min="20" max="1800" /> degrees per second
                                 </td>
                             </tr>
                             <tr class="requires-v1_4">
                                 <th i18n="magHoldYawRate"></th>
                                 <td >
-                                    <input type="number" id="magHoldYawRate" step="5" min="10" max="250" /> degrees per second
+                                    <input type="number" id="magHoldYawRate" class="rate-tpa_input" step="5" min="10" max="250" /> degrees per second
                                     <div class="helpicon cf_tip" i18n_title="pidTuningMagHoldYawRateHelp"></div>
                                 </td>
                             </tr>
@@ -182,14 +180,14 @@
                             <tr>
                                 <th i18n="yawJumpPreventionLimit"></th>
                                 <td >
-                                    <input type="number" id="yawJumpPreventionLimit" step="10" min="80" max="500" />
+                                    <input type="number" id="yawJumpPreventionLimit" class="rate-tpa_input" step="10" min="80" max="500" />
                                     <div class="helpicon cf_tip" i18n_title="yawJumpPreventionLimitHelp"></div>
                                 </td>
                             </tr>
                             <tr>
                                 <th i18n="yawPLimit"></th>
                                 <td >
-                                    <input type="number" id="yawPLimit" step="10" min="100" max="500" />
+                                    <input type="number" id="yawPLimit" class="rate-tpa_input" step="10" min="100" max="500" />
                                     <div class="helpicon cf_tip" i18n_title="yawPLimitHelp"></div>
                                 </td>
                             </tr>
@@ -224,9 +222,9 @@
                         </thead>
                         <tbody>
                             <tr>
-                                <td class="leftzero"><input type="number" name="tpa" step="0.01" min="0"
+                                <td class="leftzero"><input class="rate-tpa_input" type="number" name="tpa" step="0.01" min="0"
                                     max="1.00" /></td>
-                                <td class="tpa-breakpoint"><input type="number" name="tpa-breakpoint" step="10"
+                                <td class="tpa-breakpoint"><input class="rate-tpa_input" type="number" name="tpa-breakpoint" step="10"
                                     min="1000" max="2000" /></td>
                             </tr>
                         </tbody>
@@ -235,7 +233,43 @@
             </div>
         </form>
         <div class="clear-both"></div>
+        <div class="tab_title requires-v1_4" i18n="tabFiltering" style="margin-top: 1em;"></div>
+        <div class="cf_column half requires-v1_4">
+            <table class="rate-tpa rate-tpa--filtering">
+                <tbody>
+                    <tr>
+                        <th i18n="gyroLpfCutoffFrequency"></th>
+                        <td>
+                            <input type="number" id="gyroSoftLpfHz" class="rate-tpa_input" step="1" min="0" max="200" /> Hz
+                            <div class="helpicon cf_tip" i18n_title="gyroLpfCutoffFrequencyHelp"></div>
+                        </td>
+                    </tr>
+                    <tr>
+                        <th i18n="accLpfCutoffFrequency"></th>
+                        <td>
+                            <input type="number" id="accSoftLpfHz" class="rate-tpa_input" step="1" min="0" max="200" /> Hz
+                            <div class="helpicon cf_tip" i18n_title="accLpfCutoffFrequencyHelp"></div>
+                        </td>
+                    </tr>
+                    <tr>
+                        <th i18n="dtermLpfCutoffFrequency"></th>
+                        <td>
+                            <input type="number" id="dtermLpfHz" class="rate-tpa_input" step="1" min="0" max="200" /> Hz
+                            <div class="helpicon cf_tip" i18n_title="dtermLpfCutoffFrequencyHelp"></div>
+                        </td>
+                    </tr>
+                    <tr>
+                        <th i18n="yawLpfCutoffFrequency"></th>
+                        <td>
+                            <input type="number" id="yawLpfHz" class="rate-tpa_input" step="1" min="0" max="200" /> Hz
+                            <div class="helpicon cf_tip" i18n_title="yawLpfCutoffFrequencyHelp"></div>
+                        </td>
+                    </tr>
+                </tbody>
+            </table>
+        </div>
     </div>
+    <div class="clear-both"></div>
     <div class="content_toolbar">
         <div class="btn save_btn">
             <a class="update" href="#" i18n="pidTuningButtonSave"></a>

tabs/pid_tuning.js

@@ -34,7 +34,7 @@ TABS.pid_tuning.initialize = function (callback) {
     }
 
     function loadPidAdvanced() {
-        var next_callback = load_html;
+        var next_callback = loadFilterConfig;
         if (semver.gte(CONFIG.flightControllerVersion, "1.4.0")) {
             MSP.send_message(MSP_codes.MSP_PID_ADVANCED, false, false, next_callback);
         } else {
@@ -42,6 +42,15 @@ TABS.pid_tuning.initialize = function (callback) {
         }
     }
 
+    function loadFilterConfig() {
+        var next_callback = load_html;
+        if (semver.gte(CONFIG.flightControllerVersion, "1.4.0")) {
+            MSP.send_message(MSP_codes.MSP_FILTER_CONFIG, false, false, next_callback);
+        } else {
+            next_callback();
+        }
+    }
+
 
     function load_html() {
         $('#content').load("./tabs/pid_tuning.html", process_html);
@@ -182,14 +191,21 @@ TABS.pid_tuning.initialize = function (callback) {
         }
 
         if (semver.gte(CONFIG.flightControllerVersion, "1.4.0")) {
-
             var $magHoldYawRate         = $("#magHoldYawRate"),
                 $yawJumpPreventionLimit = $('#yawJumpPreventionLimit'),
-                $yawPLimit              = $('#yawPLimit');
+                $yawPLimit              = $('#yawPLimit'),
+                $gyroSoftLpfHz          = $('#gyroSoftLpfHz'),
+                $accSoftLpfHz           = $('#accSoftLpfHz'),
+                $dtermLpfHz             = $('#dtermLpfHz'),
+                $yawLpfHz               = $('#yawLpfHz');
 
             $magHoldYawRate.val(INAV_PID_CONFIG.magHoldRateLimit);
             $yawJumpPreventionLimit.val(INAV_PID_CONFIG.yawJumpPreventionLimit);
             $yawPLimit.val(PID_ADVANCED.yawPLimit);
+            $gyroSoftLpfHz.val(FILTER_CONFIG.gyroSoftLpfHz);
+            $accSoftLpfHz.val(INAV_PID_CONFIG.accSoftLpfHz);
+            $dtermLpfHz.val(FILTER_CONFIG.dtermLpfHz);
+            $yawLpfHz.val(FILTER_CONFIG.yawLpfHz);
 
             $magHoldYawRate.change(function () {
                 INAV_PID_CONFIG.magHoldRateLimit = parseInt($magHoldYawRate.val(), 10);
@@ -201,7 +217,23 @@ TABS.pid_tuning.initialize = function (callback) {
 
             $yawPLimit.change(function () {
                 PID_ADVANCED.yawPLimit = parseInt($yawPLimit.val(), 10);
-            })
+            });
+
+            $gyroSoftLpfHz.change(function () {
+                FILTER_CONFIG.gyroSoftLpfHz = parseInt($gyroSoftLpfHz.val(), 10);
+            });
+
+            $accSoftLpfHz.change(function () {
+                INAV_PID_CONFIG.accSoftLpfHz = parseInt($accSoftLpfHz.val(), 10);
+            });
+
+            $dtermLpfHz.change(function () {
+                FILTER_CONFIG.dtermLpfHz = parseInt($dtermLpfHz.val(), 10);
+            });
+
+            $yawLpfHz.change(function () {
+                FILTER_CONFIG.yawLpfHz = parseInt($yawLpfHz.val(), 10);
+            });
 
             $('.requires-v1_4').show();
         } else {
@@ -238,7 +270,7 @@ TABS.pid_tuning.initialize = function (callback) {
             }
 
             function savePidAdvanced() {
-                var next_callback = save_to_eeprom;
+                var next_callback = saveFilterConfig;
                 if(semver.gte(CONFIG.flightControllerVersion, "1.4.0")) {
                    MSP.send_message(MSP_codes.MSP_SET_PID_ADVANCED, MSP.crunch(MSP_codes.MSP_SET_PID_ADVANCED), false, next_callback);
                 } else {
@@ -246,6 +278,15 @@ TABS.pid_tuning.initialize = function (callback) {
                 }
             }
 
+            function saveFilterConfig() {
+                var next_callback = save_to_eeprom;
+                if(semver.gte(CONFIG.flightControllerVersion, "1.4.0")) {
+                   MSP.send_message(MSP_codes.MSP_SET_FILTER_CONFIG, MSP.crunch(MSP_codes.MSP_SET_FILTER_CONFIG), false, next_callback);
+                } else {
+                   next_callback();
+                }
+            }
+
             function save_to_eeprom() {
                 MSP.send_message(MSP_codes.MSP_EEPROM_WRITE, false, false, function () {
                     GUI.log(chrome.i18n.getMessage('pidTuningEepromSaved'));