From 084c81386045ea42dabaa949a5c784586e0c4691 Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Mon, 15 Jun 2026 11:02:09 +0900
Subject: [PATCH 1/9] =?UTF-8?q?=E3=83=90=E3=82=A4=E3=83=8A=E3=83=AA?=
 =?UTF-8?q?=E5=87=BA=E5=8A=9B=E5=BD=A2=E5=BC=8F=E3=81=AB=E5=A4=89=E6=9B=B4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../RT-9DOF-IMU-V3-Quat.ino                   | 441 ++++++++++++------
 1 file changed, 304 insertions(+), 137 deletions(-)

diff --git a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
index 42785eb..aad29d0 100644
--- a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
+++ b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
@@ -8,10 +8,10 @@
  * Based on original code by:
  * Owen Lyke @ SparkFun Electronics
  * Original Creation Date: April 17 2019
- * 
+ *
  * ** This example is based on InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
  * ** We are grateful to InvenSense for sharing this with us.
- * 
+ *
  * ** Important note: by default the DMP functionality is disabled in the library
  * ** as the DMP firmware takes up 14301 Bytes of program memory.
  * ** To use the DMP, you will need to:
@@ -26,32 +26,44 @@
  * Distributed as-is; no warranty is given.
  ***************************************************************/
 
+#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU // should install v1.2.5
 
-#include "ICM_20948.h"  // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU // should install v1.2.5
-
-#define USE_SPI  // Uncomment this to use SPI
+#define USE_SPI // Uncomment this to use SPI
 
 #define SERIAL_PORT Serial
 
-#define SPI_PORT SPI  // Your desired SPI port.       Used only when "USE_SPI" is defined
-#define SPI_FREQ 5000000  // You can override the default SPI frequency
-#define CS_PIN D4     // Which pin you connect CS to. Used only when "USE_SPI" is defined
+#define SPI_PORT SPI     // Your desired SPI port.       Used only when "USE_SPI" is defined
+#define SPI_FREQ 5000000 // You can override the default SPI frequency
+#define CS_PIN D4        // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
-#define WIRE_PORT Wire  // Your desired Wire port.      Used when "USE_SPI" is not defined
+#define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
 // The value of the last bit of the I2C address.
 // On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
 #define AD0_VAL 1
 
 #ifdef USE_SPI
-ICM_20948_SPI myICM;  // If using SPI create an ICM_20948_SPI object
+ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
 #else
-ICM_20948_I2C myICM;  // Otherwise create an ICM_20948_I2C object
+ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
 #endif
 
 #define LED_BUILTIN PA15
 HardwareTimer *Timer6 = new HardwareTimer(TIM6);
 
-void timerInterrupt(void) {
+#define FW_VERSION 0x01
+
+uint8_t cal_checksum(const uint8_t *data, size_t size)
+{
+  uint8_t sum = 0;
+  for (size_t i = 0; i < size; i++)
+  {
+    sum += data[i];
+  }
+  return sum;
+}
+
+void timerInterrupt(void)
+{
 
   // Read any DMP data waiting in the FIFO
   // Note:
@@ -62,56 +74,98 @@ void timerInterrupt(void) {
   //    readDMPdataFromFIFO will return ICM_20948_Stat_FIFOMoreDataAvail if a valid frame was read _and_ the FIFO contains more (unread) data.
   icm_20948_DMP_data_t data;
   myICM.readDMPdataFromFIFO(&data);
+  uint8_t raw_data[33];
+  static uint8_t time_stamp = 0;
 
-  if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail))  // Was valid data available?
+  if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) // Was valid data available?
   {
-    if ((data.header & DMP_header_bitmap_Quat6) > 0)// && (data.header & DMP_header_bitmap_Gyro) > 0)  
+    if ((data.header & DMP_header_bitmap_Quat6) > 0) // && (data.header & DMP_header_bitmap_Gyro) > 0)
     {
-      double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0;  // Convert to double. Divide by 2^30
-      double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0;  // Convert to double. Divide by 2^30
-      double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0;  // Convert to double. Divide by 2^30
-      double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
-
-      double gx = ((double)data.Raw_Gyro.Data.X) / 16.3835 * PI / 180.0; // [rad/s] 16.3835=32767/2000   deg2rad 
-      double gy = ((double)data.Raw_Gyro.Data.Y) / 16.3835 * PI / 180.0;
-      double gz = ((double)data.Raw_Gyro.Data.Z) / 16.3835 * PI / 180.0;
-
-      uint32_t time_sec = micros()/1000000;
-      uint32_t time_subsecs = micros() - time_sec*1000000;
-
-      SERIAL_PORT.print(time_sec);
-      SERIAL_PORT.print(F("."));
-      SERIAL_PORT.print(time_subsecs);
-      SERIAL_PORT.print(F(","));
-      SERIAL_PORT.print(q1, 4);
-      SERIAL_PORT.print(F(","));
-      SERIAL_PORT.print(q2, 4);
-      SERIAL_PORT.print(F(","));
-      SERIAL_PORT.print(q3, 4);
-      SERIAL_PORT.print(F(","));
-      SERIAL_PORT.print(q0, 4);
-      SERIAL_PORT.print(F(","));
-      SERIAL_PORT.print(gx, 4);
-      SERIAL_PORT.print(F(","));
-      SERIAL_PORT.print(gy, 4);
-      SERIAL_PORT.print(F(","));
-      SERIAL_PORT.print(gz, 4);
-      SERIAL_PORT.println(F(""));
-
+      // header
+      raw_data[0] = 0xff;       // header1
+      raw_data[1] = 0xff;       // header2
+      raw_data[2] = 0x52;       // ASCII 'R'
+      raw_data[3] = 0x54;       // ASCII 'T'
+      raw_data[4] = 0x40;       // device ID_L
+      raw_data[5] = 0x41;       // device ID_H
+      raw_data[6] = FW_VERSION; // FW version
+
+      // data
+      raw_data[7] = time_stamp++;
+      raw_data[8] = (data.Raw_Accel.Data.X >> 8) & 0xFF;
+      raw_data[9] = (data.Raw_Accel.Data.X) & 0xFF;
+      raw_data[10] = (data.Raw_Accel.Data.Y >> 8) & 0xFF;
+      raw_data[11] = (data.Raw_Accel.Data.Y) & 0xFF;
+      raw_data[12] = (data.Raw_Accel.Data.Z >> 8) & 0xFF;
+      raw_data[13] = (data.Raw_Accel.Data.Z) & 0xFF;
+
+      raw_data[14] = (data.Raw_Gyro.Data.X >> 8) & 0xFF;
+      raw_data[15] = (data.Raw_Gyro.Data.X) & 0xFF;
+      raw_data[16] = (data.Raw_Gyro.Data.Y >> 8) & 0xFF;
+      raw_data[17] = (data.Raw_Gyro.Data.Y) & 0xFF;
+      raw_data[18] = (data.Raw_Gyro.Data.Z >> 8) & 0xFF;
+      raw_data[19] = (data.Raw_Gyro.Data.Z) & 0xFF;
+
+      raw_data[20] = (data.Quat6.Data.Q1 >> 0) & 0xFF;
+      raw_data[21] = (data.Quat6.Data.Q1 >> 8) & 0xFF;
+      raw_data[22] = (data.Quat6.Data.Q1 >> 16) & 0xFF;
+      raw_data[23] = (data.Quat6.Data.Q1 >> 24) & 0xFF;
+      raw_data[24] = (data.Quat6.Data.Q2 >> 0) & 0xFF;
+      raw_data[25] = (data.Quat6.Data.Q2 >> 8) & 0xFF;
+      raw_data[26] = (data.Quat6.Data.Q2 >> 16) & 0xFF;
+      raw_data[27] = (data.Quat6.Data.Q2 >> 24) & 0xFF;
+      raw_data[28] = (data.Quat6.Data.Q3 >> 0) & 0xFF;
+      raw_data[29] = (data.Quat6.Data.Q3 >> 8) & 0xFF;
+      raw_data[30] = (data.Quat6.Data.Q3 >> 16) & 0xFF;
+      raw_data[31] = (data.Quat6.Data.Q3 >> 24) & 0xFF;
+
+      raw_data[32] = cal_checksum(raw_data, 32);
+
+      SERIAL_PORT.write(raw_data, 33);
+
+      // double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30
+      // double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30
+      // double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30
+      // double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
+
+      // double gx = ((double)data.Raw_Gyro.Data.X) / 16.3835 * PI / 180.0; // [rad/s] 16.3835=32767/2000   deg2rad
+      // double gy = ((double)data.Raw_Gyro.Data.Y) / 16.3835 * PI / 180.0;
+      // double gz = ((double)data.Raw_Gyro.Data.Z) / 16.3835 * PI / 180.0;
+
+      // uint32_t time_sec = micros() / 1000000;
+      // uint32_t time_subsecs = micros() - time_sec * 1000000;
+
+      // SERIAL_PORT.print(time_sec);
+      // SERIAL_PORT.print(F("."));
+      // SERIAL_PORT.print(time_subsecs);
+      // SERIAL_PORT.print(F(","));
+      // SERIAL_PORT.print(q1, 4);
+      // SERIAL_PORT.print(F(","));
+      // SERIAL_PORT.print(q2, 4);
+      // SERIAL_PORT.print(F(","));
+      // SERIAL_PORT.print(q3, 4);
+      // SERIAL_PORT.print(F(","));
+      // SERIAL_PORT.print(q0, 4);
+      // SERIAL_PORT.print(F(","));
+      // SERIAL_PORT.print(gx, 4);
+      // SERIAL_PORT.print(F(","));
+      // SERIAL_PORT.print(gy, 4);
+      // SERIAL_PORT.print(F(","));
+      // SERIAL_PORT.print(gz, 4);
+      // SERIAL_PORT.println(F(""));
     }
-
-
   }
 
