wip

input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.ino

+//
+// hello.LSM6DSV16X.fusion.ino
+//    LSM6DSV16X IMU sensor fusion hello-world
+//
+// Neil Gershenfeld 5/18/25
+//
+// This work may be reproduced, modified, distributed,
+// performed, and displayed for any purpose, but must
+// acknowledge this project. Copyright is retained and
+// must be preserved. The work is provided as is; no
+// warranty is provided, and users accept all liability.
+//
+#include <Wire.h>
+#define address 0x6B
+//
+// I2C register write
+//
+void I2C_write_reg(uint8_t addr,uint8_t reg,uint8_t value) {
+   Wire.beginTransmission(addr);
+   Wire.write(reg);
+   Wire.write(value);
+   if (Wire.endTransmission() != 0)
+      Serial.println("I2C_write_reg failure");
+   }
+//
+// I2C register read
+//
+uint8_t I2C_read_reg(uint8_t addr,uint8_t reg) {
+   Wire.beginTransmission(addr);
+   Wire.write(reg);
+   if (Wire.endTransmission() != 0)
+      Serial.println("I2C_read_reg failure");
+   Wire.requestFrom(0x6B,1);
+   return Wire.read();
+   }
+//
+// Converts uint16_t half-precision number into a uint32_t single-precision number
+// from: https://github.com/stm32duino/LSM6DSV16X/blob/main/src/LSM6DSV16XSensor.cpp
+//
+void npy_halfbits_to_floatbits(uint16_t h,uint32_t *f) {
+  uint16_t h_exp,h_sig;
+  uint32_t f_sgn,f_exp,f_sig;
+  h_exp = (h & 0x7c00u);
+  f_sgn = ((uint32_t)h & 0x8000u) << 16;
+  switch (h_exp) {
+    case 0x0000u: /* 0 or subnormal */
+      h_sig = (h & 0x03ffu);
+      /* Signed zero */
+      if (h_sig == 0) {
+        *f = f_sgn;
+        return;
+        }
+      /* Subnormal */
+      h_sig <<= 1;
+      while ((h_sig & 0x0400u) == 0) {
+        h_sig <<= 1;
+        h_exp++;
+        }
+      f_exp = ((uint32_t)(127-15-h_exp)) << 23;
+      f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13;
+      *f = f_sgn + f_exp + f_sig;
+      return;
+    case 0x7c00u: /* inf or NaN */
+      /* All-ones exponent and a copy of the significand */
+      *f = f_sgn+0x7f800000u+(((uint32_t)(h & 0x03ffu)) << 13);
+      return;
+    default: /* normalized */
+      /* Just need to adjust the exponent and shift */
+      *f = f_sgn+(((uint32_t)(h & 0x7fffu)+0x1c000u) << 13);
+      return;
+    }
+  }
+//
+// Converts uint16_t half-precision number into a float single-precision number
+// from: https://github.com/stm32duino/LSM6DSV16X/blob/main/src/LSM6DSV16XSensor.cpp
+//
+void npy_half_to_float(uint16_t h,float *f) {
+  union {
+    float ret;
+    uint32_t retbits;
+    } conv;
+  npy_halfbits_to_floatbits(h,&conv.retbits);
+  *f = conv.ret;
+  }
+//
+// Compute quaternions
+// from: https://github.com/stm32duino/LSM6DSV16X/blob/main/src/LSM6DSV16XSensor.cpp
+//
+void sflp2q(float *qx,float *qy,float *qz,float *qw,uint16_t x,uint16_t y,uint16_t z) {
+  float sumsq = 0;
+  npy_half_to_float(x,qx);
+  npy_half_to_float(y,qy);
+  npy_half_to_float(z,qz);
+  sumsq = (*qx)*(*qx)+(*qy)*(*qy)+(*qz)*(*qz);
+  if (sumsq > 1.0f) {
+    float n = sqrtf(sumsq);
+    *qx /= n;
+    *qy /= n;
+    *qz /= n;
+    sumsq = 1.0f;
+    }
+  *qw = sqrtf(1.0f-sumsq);
+  }
+//
+// IMU integer read
+//
+void IMU_read_int(uint8_t *tag,int16_t *x,int16_t *y,int16_t *z) {
+   uint8_t xlo,xhi,ylo,yhi,zlo,zhi;
+   *tag = I2C_read_reg(address,0x78) >> 3; // FIFO_DATA_OUT_TAG
+   xlo = I2C_read_reg(address,0x79); // FIFO_DATA_OUT_X_L
+   xhi = I2C_read_reg(address,0x7A); // FIFO_DATA_OUT_X_H
+   *x = (xhi << 8) | xlo;
+   ylo = I2C_read_reg(address,0x7B); // FIFO_DATA_OUT_Y_L
+   yhi = I2C_read_reg(address,0x7C); // FIFO_DATA_OUT_Y_H
+   *y = (yhi << 8) | ylo;
+   zlo = I2C_read_reg(address,0x7D); // FIFO_DATA_OUT_Z_L
+   zhi = I2C_read_reg(address,0x7E); // FIFO_DATA_OUT_Z_H
+   *z = (zhi << 8) | zlo;
+   }
+//
+// IMU float read
+//
+void IMU_read_quaternion(uint8_t *tag,float *qx,float *qy,float *qz,float *qw) {
+   uint8_t xlo,xhi,ylo,yhi,zlo,zhi;
+   uint16_t ix,iy,iz;
+   *tag = I2C_read_reg(address,0x78) >> 3; // FIFO_DATA_OUT_TAG
+   xlo = I2C_read_reg(address,0x79); // FIFO_DATA_OUT_X_L
+   xhi = I2C_read_reg(address,0x7A); // FIFO_DATA_OUT_X_H
+   ix = (xhi << 8) | xlo;
+   ylo = I2C_read_reg(address,0x7B); // FIFO_DATA_OUT_Y_L
+   yhi = I2C_read_reg(address,0x7C); // FIFO_DATA_OUT_Y_H
+   iy = (yhi << 8) | ylo;
+   zlo = I2C_read_reg(address,0x7D); // FIFO_DATA_OUT_Z_L
+   zhi = I2C_read_reg(address,0x7E); // FIFO_DATA_OUT_Z_H
+   iz = (zhi << 8) | zlo;
+   sflp2q(qx,qy,qz,qw,ix,iy,iz);
+   }
+//
+// setup
+//
+void setup() {
+   //
+   // start serial
+   //
+   Serial.begin(115200);
+   delay(1000);
+   //
+   // start I2C
+   //
+   Wire.begin();
+   Wire.setClock(1000000);
+   // 
+   // initialize gyro and accelerometer
+   //
+   I2C_write_reg(address,0x10,0x06); // CTRL1: accel on, 120 Hz, high performance
+   I2C_write_reg(address,0x11,0x06); // CTRL2: gyro on, 120 Hz, high performance
+   I2C_write_reg(address,0x12,0x44); // CTRL3: block data update, auto inc addresses
+   I2C_write_reg(address,0x13,0x00); // CTRL4: for interrupts
+   I2C_write_reg(address,0x14,0x00); // CTRL5: for interrupts
+   I2C_write_reg(address,0x15,0x04); // CTRL6: gyro 2000 dps
+   I2C_write_reg(address,0x16,0x00); // CTRL7: analog hub
+   I2C_write_reg(address,0x17,0x01); // CTRL8: accel 4g
+   I2C_write_reg(address,0x18,0x00); // CTRL9: accel filter
+   I2C_write_reg(address,0x19,0x00); // CTRL10: self test
+   //
+   // initialize FIFO
+   //
+   I2C_write_reg(address,0x01,0x80);  // FUNC_CFG_ACCESS: enable embedded access
+   I2C_write_reg(address,0x04,0b00000010); // EMB_FUNC_EN_A: SFLP_GAME_EN
+   I2C_write_reg(address,0x44,0b00010010); // EMB_FUNC_FIFO_EN_A: 0b00010000 gravity 0b00000010 game
+   I2C_write_reg(address,0x01,0x00);  // FUNC_CFG_ACCESS: exit embedded access
+   I2C_write_reg(address,0x05,0); // EMB_FUNC_EN_B: embedded functions
+   I2C_write_reg(address,0x07,0); // FIFO_CTRL1: watermark
+   I2C_write_reg(address,0x08,0); // FIFO_CTRL2: modes
+   I2C_write_reg(address,0x09,0b01100110); // FIFO_CTRL3:  0 accel gyro not batched 0b01100110 120 Hz gyro and accel batch
+   I2C_write_reg(address,0x0A,0b00000110); // FIFO_CTRL4: continuous mode
+   I2C_write_reg(address,0x5E,0b01011011); // SFLP_ODR: 120 Hz sensor fusion output data rate
+   }
+//
+// main loop
+//
+void loop() {
+   uint8_t tag;
+   int16_t x,y,z;
+   float qx,qy,qz,qw;
+   //
+   // read accelerometer
+   //
+   while (true) {
+      IMU_read_int(&tag,&x,&y,&z);
+      if (tag == 2) {
+         Serial.print("ax ay az: ");
+         Serial.print(x);
+         Serial.print(",");
+         Serial.print(y);
+         Serial.print(",");
+         Serial.print(z);
+         Serial.println("");
+         break;
+         }
+      }
+   //
+   // read gyro
+   //
+   while (true) {
+      IMU_read_int(&tag,&x,&y,&z);
+      if (tag == 1) {
+         Serial.print("rx ry rz: ");
+         Serial.print(x);
+         Serial.print(",");
+         Serial.print(y);
+         Serial.print(",");
+         Serial.print(z);
+         Serial.println("");
+         break;
+         }
+      }
+   //
+   // read gravity
+   //
+   while (true) {
+      IMU_read_int(&tag,&x,&y,&z);
+      if (tag == 0x17) {
+         Serial.print("gx gy gz: ");
+         Serial.print(x);
+         Serial.print(",");
+         Serial.print(y);
+         Serial.print(",");
+         Serial.print(z);
+         Serial.println("");
+         break;
+         }
+      }
+   //
+   // read quaternions
+   //
+   while (true) {
+      IMU_read_quaternion(&tag,&qx,&qy,&qz,&qw);
+      if (tag == 0x13) {
+         Serial.print("qw qx qy qz: ");
+         Serial.print(qw);
+         Serial.print(",");
+         Serial.print(qx);
+         Serial.print(",");
+         Serial.print(qy);
+         Serial.print(",");
+         Serial.print(qz);
+         Serial.println("");
+         break;
+         }
+      }
+   }

