qqqlab · qqqlab · Mar 7, 2026 · Mar 7, 2026
diff --git a/src/cfg/cfg.h b/src/cfg/cfg.h
@@ -205,7 +205,7 @@ SOFTWARE.
   MF_PARAM( gps_baud,          0, int32_t, 'i') \
 \
   /*IMU - Inertial Measurement Unit (acc/gyro)*/ \
-  MF_PARAM( imu_gizmo,         0, int32_t, 'e', mf_NONE,mf_BMI270,mf_MPU6000,mf_MPU6050,mf_MPU6500,mf_MPU9150,mf_MPU9250,mf_ICM45686,mf_ICM42688,mf_ICM42688P,mf_AUTO,mf_LSM6DSV,mf_LSM6DSO) \
+  MF_PARAM( imu_gizmo,         0, int32_t, 'e', mf_NONE,mf_BMI270,mf_MPU6000,mf_MPU6050,mf_MPU6500,mf_MPU9150,mf_MPU9250,mf_ICM45686,mf_ICM42688,mf_ICM42688P,mf_AUTO,mf_LSM6DSV,mf_LSM6DSO,mf_LSM6DSV16B) \
   MF_PARAM( imu_spi_bus,      -1, int32_t, 'i') \
   MF_PARAM( imu_i2c_bus,      -1, int32_t, 'i') \
   MF_PARAM( imu_i2c_adr,       0, int32_t, 'i') \

diff --git a/src/imu/ImuGizmoAuto.h b/src/imu/ImuGizmoAuto.h
@@ -29,6 +29,7 @@ SOFTWARE.
 #include "ImuGizmoICM45686.h"
 #include "ImuGizmoICM426XX.h"
 #include "ImuGizmoLSM6DSV.h"
+#include "ImuGizmoLSM6DSV16B.h"
 #include "ImuGizmoLSM6DSO.h"
 #include "ImuGizmoBMI270.h"
 #include "ImuGizmoMPUXXXX.h"