-  if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail)  // If more data is available then we should read it right away - and not delay
+  if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay
   {
     // delay(1);
   }
 }
 
-void setup() {
+void setup()
+{
 
-  SERIAL_PORT.begin(2000000);  // Start the serial console
+  SERIAL_PORT.begin(2000000); // Start the serial console
   delay(100);
 
 #ifdef USE_SPI
@@ -122,7 +176,8 @@ void setup() {
 #endif
 
   bool initialized = false;
-  while (!initialized) {
+  while (!initialized)
+  {
 
     // Initialize the ICM-20948
     // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually.
@@ -132,15 +187,17 @@ void setup() {
     myICM.begin(WIRE_PORT, AD0_VAL);
 #endif
 
-    if (myICM.status != ICM_20948_Stat_Ok) {
+    if (myICM.status != ICM_20948_Stat_Ok)
+    {
       delay(500);
-    } else {
+    }
+    else
+    {
       initialized = true;
     }
   }
 
-
-  bool success = true;  // Use success to show if the DMP configuration was successful
+  bool success = true; // Use success to show if the DMP configuration was successful
 
   // Initialize the DMP. initializeDMP is a weak function. You can overwrite it if you want to e.g. to change the sample rate
   success &= (myICM.initializeDMP() == ICM_20948_Stat_Ok);
@@ -168,8 +225,8 @@ void setup() {
   // Enable any additional sensors / features
   // success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_GYROSCOPE) == ICM_20948_Stat_Ok);
   success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == ICM_20948_Stat_Ok);
-  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok);
-  //success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok);
+  // success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok);
+  // success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok);
 
   // Configuring DMP to output data at multiple ODRs:
   // DMP is capable of outputting multiple sensor data at different rates to FIFO.
@@ -177,12 +234,12 @@ void setup() {
   // Value = (DMP running rate / ODR ) - 1
   // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
   // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok);  // Set to the maximum
-  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok);  // Set to the maximum
-  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum
   success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum
-  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
-  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum
-  //success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
 
   // Enable the FIFO
   success &= (myICM.enableFIFO() == ICM_20948_Stat_Ok);
@@ -197,12 +254,15 @@ void setup() {
   success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
 
   // Check success
-  if (success) {
-  } else {
+  if (success)
+  {
+  }
+  else
+  {
     SERIAL_PORT.println(F("Enable DMP failed!"));
     SERIAL_PORT.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..."));
     while (1)
-      ;  // Do nothing more
+      ; // Do nothing more
   }
 
   pinMode(LED_BUILTIN, OUTPUT);
@@ -211,14 +271,18 @@ void setup() {
   Timer6->setOverflow(1000, MICROSEC_FORMAT);
   Timer6->attachInterrupt(timerInterrupt);
   Timer6->refresh();
-  Timer6->resume();  //Timter Start
+  Timer6->resume(); // Timter Start
 }
 
-void loop() {
-  if (digitalRead(LED_BUILTIN) == HIGH) {
+void loop()
+{
+  if (digitalRead(LED_BUILTIN) == HIGH)
+  {
     digitalWrite(LED_BUILTIN, LOW);
     delay(500);
-  } else {
+  }
+  else
+  {
     digitalWrite(LED_BUILTIN, HIGH);
     delay(500);
   }
@@ -234,12 +298,12 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
     return ICM_20948_Stat_DMPNotSupported;
   }
 
-  ICM_20948_Status_e  worstResult = ICM_20948_Stat_Ok;
+  ICM_20948_Status_e worstResult = ICM_20948_Stat_Ok;
 
   // The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration
   // sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
 
-  ICM_20948_Status_e  result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful
+  ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful
 
   // Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer
   // to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read
@@ -262,7 +326,9 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // false: clear the I2C_SLV0_CTRL I2C_SLV0_REG_DIS (we want to write the register value)
   // true: set the I2C_SLV0_CTRL I2C_SLV0_GRP bit to show the register pairing starts at byte 1+2 (copied from inv_icm20948_resume_akm)
   // true: set the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW to byte-swap the data from the mag (copied from inv_icm20948_resume_akm)
-  result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true); if (result > worstResult) worstResult = result;
+  result = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_RSV2, 10, true, true, false, true, true);
+  if (result > worstResult)
+    worstResult = result;
   //
   // We also need to set up I2C_SLV1 to do the Single Measurement triggering:
   // 1: use I2C_SLV1
@@ -275,7 +341,9 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // false: clear the I2C_SLV0_CTRL I2C_SLV0_GRP bit
   // false: clear the I2C_SLV0_CTRL I2C_SLV0_BYTE_SW bit
   // AK09916_mode_single: tell I2C_SLV1 to write the Single Measurement command each sample
-  result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single); if (result > worstResult) worstResult = result;
+  result = i2cControllerConfigurePeripheral(1, MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL2, 1, false, true, false, false, false, AK09916_mode_single);
+  if (result > worstResult)
+    worstResult = result;
 
   // Set the I2C Master ODR configuration
   // It is not clear why we need to do this... But it appears to be essential! From the datasheet:
@@ -285,29 +353,45 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   //  If gyroscope is disabled, then all sensors (including I2C_MASTER) use the accelerometer ODR."
   // Since both gyro and accel are running, setting this register should have no effect. But it does. Maybe because the Gyro and Accel are placed in Low Power Mode (cycled)?
   // You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case.
-  result = setBank(3); if (result > worstResult) worstResult = result; // Select Bank 3
+  result = setBank(3);
+  if (result > worstResult)
+    worstResult = result;      // Select Bank 3
   uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz
-  result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1); if (result > worstResult) worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register  
+  result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1);
+  if (result > worstResult)
+    worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register
 
   // Configure clock source through PWR_MGMT_1
   // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator
-  result = setClockSource(ICM_20948_Clock_Auto); if (result > worstResult) worstResult = result; // This is shorthand: success will be set to false if setClockSource fails
+  result = setClockSource(ICM_20948_Clock_Auto);
+  if (result > worstResult)
+    worstResult = result; // This is shorthand: success will be set to false if setClockSource fails
 
   // Enable accel and gyro sensors through PWR_MGMT_2
   // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2
-  result = setBank(0); if (result > worstResult) worstResult = result;                               // Select Bank 0
-  uint8_t pwrMgmt2 = 0x40;                                                          // Set the reserved bit 6 (pressure sensor disable?)
-  result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1); if (result > worstResult) worstResult = result; // Write one byte to the PWR_MGMT_2 register
+  result = setBank(0);
+  if (result > worstResult)
+    worstResult = result;  // Select Bank 0
+  uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?)
+  result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1);
+  if (result > worstResult)
+    worstResult = result; // Write one byte to the PWR_MGMT_2 register
 
   // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG
   // The InvenSense Nucleo example initially puts the accel and gyro into low power mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power Mode
-  result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled); if (result > worstResult) worstResult = result;
+  result = setSampleMode(ICM_20948_Internal_Mst, ICM_20948_Sample_Mode_Cycled);
+  if (result > worstResult)
+    worstResult = result;
 
   // Disable the FIFO
