Revive MSP message for mag declination (#3676)

* Add back mag declination * Conditionally allow mag configuration
betaflight · Dec 19, 2023 · 4679e33 · 4679e33
1 parent 2fed3a0
commit 4679e33
Show file tree

Hide file tree

Showing 8 changed files with 73 additions and 7 deletions.
diff --git a/locales/en/messages.json b/locales/en/messages.json
@@ -863,6 +863,9 @@
     "initialSetupMagHead": {
         "message": "Magnetometer"
     },
+    "initialSetupMagDeclination": {
+        "message": "Mag Declination:"
+    },
     "initialSetupInfoHead": {
         "message": "System info"
     },
@@ -1362,6 +1365,9 @@
     "configurationArmingHelp": {
         "message": "Some Arming options may require accelerometer be enabled"
     },
+    "configurationMagDeclination": {
+        "message": "Magnetometer Declination [deg]"
+    },
     "configurationReverseMotorSwitch": {
         "message": "Motor direction is reversed"
     },

diff --git a/src/js/backup_restore.js b/src/js/backup_restore.js
@@ -9,7 +9,7 @@ import FC from "./fc";
 import { mspHelper } from "./msp/MSPHelper";
 import MSP from "./msp";
 import MSPCodes from "./msp/MSPCodes";
-import CONFIGURATOR, { API_VERSION_1_41, API_VERSION_1_45 } from "./data_storage";
+import CONFIGURATOR, { API_VERSION_1_41, API_VERSION_1_45, API_VERSION_1_46 } from "./data_storage";
 import { gui_log } from './gui_log';
 import { generateFilename } from "./utils/generate_filename";
 import semver from "semver";
@@ -117,6 +117,9 @@ export function configuration_backup(callback) {
         uniqueData.push(MSPCodes.MSP_MOTOR_CONFIG);
         uniqueData.push(MSPCodes.MSP_RSSI_CONFIG);
         uniqueData.push(MSPCodes.MSP_GPS_CONFIG);
+        if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_46)) {
+            uniqueData.push(MSPCodes.MSP_COMPASS_CONFIG);
+        }
         uniqueData.push(MSPCodes.MSP_FEATURE_CONFIG);
         uniqueData.push(MSPCodes.MSP_MODE_RANGES_EXTRA);
     }
@@ -161,6 +164,9 @@ export function configuration_backup(callback) {
                 configuration.FEATURE_CONFIG = jQuery.extend(true, {}, FC.FEATURE_CONFIG);
                 configuration.MOTOR_CONFIG = jQuery.extend(true, {}, FC.MOTOR_CONFIG);
                 configuration.GPS_CONFIG = jQuery.extend(true, {}, FC.GPS_CONFIG);
+                if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_46)) {
+                    configuration.COMPASS_CONFIG = jQuery.extend(true, {}, FC.COMPASS_CONFIG);
+                }
                 configuration.BEEPER_CONFIG = jQuery.extend(true, {}, FC.BEEPER_CONFIG);
                 configuration.MODE_RANGES_EXTRA = jQuery.extend(true, [], FC.MODE_RANGES_EXTRA);
 
@@ -803,6 +809,7 @@ export function configuration_restore(callback) {
                     uniqueData.push(MSPCodes.MSP_SET_FEATURE_CONFIG);
                     uniqueData.push(MSPCodes.MSP_SET_MOTOR_CONFIG);
                     uniqueData.push(MSPCodes.MSP_SET_GPS_CONFIG);
+                    uniqueData.push(MSPCodes.MSP_SET_COMPASS_CONFIG);
                     uniqueData.push(MSPCodes.MSP_SET_RSSI_CONFIG);
                 }
 