input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.py

+#
+# hello.LSM6DSV16X.fusion.py
+#    LSM6DSV16X IMU hello-world
+#
+# Neil Gershenfeld 5/19/25
+#
+# This work may be reproduced, modified, distributed,
+# performed, and displayed for any purpose, but must
+# acknowledge this project. Copyright is retained and
+# must be preserved. The work is provided as is; no
+# warranty is provided, and users accept all liability.
+#
+from tkinter import *
+import serial,sys,math
+#
+WIDTH = 750 # window width
+NBARS = 12 # number of bar graphs
+BARHEIGHT = 70 # bar graph height
+BORDER = 10 # bar graph border
+HEIGHT = NBARS*BARHEIGHT # window height
+FONTSIZE = 28 # font size
+EPS = 0.2 # smoothing filter
+filter = {}
+#
+def idle(parent,canvas):
+   #
+   # read a line
+   #
+   line = serialport.readline().decode('utf-8').rstrip()
+   #
+   # parse and plot
+   #
+   line = line.split(':')
+   match line[0]:
+      case "ax ay az":
+         try:
+            line = line[1].split(',')
+            line = list(map(int,line))
+            update_bar_graph("ax",line[0],-10000,10000)
+            update_bar_graph("ay",line[1],-10000,10000)
+            update_bar_graph("az",line[2],-10000,10000)
+         except:
+            pass
+      case "gx gy gz":
+         try:
+            line = line[1].split(',')
+            line = list(map(int,line))
+            gx = line[0]
+            gy = line[1]
+            gz = line[2]
+            norm = math.sqrt(gx*gx+gy*gy+gz*gz)
+            gx /= norm
+            gy /= norm
+            gz /= norm
+            update_bar_graph("gx",gx,-1,1)
+            update_bar_graph("gy",gy,-1,1)
+            update_bar_graph("gz",gz,-1,1)
+         except:
+            pass
+      case "rx ry rz":
+         try:
+            line = line[1].split(',')
+            line = list(map(int,line))
+            update_bar_graph("rx",line[0],-10000,10000)
+            update_bar_graph("ry",line[1],-10000,10000)
+            update_bar_graph("rz",line[2],-10000,10000)
+         except:
+            pass
+      case "qw qx qy qz":
+         try:
+            line = line[1].split(',')
+            line = list(map(float,line))
+            qw = line[0]
+            qx = line[1]
+            qy = line[2]
+            qz = line[3]
+            roll = math.atan2(
+               2*qy*qw-2*qx*qz,
+               1-2*qy*qy-2*qz*qz)
+            yaw = math.asin(
+               2*qx*qy+2*qz*qw)
+            pitch = math.atan2(
+               2*qx*qw-2*qy*qz,
+               1-2*qx*qx-2*qz*qz)
+            yaw = 180*yaw/math.pi
+            pitch = 180*pitch/math.pi
+            roll = 180*roll/math.pi
+            update_bar_graph("yaw",yaw,-90,90)
+            update_bar_graph("pitch",pitch,-180,180)
+            update_bar_graph("roll",roll,-180,180)
+         except:
+            pass
+   #
+   # update canvas
+   #
+   canvas.update()
+   parent.after_idle(idle,parent,canvas)
+#
+def update_bar_graph(label,value,vmin,vmax):
+   filt = EPS*value+(1-EPS)*filter[label]
+   filter[label] = filt
+   canvas.itemconfigure(label,text=f"{label}: {filt:.1f}")
+   tuple = canvas.coords(label+'r')
+   tuple[2] = BORDER+(WIDTH-2*BORDER)*(filt-vmin)/(vmax-vmin)
+   canvas.coords(label+'r',tuple)
+#
+def create_bar_graph(label,n):
+   canvas.create_rectangle(BORDER,n*BARHEIGHT+BORDER,WIDTH-BORDER,(n+1)*BARHEIGHT-BORDER,tags=label+'b', fill='#0000b0')
+   canvas.create_rectangle(BORDER,n*BARHEIGHT+BORDER,BORDER,(n+1)*BARHEIGHT-BORDER,tags=label+'r', fill='#b00000')
+   canvas.create_text(WIDTH/2,(n+0.5)*BARHEIGHT,text=label,font=("Helvetica",FONTSIZE),tags=label,fill="#FFFFFF",anchor=CENTER)
+   filter[label] = 0
+#
+#  check command line arguments
+#
+if (len(sys.argv) != 2):
+   print("command line: hello.LSM6DSV16X.fusion.py serial_port")
+   sys.exit()
+port = sys.argv[1]
+#
+# open serial port
+#
+serialport = serial.Serial(port,115200)
+#
+# set up GUI
+#
+root = Tk()
+root.title('hello.LSM6DSV16X.fusion.py (q to exit)')
+root.bind('q','exit')
+canvas = Canvas(root,width=WIDTH,height=HEIGHT,background='white')
+#
+create_bar_graph("ax",0)
+create_bar_graph("ay",1)
+create_bar_graph("az",2)
+#
+create_bar_graph("gx",3)
+create_bar_graph("gy",4)
+create_bar_graph("gz",5)
+#
+create_bar_graph("rx",6)
+create_bar_graph("ry",7)
+create_bar_graph("rz",8)
+#
+create_bar_graph("roll",9)
+create_bar_graph("pitch",10)
+create_bar_graph("yaw",11)
+#
+canvas.pack()
+#
+# start idle loop
+#
+root.after(100,idle,root,canvas)
+root.mainloop()
+