-  result = enableFIFO(false); if (result > worstResult) worstResult = result;
+  result = enableFIFO(false);
+  if (result > worstResult)
+    worstResult = result;
 
   // Disable the DMP
-  result = enableDMP(false); if (result > worstResult) worstResult = result;
+  result = enableDMP(false);
+  if (result > worstResult)
+    worstResult = result;
 
   // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1
   // Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG
@@ -322,69 +406,103 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
                          // dps500
                          // dps1000
                          // dps2000
-  result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS); if (result > worstResult) worstResult = result;
+  result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
+  if (result > worstResult)
+    worstResult = result;
 
   // The InvenSense Nucleo code also enables the gyro DLPF (but leaves GYRO_DLPFCFG set to zero = 196.6Hz (3dB))
   // We found this by going through the SPI data generated by ZaneL's Teensy-ICM-20948 library byte by byte...
   // The gyro DLPF is enabled by default (GYRO_CONFIG_1 = 0x01) so the following line should have no effect, but we'll include it anyway
-  result = enableDLPF(ICM_20948_Internal_Gyr, true); if (result > worstResult) worstResult = result;
+  result = enableDLPF(ICM_20948_Internal_Gyr, true);
+  if (result > worstResult)
+    worstResult = result;
 
   // Enable interrupt for FIFO overflow from FIFOs through INT_ENABLE_2
   // If we see this interrupt, we'll need to reset the FIFO
-  //result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs
+  // result = intEnableOverflowFIFO( 0x1F ); if (result > worstResult) worstResult = result; // Enable the interrupt on all FIFOs
 
   // Turn off what goes into the FIFO through FIFO_EN_1, FIFO_EN_2
   // Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1
-  result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
+  result = setBank(0);
+  if (result > worstResult)
+    worstResult = result; // Select Bank 0
   uint8_t zero = 0;
-  result = write(AGB0_REG_FIFO_EN_1, &zero, 1); if (result > worstResult) worstResult = result;
+  result = write(AGB0_REG_FIFO_EN_1, &zero, 1);
+  if (result > worstResult)
+    worstResult = result;
   // Stop the accelerometer, gyro and temperature data from being written to the FIFO by writing zero to FIFO_EN_2
-  result = write(AGB0_REG_FIFO_EN_2, &zero, 1); if (result > worstResult) worstResult = result;
+  result = write(AGB0_REG_FIFO_EN_2, &zero, 1);
+  if (result > worstResult)
+    worstResult = result;
 
   // Turn off data ready interrupt through INT_ENABLE_1
-  result = intEnableRawDataReady(false); if (result > worstResult) worstResult = result;
+  result = intEnableRawDataReady(false);
+  if (result > worstResult)
+    worstResult = result;
 
   // Reset FIFO through FIFO_RST
-  result = resetFIFO(); if (result > worstResult) worstResult = result;
+  result = resetFIFO();
+  if (result > worstResult)
+    worstResult = result;
 
   // Set gyro sample rate divider with GYRO_SMPLRT_DIV
   // Set accel sample rate divider with ACCEL_SMPLRT_DIV_2
   ICM_20948_smplrt_t mySmplrt;
-  //mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13).
-  //mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13).
+  // mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13).
+  // mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13).
   mySmplrt.g = 4; // 225Hz
   mySmplrt.a = 4; // 225Hz
-  //mySmplrt.g = 8; // 112Hz
-  //mySmplrt.a = 8; // 112Hz
-  result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt); if (result > worstResult) worstResult = result;
+  // mySmplrt.g = 8; // 112Hz
+  // mySmplrt.a = 8; // 112Hz
+  result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt);
+  if (result > worstResult)
+    worstResult = result;
 
   // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
-  result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS
+  result = setDMPstartAddress();
+  if (result > worstResult)
+    worstResult = result; // Defaults to DMP_START_ADDRESS
 
   // Now load the DMP firmware
-  result = loadDMPFirmware(); if (result > worstResult) worstResult = result;
+  result = loadDMPFirmware();
+  if (result > worstResult)
+    worstResult = result;
 
   // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
-  result = setDMPstartAddress(); if (result > worstResult) worstResult = result; // Defaults to DMP_START_ADDRESS
+  result = setDMPstartAddress();
+  if (result > worstResult)
+    worstResult = result; // Defaults to DMP_START_ADDRESS
 
   // Set the Hardware Fix Disable register to 0x48
-  result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
+  result = setBank(0);
+  if (result > worstResult)
+    worstResult = result; // Select Bank 0
   uint8_t fix = 0x48;
-  result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1); if (result > worstResult) worstResult = result;
+  result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1);
+  if (result > worstResult)
+    worstResult = result;
 
   // Set the Single FIFO Priority Select register to 0xE4
-  result = setBank(0); if (result > worstResult) worstResult = result; // Select Bank 0
+  result = setBank(0);
+  if (result > worstResult)
+    worstResult = result; // Select Bank 0
   uint8_t fifoPrio = 0xE4;
-  result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1); if (result > worstResult) worstResult = result;
+  result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1);
+  if (result > worstResult)
+    worstResult = result;
 
   // Configure Accel scaling to DMP
   // The DMP scales accel raw data internally to align 1g as 2^25
   // In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g
   const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00};
-  result = writeDMPmems(ACC_SCALE, 4, &accScale[0]); if (result > worstResult) worstResult = result; // Write accScale to ACC_SCALE DMP register
+  result = writeDMPmems(ACC_SCALE, 4, &accScale[0]);
+  if (result > worstResult)
+    worstResult = result; // Write accScale to ACC_SCALE DMP register
   // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g
   const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00};
-  result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]); if (result > worstResult) worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register
+  result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]);
+  if (result > worstResult)
+    worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register
 
   // Configure Compass mount matrix and scale to DMP
   // The mount matrix write to DMP register is used to align the compass axes with accel/gyro.
@@ -398,35 +516,73 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
   const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99};  // Value taken from InvenSense Nucleo example
   const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example
-  result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]); if (result > worstResult) worstResult = result;
+  result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(CPASS_MTX_01, 4, &mountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(CPASS_MTX_02, 4, &mountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(CPASS_MTX_10, 4, &mountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(CPASS_MTX_11, 4, &mountMultiplierMinus[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(CPASS_MTX_12, 4, &mountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(CPASS_MTX_20, 4, &mountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(CPASS_MTX_21, 4, &mountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(CPASS_MTX_22, 4, &mountMultiplierMinus[0]);
+  if (result > worstResult)
+    worstResult = result;
 
   // Configure the B2S Mounting Matrix
   const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
   const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
-  result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]); if (result > worstResult) worstResult = result;
-  result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]); if (result > worstResult) worstResult = result;
+  result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(B2S_MTX_01, 4, &b2sMountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(B2S_MTX_02, 4, &b2sMountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(B2S_MTX_10, 4, &b2sMountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(B2S_MTX_11, 4, &b2sMountMultiplierPlus[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(B2S_MTX_12, 4, &b2sMountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(B2S_MTX_20, 4, &b2sMountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(B2S_MTX_21, 4, &b2sMountMultiplierZero[0]);
+  if (result > worstResult)
+    worstResult = result;
+  result = writeDMPmems(B2S_MTX_22, 4, &b2sMountMultiplierPlus[0]);
+  if (result > worstResult)
+    worstResult = result;
 
   // Configure the DMP Gyro Scaling Factor
   // @param[in] gyro_div Value written to GYRO_SMPLRT_DIV register, where
   //            0=1125Hz sample rate, 1=562.5Hz sample rate, ... 4=225Hz sample rate, ...
   //            10=102.2727Hz sample rate, ... etc.
   // @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps
-  result = setGyroSF(4, 3); if (result > worstResult) worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above)
+  result = setGyroSF(4, 3);
+  if (result > worstResult)
+    worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above)
 
   // Configure the Gyro full scale
   // 2000dps : 2^28
@@ -434,39 +590,50 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   //  500dps : 2^26
   //  250dps : 2^25
   const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28
-  result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]); if (result > worstResult) worstResult = result;
+  result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]);
+  if (result > worstResult)
+    worstResult = result;
 
   // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
-  //const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
+  // const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
   const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz
-  //const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz
-  result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]); if (result > worstResult) worstResult = result;
+  // const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz
+  result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]);
+  if (result > worstResult)
+    worstResult = result;
 
   // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
-  //const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
+  // const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
   const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz
-  //const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz
-  result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]); if (result > worstResult) worstResult = result;
+  // const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz
+  result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]);
+  if (result > worstResult)
+    worstResult = result;
 
   // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
