Improve sensor handling and add configuration for sonar and opticalflow (#4327)

haslinghuis · web-flow · commit 5c4d3da7ffab · 2025-02-05T17:47:44.000+01:00
* Add configuration for sonar and opticalflow

* Add opticalflow info and put sensortypes in separate file

* Add new file

* did not add this :)

* rename

* update comment

* Do not duplicate messages

* Fix sonar

* Update messages

* Add some helper functions

* Add gps protocol types

* Add opticalflow to sensor_helpers
diff --git a/locales/en/messages.json b/locales/en/messages.json
@@ -947,11 +947,11 @@
     },
     "initialSetupNotInBuild": {
         "message": "Not included in build",
-        "description": "Message that pops up when hardware support not are supported in Build"
+        "description": "Message that pops up when hardware support is not included in build"
     },
     "initialSetupNotDetected": {
-        "message": "Not detected",
-        "description": "Message that pops up when hardware are not detected"
+        "message": "Not detected or disabled",
+        "description": "Message that pops up when hardware is not detected or disabled"
     },
     "initialSetupRSSIValue": {
         "message": "$1 dBm"
@@ -980,6 +980,9 @@
     "initialSetupSensorSonar": {
         "message": "Sonar:"
     },
+    "initialSetupSensorOpticalflow": {
+        "message": "Optical Flow:"
+    },
     "initialSetupSensorRadar": {
         "message": "Radar:"
     },
@@ -1408,6 +1411,24 @@
     "configurationMagDeclinationHelp": {
         "message": "The magnetic declination is the angle between magnetic north and true north. It is different for every location on earth. You can find the declination for your location on the internet"
     },
+    "configurationRangefinder": {
+        "message": "$t(initialSetupSensorSonar.message)"
+    },
+    "configurationRangefinderHelp": {
+        "message": "Enable rangefinder to measure distance to ground in cm"
+    },
+    "configurationRangefinderType": {
+        "message": "Type"
+    },
+    "configurationOpticalflow": {
+        "message": "$t(initialSetupSensorOpticalflow.message)"
+    },
+    "configurationOpticalflowHelp": {
+        "message": "Enable optical flow sensor"
+    },
+    "configurationOpticalflowType": {
+        "message": "$t(configurationRangefinderType.message)"
+    },
     "configurationBoardAlignment": {
         "message": "Board Alignment"
     },
diff --git a/src/js/fc.js b/src/js/fc.js
@@ -592,6 +592,7 @@ const FC = {
             baro_hardware: 0,
             mag_hardware: 0,
             sonar_hardware: 0,
+            opticalflow_hardware: 0,
         };
 
         this.SENSOR_CONFIG_ACTIVE = { gyro_hardware: 0, ...this.SENSOR_CONFIG };
diff --git a/src/js/msp/MSPHelper.js b/src/js/msp/MSPHelper.js
@@ -1209,6 +1209,9 @@ MspHelper.prototype.process_data = function (dataHandler) {
                     if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_46)) {
                         FC.SENSOR_CONFIG.sonar_hardware = data.readU8();
                     }
+                    if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
+                        FC.SENSOR_CONFIG.opticalflow_hardware = data.readU8();
+                    }
                     break;
                 case MSPCodes.MSP2_SENSOR_CONFIG_ACTIVE:
                     FC.SENSOR_CONFIG_ACTIVE.gyro_hardware = data.readU8();
@@ -1218,6 +1221,9 @@ MspHelper.prototype.process_data = function (dataHandler) {
                     if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_46)) {
                         FC.SENSOR_CONFIG_ACTIVE.sonar_hardware = data.readU8();
                     }
+                    if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
+                        FC.SENSOR_CONFIG_ACTIVE.opticalflow_hardware = data.readU8();
+                    }
                     break;
                 case MSPCodes.MSP2_MCU_INFO:
                     if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
@@ -2206,10 +2212,13 @@ MspHelper.prototype.crunch = function (code, modifierCode = undefined) {
             buffer.push16(FC.ADVANCED_TUNING.tpaBreakpoint);
             break;
         case MSPCodes.MSP_SET_SENSOR_CONFIG:
-            buffer
-                .push8(FC.SENSOR_CONFIG.acc_hardware)
-                .push8(FC.SENSOR_CONFIG.baro_hardware)
-                .push8(FC.SENSOR_CONFIG.mag_hardware);
+            buffer.push8(FC.SENSOR_CONFIG.acc_hardware);
+            buffer.push8(FC.SENSOR_CONFIG.baro_hardware);
+            buffer.push8(FC.SENSOR_CONFIG.mag_hardware);
+            if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
+                buffer.push8(FC.SENSOR_CONFIG.sonar_hardware);
+                buffer.push8(FC.SENSOR_CONFIG.opticalflow_hardware);
+            }
             break;
 
         case MSPCodes.MSP_SET_NAME:
diff --git a/src/js/sensor_helpers.js b/src/js/sensor_helpers.js
@@ -15,6 +15,8 @@ export function have_sensor(sensors_detected, sensor_code) {
             return bit_check(sensors_detected, 4);
         case "gyro":
             return bit_check(sensors_detected, 5);
+        case "opticalflow":
+            return bit_check(sensors_detected, 6);
     }
     return false;
 }
diff --git a/src/js/sensor_types.js b/src/js/sensor_types.js
@@ -0,0 +1,139 @@
+import semver from "semver";
+import FC from "./fc";
+import { API_VERSION_1_47 } from "./data_storage";
+
+export function sensorTypes() {
+    const sensorTypes = {
+        acc: {
+            name: "Accelerometer",
+            elements: [
+                "AUTO",
+                "NONE",
+                "ADXL345",
+                "MPU6050",
+                "MMA8452",
+                "BMA280",
+                "LSM303DLHC",
+                "MPU6000",
+                "MPU6500",
+                "MPU9250",
+                "ICM20601",
+                "ICM20602",
+                "ICM20608G",
+                "ICM20649",
+                "ICM20689",
+                "ICM42605",
+                "ICM42688P",
+                "BMI160",
+                "BMI270",
+                "LSM6DSO",
+                "LSM6DSV16X",
+                "VIRTUAL",
+            ],
+        },
+        gyro: {
+            name: "Gyroscope",
+            elements: [
+                "AUTO",
+                "NONE",
+                "MPU6050",
+                "L3G4200D",
+                "MPU3050",
+                "L3GD20",
+                "MPU6000",
+                "MPU6500",
+                "MPU9250",
+                "ICM20601",
+                "ICM20602",
+                "ICM20608G",
+                "ICM20649",
+                "ICM20689",
+                "ICM42605",
+                "ICM42688P",
+                "BMI160",
+                "BMI270",
+                "LSM6DSO",
+                "LSM6DSV16X",
+                "VIRTUAL",
+            ],
+        },
+        baro: {
+            name: "Barometer",
+            elements: [
+                "DEFAULT",
+                "NONE",
+                "BMP085",
+                "MS5611",
+                "BMP280",
+                "LPS",
+                "QMP6988",
+                "BMP388",
+                "DPS310",
+                "2SMPB_02B",
+                "VIRTUAL",
+            ],
+        },
+        mag: {
+            name: "Magnetometer",
+            elements: [
+                "DEFAULT",
+                "NONE",
+                "HMC5883",
+                "AK8975",
+                "AK8963",
+                "QMC5883",
+                "LIS2MDL",
+                "LIS3MDL",
+                "MPU925X_AK8963",
+                "IST8310",
+            ],
+        },
+        gps: {
+            name: "GPS",
+            elements: ["NMEA", "UBLOX", "MSP"],
+        },
+        sonar: {
+            name: "Sonar",
+            elements: ["NONE", "HCSR04", "TFMINI", "TF02", "MTF01", "MTF02", "MTF01P", "MTF02P"],
+        },
+        opticalflow: {
+            name: "Optical Flow",
+            elements: ["NONE", "MT"],
+        },
+    };
+
+    function removeElement(elements, element) {
+        const index = elements.indexOf(element);
+        if (index !== -1) {
+            elements.splice(index, 1);
+        }
+    }
+
+    function addElement(elements, element, afterElement) {
+        const elementIndex = elements.indexOf(element);
+        if (elementIndex === -1) {
+            elements.splice(elements.indexOf(afterElement) + 1, 0, element);
+        }
+    }
+
+    const gyroElements = sensorTypes.gyro.elements;
+    const accElements = sensorTypes.acc.elements;
+    const gpsElements = sensorTypes.gps.elements;
+
+    // remove deprecated sensors or add new ones
+    if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
+        removeElement(gyroElements, "L3G4200D");
+        removeElement(gyroElements, "MPU3050");
+        addElement(gyroElements, "IIM42653", "LSM6DSV16X");
+
+        removeElement(accElements, "ADXL345");
+        removeElement(accElements, "MMA8452");
+        removeElement(accElements, "BMA280");
+        removeElement(accElements, "LSM303DLHC");
+        addElement(accElements, "IIM42653", "LSM6DSV16X");
+
+        addElement(gpsElements, "VIRTUAL", "MSP");
+    }
+
+    return sensorTypes;
+}
diff --git a/src/js/tabs/configuration.js b/src/js/tabs/configuration.js
@@ -10,6 +10,7 @@ import { API_VERSION_1_45, API_VERSION_1_46, API_VERSION_1_47 } from "../data_st
 import { updateTabList } from "../utils/updateTabList";
 import $ from "jquery";
 import { have_sensor } from "../sensor_helpers";