@@ -49,6 +50,7 @@ class ImuGizmoAuto : public ImuGizmo {
           if(!gizmo) gizmo = ImuGizmoICM45686::create(config, state);
           if(!gizmo) gizmo = ImuGizmoICM426XX::create(config, state);
           if(!gizmo) gizmo = ImuGizmoLSM6DSV::create(config, state);
+          if(!gizmo) gizmo = ImuGizmoLSM6DSV16B::create(config, state);
           if(!gizmo) gizmo = ImuGizmoLSM6DSO::create(config, state);
           if(!gizmo) gizmo = ImuGizmoBMI270::create(config, state);
           if(!gizmo) gizmo = ImuGizmoMPUXXXX::create(config, state);

diff --git a/src/imu/ImuGizmoLSM6DSV16B.h b/src/imu/ImuGizmoLSM6DSV16B.h
@@ -0,0 +1,104 @@
+/*==========================================================================================
+MIT License
+
+Copyright (c) 2026 https://madflight.com
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+===========================================================================================*/
+
+#pragma once
+
+#include "imu.h"
+#include "LSM6DSV16B/LSM6DSV16B.h"
+
+class ImuGizmoLSM6DSV16B : public ImuGizmo {
+  private:
+    ImuGizmoLSM6DSV16B() {} //private constructor
+    ImuState *state = nullptr;
+
+  public:
+    const char* name() override {return "LSM6DSV16B";}
+    LSM6DSV16B *sensor = nullptr;
+    SensorDevice *dev = nullptr;
+
+    ~ImuGizmoLSM6DSV16B() {
+        delete sensor;
+        delete dev;
+    }
+
+    static ImuGizmo* create(ImuConfig *config, ImuState *state) {
+      if(!config || !state) return nullptr;
+
+      // Create SensorDevice
+      SensorDevice *dev = SensorDevice::createImuDevice(config);
+      if(!dev) return nullptr;
+
+      // Detect sensor, exit without printing anything
+      if (!LSM6DSV16B::detect(dev)) {
+          delete dev;
+          return nullptr;
+      }
+
+      // Create sensor
+      auto *sensor = new LSM6DSV16B();
+      int rv = sensor->begin(dev);
+      if(rv < 0) {
+          delete sensor;
+          delete dev;
+          Serial.printf("IMU: LSM6DSV16B init failed, rv=%d\n", rv);
+          return nullptr;
+      }
+
+      // Create gizmo
+      auto gizmo = new ImuGizmoLSM6DSV16B();
+      gizmo->state = state;
+      gizmo->sensor = sensor;
+      gizmo->dev = dev;
+      //return config
+      config->sample_rate = sensor->actual_sample_rate_hz;
+      Serial.printf("IMU: LSM6DSV16B started, sample_rate:%d\n", sensor->actual_sample_rate_hz);
+      return gizmo;
+  }
+
+/* Get sensor data in NED frame
+   x=North (forward), y=East (right), z=Down 
+   acc: gravitation force is positive in axis direction: 
+        [ax,ay,az] nose-down:[1,0,0], right-down:[0,1,0], level:[0,0,1]
+   gyr: direction of positive rotation by right hand rule: 
+        [gx,gy,gz] roll-right:[positive,0,0], pitch-up:[0,positive,0], yaw-right:[0,0,positive]
+
+LSM6DSV has NWU orientation
+
+  * +--+         Y
+    |  | --> X   ^   Z-up
+    +--+         |
+
+*/
+
+  void getMotion6NED(float *ax, float *ay, float *az, float *gx, float *gy, float *gz) override {
+    int16_t raw[6];
+    sensor->readraw(raw);
+    *gx =  raw[0] * sensor->gyr_scale; // N =  N
+    *gy = -raw[1] * sensor->gyr_scale; // E = -W
+    *gz = -raw[2] * sensor->gyr_scale; // D = -U
+    *ax = -raw[5] * sensor->acc_scale; //-N = -N
+    *ay =  raw[4] * sensor->acc_scale; //-E =  W
+    *az =  raw[3] * sensor->acc_scale; //-D =  U
+  }
+};
diff --git a/src/imu/LSM6DSV/LSM6DSV.cpp b/src/imu/LSM6DSV/LSM6DSV.cpp
@@ -45,43 +45,46 @@ int LSM6DSV::begin(SensorDevice* dev) {
     // Autoincrement register address when doing block SPI reads and update continuously
     dev->writeReg(LSM6DSV_CTRL3, LSM6DSV_CTRL3_IF_INC | LSM6DSV_CTRL3_BDU);      /*BDU bit need to be set*/
 
-    // Select high-accuracy ODR mode 1
+    // Select high-accuracy ODR mode 1 - 0x62 = 0x01
+    // Note: HAODR mode has to be enabled / disabled when the device is in power-down mode.
     dev->writeReg(LSM6DSV_HAODR_CFG,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_HAODR_MODE1,
                                     LSM6DSV_HAODR_CFG_HAODR_SEL_MASK,
                                     LSM6DSV_HAODR_CFG_HAODR_SEL_SHIFT));
 
-    // Enable the accelerometer in high accuracy
+    // Enable the accelerometer in high accuracy - 0x10 = 0x10
+    // Note: HAODR mode has to be enabled / disabled when the device is in power-down mode.
     dev->writeReg(LSM6DSV_CTRL1,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_OP_MODE_XL_HIGH_ACCURACY,
                                     LSM6DSV_CTRL1_OP_MODE_XL_MASK,
                                     LSM6DSV_CTRL1_OP_MODE_XL_SHIFT));
 
-    // Enable the gyro in high accuracy
+    // Enable the gyro in high accuracy - 0x11 = 0x10
+    // Note: HAODR mode has to be enabled / disabled when the device is in power-down mode.
     dev->writeReg(LSM6DSV_CTRL2,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_OP_MODE_G_HIGH_ACCURACY,
                                     LSM6DSV_CTRL2_OP_MODE_G_MASK,
                                     LSM6DSV_CTRL2_OP_MODE_G_SHIFT));
 
-    // Enable 16G sensitivity
+    // Enable 16G sensitivity - 0x17 = 0x03
     dev->writeReg(LSM6DSV_CTRL8,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL8_FS_XL_16G,
                                     LSM6DSV_CTRL8_FS_XL_MASK,
                                     LSM6DSV_CTRL8_FS_XL_SHIFT));
 