-  //const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
+  // const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
   const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz
-  //const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz
-  result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]); if (result > worstResult) worstResult = result;
+  // const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz
+  result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]);
+  if (result > worstResult)
+    worstResult = result;
 
   // Configure the Accel Cal Rate
   const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example
-  result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]); if (result > worstResult) worstResult = result;
+  result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]);
+  if (result > worstResult)
+    worstResult = result;
 
   // Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz.
   // Let's set the Compass Time Buffer to 69 (Hz).
   const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz
-  result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]); if (result > worstResult) worstResult = result;
+  result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]);
+  if (result > worstResult)
+    worstResult = result;
 
   // Enable DMP interrupt
   // This would be the most efficient way of getting the DMP data, instead of polling the FIFO
-  //result = intEnableDMP(true); if (result > worstResult) worstResult = result;
-
+  // result = intEnableDMP(true); if (result > worstResult) worstResult = result;
 
   return worstResult;
 }

From c4a941bea94b43e376a9db49b68ac644c2b40f7e Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Mon, 15 Jun 2026 11:22:50 +0900
Subject: [PATCH 2/9] =?UTF-8?q?=E5=8A=A0=E9=80=9F=E5=BA=A6=E6=9C=89?=
 =?UTF-8?q?=E5=8A=B9=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
index aad29d0..ebaa2be 100644
--- a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
+++ b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
@@ -225,7 +225,7 @@ void setup()
   // Enable any additional sensors / features
   // success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_GYROSCOPE) == ICM_20948_Stat_Ok);
   success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_GYROSCOPE) == ICM_20948_Stat_Ok);
-  // success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok);
+  success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_RAW_ACCELEROMETER) == ICM_20948_Stat_Ok);
   // success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED) == ICM_20948_Stat_Ok);
 
   // Configuring DMP to output data at multiple ODRs:

From c0aabc810d83a170e221a4401ad21a8ce6bf025e Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Tue, 16 Jun 2026 15:21:43 +0900
Subject: [PATCH 3/9] =?UTF-8?q?=E5=8A=A0=E9=80=9F=E5=BA=A6=E3=81=A8?=
 =?UTF-8?q?=E8=A7=92=E9=80=9F=E5=BA=A6=E3=81=AE=E3=83=95=E3=82=A9=E3=83=BC?=
 =?UTF-8?q?=E3=83=9E=E3=83=83=E3=83=88=E3=81=8C=E9=81=95=E3=81=A3=E3=81=9F?=
 =?UTF-8?q?=E3=81=AE=E3=81=A7=E4=BF=AE=E6=AD=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../RT-9DOF-IMU-V3-Quat.ino                   | 26 +++++++++----------
 1 file changed, 13 insertions(+), 13 deletions(-)

diff --git a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
index ebaa2be..f51dfa9 100644
--- a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
+++ b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
@@ -92,19 +92,19 @@ void timerInterrupt(void)
 
       // data
       raw_data[7] = time_stamp++;
-      raw_data[8] = (data.Raw_Accel.Data.X >> 8) & 0xFF;
-      raw_data[9] = (data.Raw_Accel.Data.X) & 0xFF;
-      raw_data[10] = (data.Raw_Accel.Data.Y >> 8) & 0xFF;
-      raw_data[11] = (data.Raw_Accel.Data.Y) & 0xFF;
-      raw_data[12] = (data.Raw_Accel.Data.Z >> 8) & 0xFF;
-      raw_data[13] = (data.Raw_Accel.Data.Z) & 0xFF;
-
-      raw_data[14] = (data.Raw_Gyro.Data.X >> 8) & 0xFF;
-      raw_data[15] = (data.Raw_Gyro.Data.X) & 0xFF;
-      raw_data[16] = (data.Raw_Gyro.Data.Y >> 8) & 0xFF;
-      raw_data[17] = (data.Raw_Gyro.Data.Y) & 0xFF;
-      raw_data[18] = (data.Raw_Gyro.Data.Z >> 8) & 0xFF;
-      raw_data[19] = (data.Raw_Gyro.Data.Z) & 0xFF;
+      raw_data[8] = (data.Raw_Accel.Data.X >> 0) & 0xFF;
+      raw_data[9] = (data.Raw_Accel.Data.X >> 8) & 0xFF;
+      raw_data[10] = (data.Raw_Accel.Data.Y >> 0) & 0xFF;
+      raw_data[11] = (data.Raw_Accel.Data.Y >> 8) & 0xFF;
+      raw_data[12] = (data.Raw_Accel.Data.Z >> 0) & 0xFF;
+      raw_data[13] = (data.Raw_Accel.Data.Z >> 8) & 0xFF;
+
+      raw_data[14] = (data.Raw_Gyro.Data.X >> 0) & 0xFF;
+      raw_data[15] = (data.Raw_Gyro.Data.X >> 8) & 0xFF;
+      raw_data[16] = (data.Raw_Gyro.Data.Y >> 0) & 0xFF;
+      raw_data[17] = (data.Raw_Gyro.Data.Y >> 8) & 0xFF;
+      raw_data[18] = (data.Raw_Gyro.Data.Z >> 0) & 0xFF;
+      raw_data[19] = (data.Raw_Gyro.Data.Z >> 8) & 0xFF;
 
       raw_data[20] = (data.Quat6.Data.Q1 >> 0) & 0xFF;
       raw_data[21] = (data.Quat6.Data.Q1 >> 8) & 0xFF;

From ccd3576184b5dcaa48b6e526148f68ba094ffa07 Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Tue, 16 Jun 2026 15:56:33 +0900
Subject: [PATCH 4/9] =?UTF-8?q?=E5=8A=A0=E9=80=9F=E5=BA=A6=E3=81=AE?=
 =?UTF-8?q?=E3=83=AC=E3=83=B3=E3=82=B8=E3=82=92=E5=A4=89=E6=9B=B4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
index f51dfa9..389d141 100644
--- a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
+++ b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
@@ -394,9 +394,9 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
     worstResult = result;
 
   // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1
-  // Set Accel FSR (Full scale range) to 4g through ACCEL_CONFIG
+  // Set Accel FSR (Full scale range) to 16g through ACCEL_CONFIG
   ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
-  myFSS.a = gpm4;        // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
+  myFSS.a = gpm16;       // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
                          // gpm2
                          // gpm4
                          // gpm8

From 06e7317343828d6d8df488fc146c871863f1f1c9 Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Fri, 19 Jun 2026 10:24:45 +0900
Subject: [PATCH 5/9] =?UTF-8?q?README=E3=81=AB=E3=83=87=E3=83=BC=E3=82=BF?=
 =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E=E3=83=83=E3=83=88=E3=82=92?=
 =?UTF-8?q?=E8=BF=BD=E5=8A=A0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 README.md | 38 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 38 insertions(+)