input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.raw.ino

+//
+// hello.LSM6DSV16X.raw.ino
+//
+// LSM6DSV16X IMU raw data hello-world
+//
+// Neil Gershenfeld 5/18/25
+//
+// This work may be reproduced, modified, distributed,
+// performed, and displayed for any purpose, but must
+// acknowledge this project. Copyright is retained and
+// must be preserved. The work is provided as is; no
+// warranty is provided, and users accept all liability.
+//
+
+#include <Wire.h>
+
+#define address 0x6B
+
+void I2C_write_reg(uint8_t addr,uint8_t reg,uint8_t value) {
+   Wire.beginTransmission(addr);
+   Wire.write(reg);
+   Wire.write(value);
+   if (Wire.endTransmission() != 0)
+      Serial.println("I2C_write_reg failure");
+   }
+
+uint8_t I2C_read_reg(uint8_t addr,uint8_t reg) {
+   Wire.beginTransmission(addr);
+   Wire.write(reg);
+   if (Wire.endTransmission() != 0)
+      Serial.println("I2C_read_reg failure");
+   Wire.requestFrom(0x6B,1);
+   return Wire.read();
+   }
+
+void setup() {
+   //
+   // start serial
+   //
+   Serial.begin(115200);
+   delay(1000);
+   //
+   // start I2C
+   //
+   Wire.begin();
+   Wire.setClock(1000000);
+   //
+   // initialize LSM6DSV16X
+   //
+   Serial.println("initialize LSM6DSV16X");
+   I2C_write_reg(address,0x10,0x07); // CTRL1 accel on, 240 Hz, high performance
+   I2C_write_reg(address,0x11,0x07); // CTRL2 gyro on, 240 Hz, high performance
+   I2C_write_reg(address,0x12,0x44); // CTRL3 block data update, auto inc addresses
+   I2C_write_reg(address,0x13,0x00); // CTRL4 for interrupts
+   I2C_write_reg(address,0x14,0x00); // CTRL5 for interrupts
+   I2C_write_reg(address,0x15,0x04); // CTRL6 gyro 2000 dps
+   I2C_write_reg(address,0x16,0x00); // CTRL7 analog hub
+   I2C_write_reg(address,0x17,0x01); // CTRL8 accel 4g
+   I2C_write_reg(address,0x18,0x00); // CTRL9 accel filter
+   I2C_write_reg(address,0x19,0x00); // CTRL10 self test
+   }
+
+void loop() {
+   uint8_t ret,lo,hi;
+   int16_t ax,ay,az,rx,ry,rz;
+   //
+   // check status register
+   //
+   ret = I2C_read_reg(address,0x1E); // STATUS_REG
+   //
+   // gyro data?
+   //
+   if (ret & 2) { // GDA
+      Serial.print("rx ry rz: ");
+      lo = I2C_read_reg(address,0x22); //OUTX_L_G
+      hi = I2C_read_reg(address,0x23); //OUTX_H_G
+      rx = (hi << 8) | lo;
+      Serial.print(rx);
+      Serial.print(" ");
+      lo = I2C_read_reg(address,0x24); //OUTY_L_G
+      hi = I2C_read_reg(address,0x25); //OUTY_H_G
+      ry = (hi << 8) | lo;
+      Serial.print(ry);
+      Serial.print(" ");
+      lo = I2C_read_reg(address,0x26); //OUTZ_L_G
+      hi = I2C_read_reg(address,0x27); //OUTZ_H_G
+      rz = (hi << 8) | lo;
+      Serial.print(rz);
+      Serial.println(" ");
+      }
+   //
+   // accelerometer data?
+   //
+   if (ret & 1) { // XLDA
+      Serial.print("ax ay az: ");
+      lo = I2C_read_reg(address,0x28); //OUTX_L_A
+      hi = I2C_read_reg(address,0x29); //OUTX_H_A
+      ax = (hi << 8) | lo;
+      Serial.print(ax);
+      Serial.print(" ");
+      lo = I2C_read_reg(address,0x2A); //OUTY_L_A
+      hi = I2C_read_reg(address,0x2B); //OUTY_H_A
+      ay = (hi << 8) | lo;
+      Serial.print(ay);
+      Serial.print(" ");
+      lo = I2C_read_reg(address,0x2C); //OUTZ_L_A
+      hi = I2C_read_reg(address,0x2D); //OUTZ_H_A
+      az = (hi << 8) | lo;
+      Serial.print(az);
+      Serial.println("");
+      }
+   //
+   // delay
+   //
+   delay(100);
+   }

