diff --git a/input_devices/imu/LSM6DSV16X/DM00851721.pdf b/input_devices/imu/LSM6DSV16X/DM00851721.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..ffc542ddaf48d4d9a14bad9bfa382d3be5a2c3b3
Binary files /dev/null and b/input_devices/imu/LSM6DSV16X/DM00851721.pdf differ
diff --git a/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.ino b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.ino
new file mode 100644
index 0000000000000000000000000000000000000000..35c585f2b5f4ff607ba1ab19513309d6149b007a
--- /dev/null
+++ b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.ino
@@ -0,0 +1,252 @@
+//
+// 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;
+ }
+ }
+ }
diff --git a/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.mp4 b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.mp4
new file mode 100644
index 0000000000000000000000000000000000000000..b3e74462f86954630f6a4e40658544276f6ae500
Binary files /dev/null and b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.mp4 differ
diff --git a/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.py b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.py
new file mode 100644
index 0000000000000000000000000000000000000000..ea567b2607399c7c79b069836e9a9f640a966c9a
--- /dev/null
+++ b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.fusion.py
@@ -0,0 +1,153 @@
+#
+# 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()
+
diff --git a/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.raw.ino b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.raw.ino
new file mode 100644
index 0000000000000000000000000000000000000000..14c875a8d98e47e0d6ef2eb87c618d553d2cba54
--- /dev/null
+++ b/input_devices/imu/LSM6DSV16X/hello.LSM6DSV16X.raw.ino
@@ -0,0 +1,116 @@
+//
+// 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);
+ }
diff --git a/input_devices/index.html b/input_devices/index.html
index 4393162240d0416cbed3302fec5ef2e8ffc6440a..81a3abfd9c55a9e29ddbe21eae2ec0e81b1c5512 100755
--- a/input_devices/index.html
+++ b/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>