diff --git a/README.md b/README.md
index 14b6804..a8ff113 100644
--- a/README.md
+++ b/README.md
@@ -5,6 +5,44 @@ This software is a sample program for the [RT-USB-9axisIMU3](https://www.rt-shop
 
 The file RT-9DOF-IMU-V3-Quat.ino is forked from the [SparkFun_ICM-20948_ArduinoLibrary examples](https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary/tree/main/examples).
 
+## Data format
+|Byte | Content | Description |  
+|-- | -- | -- | --
+|0 | 0xff | Header |  
+|1 | 0xff | Header |  
+|2 | 0x52 | ASCII code "R" |  
+|3 | 0x54 | ASCII code "T" |  
+|4 | 0x40 | Lower Bits of Product Identifier |  
+|5 | 0x41 | Upper Bits of Product Identifier |  
+|6 | 0x01 | FW version |  
+|7 | 0x00~0xff | Timestamp - incremented with each data transmission, wraps to 0x00 after 0xff |  
+|8 | ACC_X_L | ax=(ACC_X_L\|ACC_X_H<<8)/2048*9.8 [m/s] |  
+|9 | ACC_X_H |   |  
+|10 | ACC_Y_L |   |  
+|11 | ACC_Y_H |   |  
+|12 | ACC_Z_L |   |  
+|13 | ACC_Z_H |   |  
+|14 | GYRO_X_L | gx=(GYRO_X_L\|GYRO_X_H<<8)/(32767/2000)*PI/180 [rad/s] |  
+|15 | GYRO_X_H |   |  
+|16 | GYRO_Y_L |   |  
+|17 | GYRO_Y_H |   |  
+|18 | GYRO_Z_L |   |  
+|19 | GYRO_Z_H |   |  
+|20 | Q1_1 | Q1 multiplied by (2^30). q1=(Q1_1 \| Q1_2<<8 \| Q1_3<<16 \| Q1_4<<24 )/(2^30) |  
+|21 | Q1_2 |   |  
+|22 | Q1_3 |   |  
+|23 | Q1_4 |   |  
+|24 | Q2_1 |   |  
+|25 | Q2_2 |   |  
+|26 | Q2_3 |   |  
+|27 | Q2_4 |   |  
+|28 | Q3_1 |   |  
+|29 | Q3_2 |   |  
+|30 | Q3_3 |   |  
+|31 | Q3_4 |   |  
+|32 | Checksum | Lower 8 bits of the sum of all bytes |  
+
+
 ## Flashing from the Arduino IDE
 
 ### Setting Up Dependencies

From f5f58c402a5762d0dec5d089cd35db7321125d89 Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Fri, 19 Jun 2026 16:40:42 +0900
Subject: [PATCH 6/9] Arduino auto format

---
 .../RT-9DOF-IMU-V3-Quat.ino                   | 177 ++++++++----------
 1 file changed, 80 insertions(+), 97 deletions(-)

diff --git a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
index 389d141..5045a01 100644
--- a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
+++ b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
@@ -26,25 +26,25 @@
  * Distributed as-is; no warranty is given.
  ***************************************************************/
 
-#include "ICM_20948.h" // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU // should install v1.2.5
+#include "ICM_20948.h"  // Click here to get the library: http://librarymanager/All#SparkFun_ICM_20948_IMU // should install v1.2.5
 
-#define USE_SPI // Uncomment this to use SPI
+#define USE_SPI  // Uncomment this to use SPI
 
 #define SERIAL_PORT Serial
 
-#define SPI_PORT SPI     // Your desired SPI port.       Used only when "USE_SPI" is defined
-#define SPI_FREQ 5000000 // You can override the default SPI frequency
-#define CS_PIN D4        // Which pin you connect CS to. Used only when "USE_SPI" is defined
+#define SPI_PORT SPI      // Your desired SPI port.       Used only when "USE_SPI" is defined
+#define SPI_FREQ 5000000  // You can override the default SPI frequency
+#define CS_PIN D4         // Which pin you connect CS to. Used only when "USE_SPI" is defined
 
-#define WIRE_PORT Wire // Your desired Wire port.      Used when "USE_SPI" is not defined
+#define WIRE_PORT Wire  // Your desired Wire port.      Used when "USE_SPI" is not defined
 // The value of the last bit of the I2C address.
 // On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
 #define AD0_VAL 1
 
 #ifdef USE_SPI
-ICM_20948_SPI myICM; // If using SPI create an ICM_20948_SPI object
+ICM_20948_SPI myICM;  // If using SPI create an ICM_20948_SPI object
 #else
-ICM_20948_I2C myICM; // Otherwise create an ICM_20948_I2C object
+ICM_20948_I2C myICM;  // Otherwise create an ICM_20948_I2C object
 #endif
 
 #define LED_BUILTIN PA15
@@ -52,18 +52,15 @@ HardwareTimer *Timer6 = new HardwareTimer(TIM6);
 
 #define FW_VERSION 0x01
 
-uint8_t cal_checksum(const uint8_t *data, size_t size)
-{
+uint8_t cal_checksum(const uint8_t *data, size_t size) {
   uint8_t sum = 0;
-  for (size_t i = 0; i < size; i++)
-  {
+  for (size_t i = 0; i < size; i++) {
     sum += data[i];
   }
   return sum;
 }
 
-void timerInterrupt(void)
-{
+void timerInterrupt(void) {
 
   // Read any DMP data waiting in the FIFO
   // Note:
@@ -77,18 +74,18 @@ void timerInterrupt(void)
   uint8_t raw_data[33];
   static uint8_t time_stamp = 0;
 
-  if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) // Was valid data available?
+  if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail))  // Was valid data available?
   {
-    if ((data.header & DMP_header_bitmap_Quat6) > 0) // && (data.header & DMP_header_bitmap_Gyro) > 0)
+    if ((data.header & DMP_header_bitmap_Quat6) > 0)  // && (data.header & DMP_header_bitmap_Gyro) > 0)
     {
       // header
-      raw_data[0] = 0xff;       // header1
-      raw_data[1] = 0xff;       // header2
-      raw_data[2] = 0x52;       // ASCII 'R'
-      raw_data[3] = 0x54;       // ASCII 'T'
-      raw_data[4] = 0x40;       // device ID_L
-      raw_data[5] = 0x41;       // device ID_H
-      raw_data[6] = FW_VERSION; // FW version
+      raw_data[0] = 0xff;        // header1
+      raw_data[1] = 0xff;        // header2
+      raw_data[2] = 0x52;        // ASCII 'R'
+      raw_data[3] = 0x54;        // ASCII 'T'
+      raw_data[4] = 0x40;        // device ID_L
+      raw_data[5] = 0x41;        // device ID_H
+      raw_data[6] = FW_VERSION;  // FW version
 
       // data
       raw_data[7] = time_stamp++;
@@ -156,16 +153,15 @@ void timerInterrupt(void)
     }
   }
 
-  if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail) // If more data is available then we should read it right away - and not delay
+  if (myICM.status != ICM_20948_Stat_FIFOMoreDataAvail)  // If more data is available then we should read it right away - and not delay
   {
     // delay(1);
   }
 }
 