+import { sensorTypes } from "../sensor_types";
 
 const configuration = {
     analyticsChanges: {},
@@ -157,6 +158,36 @@ configuration.initialize = function (callback) {
             toggleMagCustomAlignmentInputs();
         });
 
+        // Range finder
+
+        if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
+            const rangeFinderType_e = $("select.rangefinderType");
+            const sonarElements = sensorTypes().sonar.elements;
+
+            for (let i = 0; i < sonarElements.length; i++) {
+                rangeFinderType_e.append(`<option value="${i}">${sonarElements[i]}</option>`);
+            }
+
+            rangeFinderType_e.val(FC.SENSOR_CONFIG.sonar_hardware);
+        } else {
+            $(".tab-configuration .rangefinder").parent().hide();
+        }
+
+        // Optical flow sensor
+
+        if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
+            const opticalflowType_e = $("select.opticalflowType");
+            const opticalflowElements = sensorTypes().opticalflow.elements;
+
+            for (let i = 0; i < opticalflowElements.length; i++) {
+                opticalflowType_e.append(`<option value="${i}">${opticalflowElements[i]}</option>`);
+            }
+
+            opticalflowType_e.val(FC.SENSOR_CONFIG.opticalflow_hardware);
+        } else {
+            $(".tab-configuration .opticalflow").parent().hide();
+        }
+
         // Multi gyro config
 
         const GYRO_DETECTION_FLAGS = {
@@ -442,6 +473,11 @@ configuration.initialize = function (callback) {
                 FC.COMPASS_CONFIG.mag_declination = $('input[name="mag_declination"]').val();
             }
 
+            if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
+                FC.SENSOR_CONFIG.sonar_hardware = $("select.rangefinderType").val();
+                FC.SENSOR_CONFIG.opticalflow_hardware = $("select.opticalflowType").val();
+            }
+
             FC.ARMING_CONFIG.small_angle = parseInt($('input[id="configurationSmallAngle"]').val());
 
             FC.SENSOR_ALIGNMENT.gyro_to_use = parseInt(orientation_gyro_to_use_e.val());
diff --git a/src/js/tabs/gps.js b/src/js/tabs/gps.js
@@ -1,6 +1,6 @@
 import { i18n } from "../localization";
 import semver from "semver";
-import { API_VERSION_1_46, API_VERSION_1_47 } from "../data_storage";
+import { API_VERSION_1_46 } from "../data_storage";
 import GUI, { TABS } from "../gui";
 import FC from "../fc";
 import MSP from "../msp";
@@ -12,6 +12,7 @@ import { updateTabList } from "../utils/updateTabList";
 import { initMap } from "./map";
 import { fromLonLat } from "ol/proj";
 import { ispConnected } from "../utils/connection";
+import { sensorTypes } from "../sensor_types";
 
 const gps = {};
 
@@ -88,11 +89,7 @@ gps.initialize = async function (callback) {
         checkUpdateGpsControls();
 
         // generate GPS
-        const gpsProtocols = ["NMEA", "UBLOX", "MSP"];
-
-        if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) {
-            gpsProtocols.push("VIRTUAL");
-        }
+        const gpsProtocols = sensorTypes().gps.elements;
 
         const gpsBaudRates = ["115200", "57600", "38400", "19200", "9600"];
 
diff --git a/src/js/tabs/setup.js b/src/js/tabs/setup.js
diff --git a/src/tabs/configuration.html b/src/tabs/configuration.html
diff --git a/src/tabs/setup.html b/src/tabs/setup.html

Original file line number	Diff line number	Diff line change
`@@ -15,6 +15,8 @@ export function have_sensor(sensors_detected, sensor_code) {`
`15`	`15`	`return bit_check(sensors_detected, 4);`
`16`	`16`	`case "gyro":`
`17`	`17`	`return bit_check(sensors_detected, 5);`
	`18`	`+ case "opticalflow":`
	`19`	`+ return bit_check(sensors_detected, 6);`
`18`	`20`	`}`
`19`	`21`	`return false;`
`20`	`22`	`}`