SparkFun ICM-20948 Arduino で、 DMP Fusion 9軸 がやっとOKになった。

1. Sample & 修正箇所。
Example6_DMP_Quat9_Orientation.ino の抜粋。
.... void setup() { .... // Configuring DMP to output data at multiple ODRs: // DMP is capable of outputting multiple sensor data at different rates to FIFO. // Setting value can be calculated as follows: // 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 -> Max speed is Bad success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Quat9, 5) == ICM_20948_Stat_Ok); // Set to the some value test by nishi 2024.2.19 .... } void loop() { ... // Scale to +/- 1 double q1 = ((double)data.Quat9.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30 double q2 = ((double)data.Quat9.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30 double q3 = ((double)data.Quat9.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30 //double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); // -- W こいつが、バグとの事。 double q0=1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)); if(q0 >= 0.0) q0 = sqrt(q0); else q0 = sqrt(q0 * -1.0) * -1.0; .... // if you feel necessary!! //if(!isnan(q0) && (data.Quat9.Data.Accuracy >= 0 && data.Quat9.Data.Accuracy <= 600)){ ....

上記修正で、多分動くと思う。
あとは、Sampling Rate を、適切に調整すれば、OK だと思う。
DMP is running at 55Hz
とコメントがあるが、おんちゃんは、とりあえず、34[Hz] 辺りで試しています。

実際の動作確認は、ROS2 を使って、Rviz2 で、TF を表示させた状態で、
IMUを回転させて、動きが正常かチェックしないと、判らないぞね。

これで、やっと、自作 Turtlebot3 の IMU で、DMP 9軸 フュージョンが使えるか?
やれやれ。 by nishi 2024.2.19

2. 参照。
ROS2 で試すなら。
humble 版で、
micro-ROS/micro_ros_arduino を使って、PC で、/tf topic を拾う。

サンプルは、同じく、humble版で下記にあるので、実際に、IMU.h のところに、
Example6_DMP_Quat9_Orientation.ino を組み込む。

micro_ros_arduino/tree/humble/examples/micro-ros_tf_publisher
注) IMU.cpp、IMU.h は、無いので、使わずに、Example6_DMP_Quat9_Orientation.ino の内容を組み込む。
または、Example6_DMP_Quat9_Orientation.ino を、IMU.cpp として組み込む。

3. その後。
ROS2 Turtlebot3 自作 に、組み込んで、試しています。
問題なさそう。こちらのほうが、地球の地磁気が使えるので、安心感があるぞね。