input_devices/index.html

@@ -143,6 +143,7 @@
       <a href=https://www.st.com/en/mems-and-sensors/lsm6dsv16x.html>LSM6DSV16X</a>
          hello.LSM6DSV16X.RP2040 <a href=imu/LSM6DSV16X/hello.LSM6DSV16X.RP2040.kicad_pro>pro</a> <a href=imu/LSM6DSV16X/hello.LSM6DSV16X.RP2040.kicad_sch>sch</a> <a href=imu/LSM6DSV16X/hello.LSM6DSV16X.RP2040.kicad_pcb>pcb</a> <a href=imu/LSM6DSV16X/hello.LSM6DSV16X.RP2040.png>board</a> <a href=imu/LSM6DSV16X/hello.LSM6DSV16X.RP2040.jpg>components</a> <a href=imu/LSM6DSV16X/production/traces.png>traces</a> <a href=imu/LSM6DSV16X/production/interior.png>interior</a>
             <a href=https://github.com/stm32duino/LSM6DSV16X>library</a> <a href=imu/LSM6DSV16X/hello.LSM6DSV16X.RP2040.mp4>video</a>
+            <a href=imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.ino>hello.LSM6DSV16X.fusion.ino</a> <a href=imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.mp4>video</a>
    9 axis accelerometer+gyroscope+magnetometer
       <a href=https://www.digikey.com/en/products/detail/ceva-technologies-inc/BNO085/9445940>BNO085</a> <a href=https://www.adafruit.com/product/4754>module</a> <a href=https://www.digikey.com/en/products/detail/ceva-technologies-inc/BNO086/14114190>BNO086</a>
          <a href=https://learn.adafruit.com/adafruit-9-dof-orientation-imu-fusion-breakout-bno085?view=all>RVC serial</a>