@@ -823,6 +830,9 @@ export function configuration_restore(callback) {
                     FC.FEATURE_CONFIG = configuration.FEATURE_CONFIG;
                     FC.MOTOR_CONFIG = configuration.MOTOR_CONFIG;
                     FC.GPS_CONFIG = configuration.GPS_CONFIG;
+                    if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_46)) {
+                        FC.COMPASS_CONFIG = configuration.COMPASS_CONFIG;
+                    }
                     FC.RSSI_CONFIG = configuration.RSSI_CONFIG;
                     FC.BOARD_ALIGNMENT_CONFIG = configuration.BOARD_ALIGNMENT_CONFIG;
                     if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_45)) {

diff --git a/src/js/fc.js b/src/js/fc.js
@@ -122,6 +122,7 @@ const FC = {
     FEATURE_CONFIG: null,
     FILTER_CONFIG: null,
     GPS_CONFIG: null,
+    COMPASS_CONFIG: null,
     GPS_DATA: null,
     GPS_RESCUE: null,
     LED_COLORS: null,
@@ -373,6 +374,10 @@ const FC = {
             ublox_use_galileo:          0,
         };
 
+        this.COMPASS_CONFIG = {
+            mag_declination:            0,
+        };
+
         this.RSSI_CONFIG = {
             channel:                    0,
         };

diff --git a/src/js/msp/MSPCodes.js b/src/js/msp/MSPCodes.js
@@ -107,7 +107,7 @@ const MSPCodes = {
     MSP_BATTERY_STATE:              130,
     MSP_MOTOR_CONFIG:               131,
     MSP_GPS_CONFIG:                 132,
-// Removed:   MSP_COMPASS_CONFIG:             133,
+    MSP_COMPASS_CONFIG:             133,
     MSP_GPS_RESCUE:                 135,
 
     MSP_VTXTABLE_BAND:              137,
@@ -160,7 +160,7 @@ const MSPCodes = {
     MSP_SET_LED_STRIP_MODECOLOR:    221,
     MSP_SET_MOTOR_CONFIG:           222,
     MSP_SET_GPS_CONFIG:             223,
-// Removed:    MSP_SET_COMPASS_CONFIG:         224,
+    MSP_SET_COMPASS_CONFIG:         224,
     MSP_SET_GPS_RESCUE:             225,
 
     MSP_SET_VTXTABLE_BAND:          227,

diff --git a/src/js/msp/MSPHelper.js b/src/js/msp/MSPHelper.js
@@ -498,6 +498,9 @@ MspHelper.prototype.process_data = function(dataHandler) {
                     FC.MOTOR_CONFIG.use_esc_sensor = data.readU8() != 0;
                 }
                 break;
+            case MSPCodes.MSP_COMPASS_CONFIG:
+                FC.COMPASS_CONFIG.mag_declination = data.read16() / 10;
+                break;
             case MSPCodes.MSP_GPS_CONFIG:
                 FC.GPS_CONFIG.provider = data.readU8();
                 FC.GPS_CONFIG.ublox_sbas = data.readU8();
@@ -1839,6 +1842,9 @@ MspHelper.prototype.crunch = function(code, modifierCode = undefined) {
                 buffer.push16(FC.GPS_RESCUE.initialClimbM);
             }
             break;
+        case MSPCodes.MSP_SET_COMPASS_CONFIG:
+            buffer.push16(Math.round(FC.COMPASS_CONFIG.mag_declination) * 10);
+            break;
         case MSPCodes.MSP_SET_RSSI_CONFIG:
             buffer.push8(FC.RSSI_CONFIG.channel);
             break;

diff --git a/src/js/tabs/failsafe.js b/src/js/tabs/failsafe.js
@@ -62,7 +62,15 @@ failsafe.initialize = function (callback) {
     }
 
     function load_motor_config() {
-        MSP.send_message(MSPCodes.MSP_MOTOR_CONFIG, false, false, load_gps_config);
+        MSP.send_message(MSPCodes.MSP_MOTOR_CONFIG, false, false, load_compass_config);
+    }
+
+    function load_compass_config() {
+        if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_46)) {
+            MSP.send_message(MSPCodes.MSP_COMPASS_CONFIG, false, false, load_gps_config);
+        } else {
+            load_gps_config();
+        }
     }
 
     function load_gps_config() {

diff --git a/src/js/tabs/gps.js b/src/js/tabs/gps.js
@@ -17,9 +17,15 @@ gps.initialize = async function (callback) {
     GUI.active_tab = 'gps';
 
     await MSP.promise(MSPCodes.MSP_FEATURE_CONFIG);
-    await MSP.promise(MSPCodes.MSP_GPS_CONFIG);
 
-    const hasMag = have_sensor(FC.CONFIG.activeSensors, 'mag');
+    // mag support added in 1.46
+    const hasMag = have_sensor(FC.CONFIG.activeSensors, 'mag') && semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_46);
+
+    if (hasMag) {
+        await MSP.promise(MSPCodes.MSP_COMPASS_CONFIG);
+    }
+
+    await MSP.promise(MSPCodes.MSP_GPS_CONFIG);
 
     load_html();
 
@@ -56,7 +62,15 @@ gps.initialize = async function (callback) {
         }
 
         function get_attitude_data() {
-            MSP.send_message(MSPCodes.MSP_ATTITUDE, false, false, hasMag ? get_imu_data : update_ui);
+            MSP.send_message(MSPCodes.MSP_ATTITUDE, false, false, load_compass_config);
+        }
+
+        function load_compass_config() {
+            if (hasMag) {
+                MSP.send_message(MSPCodes.MSP_COMPASS_CONFIG, false, false, get_imu_data);
+            } else {
+                get_imu_data();
+            }
         }
 
         function get_imu_data() {
@@ -183,6 +197,13 @@ gps.initialize = async function (callback) {
         gpsBaudrateElement.prop("disabled", true);
         gpsBaudrateElement.parent().hide();
 
+        // fill magnetometer
+        if (hasMag) {
+            $('input[name="mag_declination"]').val(FC.COMPASS_CONFIG.mag_declination.toFixed(1));
+        } else {
+            $('div.mag_declination').hide();
+        }
+
         // End GPS Configuration
 
         function update_ui() {
@@ -382,9 +403,14 @@ gps.initialize = async function (callback) {
             // fill some data
             FC.GPS_CONFIG.auto_baud = $('input[name="gps_auto_baud"]').is(':checked') ? 1 : 0;
             FC.GPS_CONFIG.auto_config = $('input[name="gps_auto_config"]').is(':checked') ? 1 : 0;
+            FC.COMPASS_CONFIG.mag_declination = parseFloat($('input[name="mag_declination"]').val());
 
             await MSP.promise(MSPCodes.MSP_SET_FEATURE_CONFIG, mspHelper.crunch(MSPCodes.MSP_SET_FEATURE_CONFIG));
+            if (hasMag) {
+                await MSP.promise(MSPCodes.MSP_SET_COMPASS_CONFIG, mspHelper.crunch(MSPCodes.MSP_SET_COMPASS_CONFIG));
+            }
             await MSP.promise(MSPCodes.MSP_SET_GPS_CONFIG, mspHelper.crunch(MSPCodes.MSP_SET_GPS_CONFIG));
+
             mspHelper.writeConfiguration(true);
         });
 

diff --git a/src/tabs/gps.html b/src/tabs/gps.html
@@ -59,6 +59,11 @@
                                     </select>
                                     <span i18n="configurationGPSubxSbas"></span>
                                 </div>
+                                <div class="number mag_declination">
+                                    <label> <input type="number" name="mag_declination" step="0.1" min="0" max="359.9" />
+                                    <span i18n="configurationMagDeclination"></span>
+                                    </label>
+                                </div>
                             </div>
                         </div>
                     </div>