-void setup()
-{
+void setup() {
 
-  SERIAL_PORT.begin(2000000); // Start the serial console
+  SERIAL_PORT.begin(2000000);  // Start the serial console
   delay(100);
 
 #ifdef USE_SPI
@@ -176,8 +172,7 @@ void setup()
 #endif
 
   bool initialized = false;
-  while (!initialized)
-  {
+  while (!initialized) {
 
     // Initialize the ICM-20948
     // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually.
@@ -187,17 +182,14 @@ void setup()
     myICM.begin(WIRE_PORT, AD0_VAL);
 #endif
 
-    if (myICM.status != ICM_20948_Stat_Ok)
-    {
+    if (myICM.status != ICM_20948_Stat_Ok) {
       delay(500);
-    }
-    else
-    {
+    } else {
       initialized = true;
     }
   }
 
-  bool success = true; // Use success to show if the DMP configuration was successful
+  bool success = true;  // Use success to show if the DMP configuration was successful
 
   // Initialize the DMP. initializeDMP is a weak function. You can overwrite it if you want to e.g. to change the sample rate
   success &= (myICM.initializeDMP() == ICM_20948_Stat_Ok);
@@ -234,9 +226,9 @@ void setup()
   // Value = (DMP running rate / ODR ) - 1
   // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.
   // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 0) == ICM_20948_Stat_Ok);  // Set to the maximum
-  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0) == ICM_20948_Stat_Ok);  // Set to the maximum
   // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum
-  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum
+  success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok);  // Set to the maximum
   // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
   // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum
   // success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum
@@ -254,15 +246,12 @@ void setup()
   success &= (myICM.resetFIFO() == ICM_20948_Stat_Ok);
 
   // Check success
-  if (success)
-  {
-  }
-  else
-  {
+  if (success) {
+  } else {
     SERIAL_PORT.println(F("Enable DMP failed!"));
     SERIAL_PORT.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..."));
     while (1)
-      ; // Do nothing more
+      ;  // Do nothing more
   }
 
   pinMode(LED_BUILTIN, OUTPUT);
@@ -271,29 +260,23 @@ void setup()
   Timer6->setOverflow(1000, MICROSEC_FORMAT);
   Timer6->attachInterrupt(timerInterrupt);
   Timer6->refresh();
-  Timer6->resume(); // Timter Start
+  Timer6->resume();  // Timter Start
 }
 
-void loop()
-{
-  if (digitalRead(LED_BUILTIN) == HIGH)
-  {
+void loop() {
+  if (digitalRead(LED_BUILTIN) == HIGH) {
     digitalWrite(LED_BUILTIN, LOW);
     delay(500);
-  }
-  else
-  {
+  } else {
     digitalWrite(LED_BUILTIN, HIGH);
     delay(500);
   }
 }
 
 // initializeDMP is a weak function. Let's overwrite it so we can increase the sample rate
-ICM_20948_Status_e ICM_20948::initializeDMP(void)
-{
+ICM_20948_Status_e ICM_20948::initializeDMP(void) {
   // First, let's check if the DMP is available
-  if (_device._dmp_firmware_available != true)
-  {
+  if (_device._dmp_firmware_available != true) {
     debugPrint(F("ICM_20948::startupDMP: DMP is not available. Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..."));
     return ICM_20948_Stat_DMPNotSupported;
   }
@@ -303,7 +286,7 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // The ICM-20948 is awake and ready but hasn't been configured. Let's step through the configuration
   // sequence from InvenSense's _confidential_ Application Note "Programming Sequence for DMP Hardware Functions".
 
-  ICM_20948_Status_e result = ICM_20948_Stat_Ok; // Use result and worstResult to show if the configuration was successful
+  ICM_20948_Status_e result = ICM_20948_Stat_Ok;  // Use result and worstResult to show if the configuration was successful
 
   // Normally, when the DMP is not enabled, startupMagnetometer (called by startupDefault, which is called by begin) configures the AK09916 magnetometer
   // to run at 100Hz by setting the CNTL2 register (0x31) to 0x08. Then the ICM20948's I2C_SLV0 is configured to read
@@ -355,27 +338,27 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // You can see by monitoring the Aux I2C pins that the next three lines reduce the bus traffic (magnetometer reads) from 1125Hz to the chosen rate: 68.75Hz in this case.
   result = setBank(3);
   if (result > worstResult)
-    worstResult = result;      // Select Bank 3
-  uint8_t mstODRconfig = 0x04; // Set the ODR configuration to 1100/2^4 = 68.75Hz
+    worstResult = result;       // Select Bank 3
+  uint8_t mstODRconfig = 0x04;  // Set the ODR configuration to 1100/2^4 = 68.75Hz
   result = write(AGB3_REG_I2C_MST_ODR_CONFIG, &mstODRconfig, 1);
   if (result > worstResult)
-    worstResult = result; // Write one byte to the I2C_MST_ODR_CONFIG register
+    worstResult = result;  // Write one byte to the I2C_MST_ODR_CONFIG register
 
   // Configure clock source through PWR_MGMT_1
   // ICM_20948_Clock_Auto selects the best available clock source – PLL if ready, else use the Internal oscillator
   result = setClockSource(ICM_20948_Clock_Auto);
   if (result > worstResult)
-    worstResult = result; // This is shorthand: success will be set to false if setClockSource fails
+    worstResult = result;  // This is shorthand: success will be set to false if setClockSource fails
 
   // Enable accel and gyro sensors through PWR_MGMT_2
   // Enable Accelerometer (all axes) and Gyroscope (all axes) by writing zero to PWR_MGMT_2
   result = setBank(0);
   if (result > worstResult)
-    worstResult = result;  // Select Bank 0
-  uint8_t pwrMgmt2 = 0x40; // Set the reserved bit 6 (pressure sensor disable?)
+    worstResult = result;   // Select Bank 0
+  uint8_t pwrMgmt2 = 0x40;  // Set the reserved bit 6 (pressure sensor disable?)
   result = write(AGB0_REG_PWR_MGMT_2, &pwrMgmt2, 1);
   if (result > worstResult)
-    worstResult = result; // Write one byte to the PWR_MGMT_2 register
+    worstResult = result;  // Write one byte to the PWR_MGMT_2 register
 
   // Place _only_ I2C_Master in Low Power Mode (cycled) via LP_CONFIG
   // The InvenSense Nucleo example initially puts the accel and gyro into low power mode too, but then later updates LP_CONFIG so only the I2C_Master is in Low Power Mode
@@ -395,17 +378,17 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
 
   // Set Gyro FSR (Full scale range) to 2000dps through GYRO_CONFIG_1
   // Set Accel FSR (Full scale range) to 16g through ACCEL_CONFIG
-  ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
-  myFSS.a = gpm16;       // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
-                         // gpm2
-                         // gpm4
-                         // gpm8
-                         // gpm16
-  myFSS.g = dps2000;     // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
-                         // dps250
-                         // dps500
-                         // dps1000
-                         // dps2000
+  ICM_20948_fss_t myFSS;  // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors
+  myFSS.a = gpm16;        // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
+                          // gpm2
+                          // gpm4
+                          // gpm8
+                          // gpm16
+  myFSS.g = dps2000;      // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
+                          // dps250
+                          // dps500
+                          // dps1000
+                          // dps2000
   result = setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
   if (result > worstResult)
     worstResult = result;
@@ -425,7 +408,7 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // Stop the peripheral data from being written to the FIFO by writing zero to FIFO_EN_1
   result = setBank(0);
   if (result > worstResult)
-    worstResult = result; // Select Bank 0
+    worstResult = result;  // Select Bank 0
   uint8_t zero = 0;
   result = write(AGB0_REG_FIFO_EN_1, &zero, 1);
   if (result > worstResult)
@@ -450,8 +433,8 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   ICM_20948_smplrt_t mySmplrt;
   // mySmplrt.g = 19; // ODR is computed as follows: 1.1 kHz/(1+GYRO_SMPLRT_DIV[7:0]). 19 = 55Hz. InvenSense Nucleo example uses 19 (0x13).
   // mySmplrt.a = 19; // ODR is computed as follows: 1.125 kHz/(1+ACCEL_SMPLRT_DIV[11:0]). 19 = 56.25Hz. InvenSense Nucleo example uses 19 (0x13).
-  mySmplrt.g = 4; // 225Hz
-  mySmplrt.a = 4; // 225Hz
+  mySmplrt.g = 4;  // 225Hz
+  mySmplrt.a = 4;  // 225Hz
   // mySmplrt.g = 8; // 112Hz
   // mySmplrt.a = 8; // 112Hz
   result = setSampleRate((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySmplrt);
@@ -461,7 +444,7 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // Setup DMP start address through PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
   result = setDMPstartAddress();
   if (result > worstResult)
-    worstResult = result; // Defaults to DMP_START_ADDRESS
+    worstResult = result;  // Defaults to DMP_START_ADDRESS
 
   // Now load the DMP firmware
   result = loadDMPFirmware();
@@ -471,12 +454,12 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // Write the 2 byte Firmware Start Value to ICM PRGM_STRT_ADDRH/PRGM_STRT_ADDRL
   result = setDMPstartAddress();
   if (result > worstResult)
-    worstResult = result; // Defaults to DMP_START_ADDRESS
+    worstResult = result;  // Defaults to DMP_START_ADDRESS
 
   // Set the Hardware Fix Disable register to 0x48
   result = setBank(0);
   if (result > worstResult)
-    worstResult = result; // Select Bank 0
+    worstResult = result;  // Select Bank 0
   uint8_t fix = 0x48;
   result = write(AGB0_REG_HW_FIX_DISABLE, &fix, 1);
   if (result > worstResult)
@@ -485,7 +468,7 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // Set the Single FIFO Priority Select register to 0xE4
   result = setBank(0);
   if (result > worstResult)
-    worstResult = result; // Select Bank 0
+    worstResult = result;  // Select Bank 0
   uint8_t fifoPrio = 0xE4;
   result = write(AGB0_REG_SINGLE_FIFO_PRIORITY_SEL, &fifoPrio, 1);
   if (result > worstResult)
@@ -494,15 +477,15 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // Configure Accel scaling to DMP
   // The DMP scales accel raw data internally to align 1g as 2^25
   // In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g
-  const unsigned char accScale[4] = {0x04, 0x00, 0x00, 0x00};
+  const unsigned char accScale[4] = { 0x04, 0x00, 0x00, 0x00 };
   result = writeDMPmems(ACC_SCALE, 4, &accScale[0]);
   if (result > worstResult)
-    worstResult = result; // Write accScale to ACC_SCALE DMP register
+    worstResult = result;  // Write accScale to ACC_SCALE DMP register
   // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g
-  const unsigned char accScale2[4] = {0x00, 0x04, 0x00, 0x00};
+  const unsigned char accScale2[4] = { 0x00, 0x04, 0x00, 0x00 };
   result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]);
   if (result > worstResult)
-    worstResult = result; // Write accScale2 to ACC_SCALE2 DMP register
+    worstResult = result;  // Write accScale2 to ACC_SCALE2 DMP register
 
   // Configure Compass mount matrix and scale to DMP
   // The mount matrix write to DMP register is used to align the compass axes with accel/gyro.
@@ -513,9 +496,9 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // Z = raw_x * CPASS_MTX_20 + raw_y * CPASS_MTX_21 + raw_z * CPASS_MTX_22
   // The AK09916 produces a 16-bit signed output in the range +/-32752 corresponding to +/-4912uT. 1uT = 6.66 ADU.
   // 2^30 / 6.66666 = 161061273 = 0x9999999
-  const unsigned char mountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
-  const unsigned char mountMultiplierPlus[4] = {0x09, 0x99, 0x99, 0x99};  // Value taken from InvenSense Nucleo example
-  const unsigned char mountMultiplierMinus[4] = {0xF6, 0x66, 0x66, 0x67}; // Value taken from InvenSense Nucleo example
+  const unsigned char mountMultiplierZero[4] = { 0x00, 0x00, 0x00, 0x00 };
+  const unsigned char mountMultiplierPlus[4] = { 0x09, 0x99, 0x99, 0x99 };   // Value taken from InvenSense Nucleo example
+  const unsigned char mountMultiplierMinus[4] = { 0xF6, 0x66, 0x66, 0x67 };  // Value taken from InvenSense Nucleo example
   result = writeDMPmems(CPASS_MTX_00, 4, &mountMultiplierPlus[0]);
   if (result > worstResult)
     worstResult = result;
@@ -545,8 +528,8 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
     worstResult = result;
 
   // Configure the B2S Mounting Matrix
-  const unsigned char b2sMountMultiplierZero[4] = {0x00, 0x00, 0x00, 0x00};
-  const unsigned char b2sMountMultiplierPlus[4] = {0x40, 0x00, 0x00, 0x00}; // Value taken from InvenSense Nucleo example
+  const unsigned char b2sMountMultiplierZero[4] = { 0x00, 0x00, 0x00, 0x00 };
+  const unsigned char b2sMountMultiplierPlus[4] = { 0x40, 0x00, 0x00, 0x00 };  // Value taken from InvenSense Nucleo example
   result = writeDMPmems(B2S_MTX_00, 4, &b2sMountMultiplierPlus[0]);
   if (result > worstResult)
     worstResult = result;
@@ -582,21 +565,21 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
   // @param[in] gyro_level 0=250 dps, 1=500 dps, 2=1000 dps, 3=2000 dps
   result = setGyroSF(4, 3);
   if (result > worstResult)
-    worstResult = result; // 19 = 55Hz (see above), 3 = 2000dps (see above)
+    worstResult = result;  // 19 = 55Hz (see above), 3 = 2000dps (see above)
 
   // Configure the Gyro full scale
   // 2000dps : 2^28
   // 1000dps : 2^27
   //  500dps : 2^26
   //  250dps : 2^25
-  const unsigned char gyroFullScale[4] = {0x10, 0x00, 0x00, 0x00}; // 2000dps : 2^28
+  const unsigned char gyroFullScale[4] = { 0x10, 0x00, 0x00, 0x00 };  // 2000dps : 2^28
   result = writeDMPmems(GYRO_FULLSCALE, 4, &gyroFullScale[0]);
   if (result > worstResult)
     worstResult = result;
 
   // Configure the Accel Only Gain: 15252014 (225Hz) 30504029 (112Hz) 61117001 (56Hz)
   // const unsigned char accelOnlyGain[4] = {0x03, 0xA4, 0x92, 0x49}; // 56Hz
-  const unsigned char accelOnlyGain[4] = {0x00, 0xE8, 0xBA, 0x2E}; // 225Hz
+  const unsigned char accelOnlyGain[4] = { 0x00, 0xE8, 0xBA, 0x2E };  // 225Hz
   // const unsigned char accelOnlyGain[4] = {0x01, 0xD1, 0x74, 0x5D}; // 112Hz
   result = writeDMPmems(ACCEL_ONLY_GAIN, 4, &accelOnlyGain[0]);
   if (result > worstResult)
@@ -604,7 +587,7 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
 
   // Configure the Accel Alpha Var: 1026019965 (225Hz) 977872018 (112Hz) 882002213 (56Hz)
   // const unsigned char accelAlphaVar[4] = {0x34, 0x92, 0x49, 0x25}; // 56Hz
-  const unsigned char accelAlphaVar[4] = {0x3D, 0x27, 0xD2, 0x7D}; // 225Hz
+  const unsigned char accelAlphaVar[4] = { 0x3D, 0x27, 0xD2, 0x7D };  // 225Hz
   // const unsigned char accelAlphaVar[4] = {0x3A, 0x49, 0x24, 0x92}; // 112Hz
   result = writeDMPmems(ACCEL_ALPHA_VAR, 4, &accelAlphaVar[0]);
   if (result > worstResult)
@@ -612,21 +595,21 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void)
 
   // Configure the Accel A Var: 47721859 (225Hz) 95869806 (112Hz) 191739611 (56Hz)
   // const unsigned char accelAVar[4] = {0x0B, 0x6D, 0xB6, 0xDB}; // 56Hz
-  const unsigned char accelAVar[4] = {0x02, 0xD8, 0x2D, 0x83}; // 225Hz
+  const unsigned char accelAVar[4] = { 0x02, 0xD8, 0x2D, 0x83 };  // 225Hz
   // const unsigned char accelAVar[4] = {0x05, 0xB6, 0xDB, 0x6E}; // 112Hz
   result = writeDMPmems(ACCEL_A_VAR, 4, &accelAVar[0]);
   if (result > worstResult)
     worstResult = result;
 
   // Configure the Accel Cal Rate
-  const unsigned char accelCalRate[4] = {0x00, 0x00}; // Value taken from InvenSense Nucleo example
+  const unsigned char accelCalRate[4] = { 0x00, 0x00 };  // Value taken from InvenSense Nucleo example
   result = writeDMPmems(ACCEL_CAL_RATE, 2, &accelCalRate[0]);
   if (result > worstResult)
     worstResult = result;
 
   // Configure the Compass Time Buffer. The I2C Master ODR Configuration (see above) sets the magnetometer read rate to 68.75Hz.
   // Let's set the Compass Time Buffer to 69 (Hz).
-  const unsigned char compassRate[2] = {0x00, 0x45}; // 69Hz
+  const unsigned char compassRate[2] = { 0x00, 0x45 };  // 69Hz
   result = writeDMPmems(CPASS_TIME_BUFFER, 2, &compassRate[0]);
   if (result > worstResult)
     worstResult = result;

From f2379be63fcbf43691440219f7a86cb1822175ae Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Fri, 19 Jun 2026 18:03:02 +0900
Subject: [PATCH 7/9] =?UTF-8?q?README=E4=BF=AE=E6=AD=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 README.md | 73 +++++++++++++++++++++++++++++--------------------------
 1 file changed, 38 insertions(+), 35 deletions(-)

diff --git a/README.md b/README.md
index a8ff113..0ac6bf1 100644
--- a/README.md
+++ b/README.md
@@ -6,41 +6,44 @@ This software is a sample program for the [RT-USB-9axisIMU3](https://www.rt-shop
 The file RT-9DOF-IMU-V3-Quat.ino is forked from the [SparkFun_ICM-20948_ArduinoLibrary examples](https://github.com/sparkfun/SparkFun_ICM-20948_ArduinoLibrary/tree/main/examples).
 
 ## Data format
-|Byte | Content | Description |  
-|-- | -- | -- | --
-|0 | 0xff | Header |  
-|1 | 0xff | Header |  
-|2 | 0x52 | ASCII code "R" |  
-|3 | 0x54 | ASCII code "T" |  
-|4 | 0x40 | Lower Bits of Product Identifier |  
-|5 | 0x41 | Upper Bits of Product Identifier |  
-|6 | 0x01 | FW version |  
-|7 | 0x00~0xff | Timestamp - incremented with each data transmission, wraps to 0x00 after 0xff |  
-|8 | ACC_X_L | ax=(ACC_X_L\|ACC_X_H<<8)/2048*9.8 [m/s] |  
-|9 | ACC_X_H |   |  
-|10 | ACC_Y_L |   |  
-|11 | ACC_Y_H |   |  
-|12 | ACC_Z_L |   |  
-|13 | ACC_Z_H |   |  
-|14 | GYRO_X_L | gx=(GYRO_X_L\|GYRO_X_H<<8)/(32767/2000)*PI/180 [rad/s] |  
-|15 | GYRO_X_H |   |  
-|16 | GYRO_Y_L |   |  
-|17 | GYRO_Y_H |   |  
-|18 | GYRO_Z_L |   |  
-|19 | GYRO_Z_H |   |  
-|20 | Q1_1 | Q1 multiplied by (2^30). q1=(Q1_1 \| Q1_2<<8 \| Q1_3<<16 \| Q1_4<<24 )/(2^30) |  
-|21 | Q1_2 |   |  
-|22 | Q1_3 |   |  
-|23 | Q1_4 |   |  
-|24 | Q2_1 |   |  
-|25 | Q2_2 |   |  
-|26 | Q2_3 |   |  
-|27 | Q2_4 |   |  
-|28 | Q3_1 |   |  
-|29 | Q3_2 |   |  
-|30 | Q3_3 |   |  
-|31 | Q3_4 |   |  
-|32 | Checksum | Lower 8 bits of the sum of all bytes |  
+Baud rate: 2000000 bps  
+Transmission frequency: 1 kHz
+
+|Byte | Content | Description | 
+|-- | -- | -- | 
+|0 | 0xff | Header | 
+|1 | 0xff | Header | 
+|2 | 0x52 | ASCII code "R" | 
+|3 | 0x54 | ASCII code "T" | 
+|4 | 0x40 | Lower Bits of Product Identifier | 
+|5 | 0x41 | Upper Bits of Product Identifier | 
+|6 | 0x01 | FW version | 
+|7 | 0x00~0xff | Timestamp - incremented with each data transmission, wraps to 0x00 after 0xff | 
+|8 | ACC_X_L | ax=(ACC_X_L\|ACC_X_H<<8)/2048*9.8 [m/s] | 
+|9 | ACC_X_H |  | 
+|10 | ACC_Y_L |  | 
+|11 | ACC_Y_H |  | 
+|12 | ACC_Z_L |  | 
+|13 | ACC_Z_H |  | 
+|14 | GYRO_X_L | gx=(GYRO_X_L\|GYRO_X_H<<8)/(32767/2000)*PI/180 [rad/s] | 
+|15 | GYRO_X_H |  | 
+|16 | GYRO_Y_L |  | 
+|17 | GYRO_Y_H |  | 
+|18 | GYRO_Z_L |  | 
+|19 | GYRO_Z_H |  | 
+|20 | Q1_1 | Q1 multiplied by (2^30). q1=(Q1_1 \| Q1_2<<8 \| Q1_3<<16 \| Q1_4<<24 )/(2^30) | 
+|21 | Q1_2 |  | 
+|22 | Q1_3 |  | 
+|23 | Q1_4 |  | 
+|24 | Q2_1 |  | 
+|25 | Q2_2 |  | 
+|26 | Q2_3 |  | 
+|27 | Q2_4 |  | 
+|28 | Q3_1 |  | 
+|29 | Q3_2 |  | 
+|30 | Q3_3 |  | 
+|31 | Q3_4 |  | 
+|32 | Checksum | Lower 8 bits of the sum of all bytes | 
 
 
 ## Flashing from the Arduino IDE

From ab409a0a52cdf933092d4c0d2ebe26c5e7f2b34e Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Thu, 25 Jun 2026 11:46:56 +0900
Subject: [PATCH 8/9] =?UTF-8?q?=E5=8A=A0=E9=80=9F=E5=BA=A616g=E3=82=88?=
 =?UTF-8?q?=E3=81=86=E3=81=AB=E4=BF=AE=E6=AD=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
index 5045a01..de3f1bb 100644
--- a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
+++ b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
@@ -477,12 +477,12 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) {
   // Configure Accel scaling to DMP
   // The DMP scales accel raw data internally to align 1g as 2^25
   // In order to align internal accel raw data 2^25 = 1g write 0x04000000 when FSR is 4g
-  const unsigned char accScale[4] = { 0x04, 0x00, 0x00, 0x00 };
+  const unsigned char accScale[4] = { 0x10, 0x00, 0x00, 0x00 };
   result = writeDMPmems(ACC_SCALE, 4, &accScale[0]);
   if (result > worstResult)
     worstResult = result;  // Write accScale to ACC_SCALE DMP register
   // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g
-  const unsigned char accScale2[4] = { 0x00, 0x04, 0x00, 0x00 };
+  const unsigned char accScale2[4] = { 0x00, 0x10, 0x00, 0x00 };
   result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]);
   if (result > worstResult)
     worstResult = result;  // Write accScale2 to ACC_SCALE2 DMP register

From d3091a1cab238a109b7529f1a39a90443894efa0 Mon Sep 17 00:00:00 2001
From: makihara <makihara@rt-net.jp>
Date: Thu, 25 Jun 2026 16:38:50 +0900
Subject: [PATCH 9/9] =?UTF-8?q?=E3=82=B9=E3=82=B1=E3=83=BC=E3=83=AB?=
 =?UTF-8?q?=E3=82=92=E6=88=BB=E3=81=99ACC=5FSaale2=E3=81=AF1/4=E3=81=AB?=
 =?UTF-8?q?=E3=81=99=E3=82=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
index de3f1bb..9e8d11e 100644
--- a/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
+++ b/STM32duino_sample/RT-9DOF-IMU-V3-Quat/RT-9DOF-IMU-V3-Quat.ino
@@ -482,7 +482,7 @@ ICM_20948_Status_e ICM_20948::initializeDMP(void) {
   if (result > worstResult)
     worstResult = result;  // Write accScale to ACC_SCALE DMP register
   // In order to output hardware unit data as configured FSR write 0x00040000 when FSR is 4g
-  const unsigned char accScale2[4] = { 0x00, 0x10, 0x00, 0x00 };
+  const unsigned char accScale2[4] = { 0x00, 0x01, 0x00, 0x00 };
   result = writeDMPmems(ACC_SCALE2, 4, &accScale2[0]);
   if (result > worstResult)
     worstResult = result;  // Write accScale2 to ACC_SCALE2 DMP register