-    // Enable the accelerometer odr at 1kHz
+    // Enable the accelerometer odr at 1kHz - 0x10 = 0x09
     dev->writeReg(LSM6DSV_CTRL1,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_ODR_XL_1000HZ,
                                     LSM6DSV_CTRL1_ODR_XL_MASK,
                                     LSM6DSV_CTRL1_ODR_XL_SHIFT));
 
-    // Enable the gyro odr at 1kHz
+    // Enable the gyro odr at 1kHz - 0x11 = 0x09
     dev->writeReg(LSM6DSV_CTRL2,
                 LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_ODR_G_1000HZ,
                                     LSM6DSV_CTRL2_ODR_G_MASK,
                                     LSM6DSV_CTRL2_ODR_G_SHIFT));
 
-    // Set the LPF1 filter bandwidth - Enable 2000 deg/s sensitivity and selected LPF1 filter setting
+    // Set the LPF1 filter bandwidth - Enable 2000 deg/s sensitivity and selected LPF1 filter setting - 0x15 = 0x04
 /*  filter options
     [LPF_NORMAL] = LSM6DSV_CTRL6_FS_G_BW_288HZ,
     [LPF_OPTION_1] = LSM6DSV_CTRL6_FS_G_BW_157HZ,
@@ -96,13 +99,13 @@ int LSM6DSV::begin(SensorDevice* dev) {
                                     LSM6DSV_CTRL6_FS_G_MASK,
                                     LSM6DSV_CTRL6_FS_G_SHIFT));
 
-    // Enable the gyro digital LPF1 filter
+    // Enable the gyro digital LPF1 filter - 0x16 = 0x01
     dev->writeReg(LSM6DSV_CTRL7, LSM6DSV_CTRL7_LPF1_G_EN);
 
-    // Generate pulse on interrupt line, not requiring a read to clear
+    // Generate pulse on interrupt line, not requiring a read to clear - 0x13 = 0x02
     dev->writeReg(LSM6DSV_CTRL4, LSM6DSV_CTRL4_DRDY_PULSED);
 
-    // Enable the INT1 output to interrupt when new gyro data is ready
+    // Enable the INT1 output to interrupt when new gyro data is ready - 0x0D = 0x02
     dev->writeReg(LSM6DSV_INT1_CTRL, LSM6DSV_INT1_CTRL_INT1_DRDY_G);
 
     // Set full speed
@@ -112,5 +115,5 @@ int LSM6DSV::begin(SensorDevice* dev) {
 }
 
 void LSM6DSV::readraw(int16_t *raw) {
-  dev->readRegs(LSM6DSV_OUTX_L_G, (uint8_t*)raw, 12); //ax,ay,az,gx,gy,gz little endian, no byte juggling needed for RP2,ESP32,STM32
+  dev->readRegs(LSM6DSV_OUTX_L_G, (uint8_t*)raw, 12); //reg 0x22 - gx,gy,gz,ax,ay,az little endian, no byte juggling needed for RP2,ESP32,STM32
 }
diff --git a/src/imu/LSM6DSV/LSM6DSV.h b/src/imu/LSM6DSV/LSM6DSV.h
@@ -39,7 +39,7 @@ class LSM6DSV {
     return (dev->readReg(0x0F) == 0x70);
   }
   int begin(SensorDevice* dev); //returns negative error code, positive warning code, or 0 on success
-  void readraw(int16_t *raw); //read gyr[3],acc[3]
+  void readraw(int16_t *raw); //read gx,gy,gz,ax,ay,az
 
   SensorDevice* dev = nullptr;
 };
diff --git a/src/imu/LSM6DSV16B/LSM6DSV16B.cpp b/src/imu/LSM6DSV16B/LSM6DSV16B.cpp
@@ -0,0 +1,78 @@
+/*==========================================================================================
+MIT License
+
+Copyright (c) 2026 https://madflight.com
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+===========================================================================================*/
+
+//Driver for LSM6DSV16B, LSM6DSV16BX gyroscope/accelometer
+
+#include "LSM6DSV16B.h"
+
+//returns negative error code, positive warning code, or 0 on success
+int LSM6DSV16B::begin(SensorDevice* dev) { 
+    this->dev = dev;
+    dev->setLowSpeed(); // Use low speed for setup
+
+    // Check who-am-i
+    if (!detect(dev)) return -1;
+
+    // Perform a software reset
+    dev->writeReg(0x12, 0x01); //CTRL3, SW_RESET
+
+    // Wait for the device to be ready
+    while (dev->readReg(0x12) & 0x01) {} //CTRL3, SW_RESET
+
+    // Autoincrement register address when doing block SPI reads and update continuously
+    dev->writeReg(0x12, 0x44);  //BDU bit need to be set - CTRL3, IF_INC | BDU
+
+    // Acc 16G sensitivity, LPF1 ODR/2, LPF2 ODR/4 (in high-performance mode)
+    dev->writeReg(0x17, 0x03); //CTRL8, FS_XL, HP_LPF2_XL_BW
+
+    // Enable acc LPF2 filter
+    dev->writeReg(0x16, 0x08); //CTRL9, LPF2_XL_EN
+
+    // Gyro 2000 deg/s sensitivity, LPF1 240Hz (@960kHz ODR)
+    dev->writeReg(0x15, 0x04); //CTRL6, FS_G, LPF1_G_BW
+
+    // Enable gyro LPF1 filter
+    dev->writeReg(0x16, 0x01); //CTRL7, LPF1_G_EN
+
+    // Acc ODR 960Hz in high-performance mode
+    dev->writeReg(0x10, 0x09); //CTRL1, OP_MODE_XL, ODR_XL
+
+    // Gyro ODR 960Hz in high-performance mode
+    dev->writeReg(0x11, 0x09); //CTRL2, OP_MODE_G, ODR_G
+
+    // Generate pulse on interrupt line, not requiring a read to clear
+    dev->writeReg(0x13, 0x02); //CTRL4, DRDY_PULSED
+
+    // Enable the INT1 output to interrupt when new gyro data is ready
+    dev->writeReg(0x0D, 0x02); //INT1_CTRL, INT1_DRDY_G
+
+    // Set full speed
+    dev->setFreq(10000000); //10MHz
+
+    return 0;
+}
+
+void LSM6DSV16B::readraw(int16_t *raw) {
+  dev->readRegs(0x22, (uint8_t*)raw, 12); //LSM6DSV_OUTX_L_G - gx,gy,gz,az,ay,ax little endian, no byte juggling needed for RP2,ESP32,STM32
+}
diff --git a/src/imu/LSM6DSV16B/LSM6DSV16B.h b/src/imu/LSM6DSV16B/LSM6DSV16B.h
@@ -0,0 +1,44 @@
+/*==========================================================================================
+MIT License
+
+Copyright (c) 2026 https://madflight.com
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+===========================================================================================*/
+
+//Driver for LSM6DSV16B, LSM6DSV16BX gyroscope/accelometer
+
+#pragma once
+
+#include <SPI.h>
+#include "../common/SensorDevice.h"
+
+class LSM6DSV16B {
+public:
+  const int actual_sample_rate_hz = 1000; //MF_TODO: allow variable sample rate
+  const float acc_scale = 1.0 / 2048.0; //Accel scale +/-16g, 16bit, 2048 LSB/g
+  const float gyr_scale = 0.070; // From datasheet: 70mdps/LSB for FS = ±2000 dps (so, actual FS = ±2294)
+
+  static bool detect(SensorDevice* dev) {
+    return (dev->readReg(0x0F) == 0x71);
+  }
+  int begin(SensorDevice* dev); //returns negative error code, positive warning code, or 0 on success
+  void readraw(int16_t *raw); //read gx,gy,gz,az,ay,ax - note swapped az,ax
+  SensorDevice* dev = nullptr;
+};
diff --git a/src/imu/imu.cpp b/src/imu/imu.cpp
@@ -107,6 +107,10 @@ int Imu::setup() {
       gizmo = ImuGizmoLSM6DSO::create(&config, (ImuState*)this);
       break;
     }
+    case Cfg::imu_gizmo_enum::mf_LSM6DSV16B : {
+      gizmo = ImuGizmoLSM6DSV16B::create(&config, (ImuState*)this);
+      break;
+    }
   }
 
   //check gizmo