Added support of SEN-14001 (9DoF Razor IMU M0)

[lebarsfa]

Resolves #28. The firmware modifications to support the M0 were originally made from the latest version of https://github.com/Razor-AHRS/razor-9dof-ahrs, forked in https://github.com/lebarsfa/razor-9dof-ahrs. Then, the ROS-specific modifications from https://github.com/KristofRobot/razor_imu_9dof were added to add ROS compatibility. Finally, following a suggestion on https://answers.ros.org/question/261398/sparkfun-9dof-razor-imu-sen-14001/, this pull request is proposed, and a pull request to https://github.com/Razor-AHRS/razor-9dof-ahrs will be proposed soon...

[fabolhak]

Hello,
thank you for this contribution. It really helped us a lot using the SEN-14001 in our project :). I just have one question: There is this accel_factor which is in the Arduino code 250 and in the ROS code 256.

Is this on purpose or a mistake? I get better results when using 256 in the Arduino code as well.

[lebarsfa]

Glad to know that it helped!

For the coefficient, it was on purpose, but that does not prevent it to be a mistake...

From what I understand of the different pieces of code, the 256 in ROS code is needed due to the #define GRAVITY 256.0f in Razor_AHRS.ino and the 250 added in Sensors.ino is supposed to compensate the 250 in Razor_AHRS.ino used for default calibration values.

Here is what should happen :

• imu.calcAccel(imu.ax) in Sensors.ino is supposed to give accelerations in g's. Let's assume that it is equal to -1 as an example.
• Therefore, accel[0] = -250.0f*imu.calcAccel(imu.ax) should be 250.
• Then, we have float ACCEL_X_OFFSET = ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) equals to 0 and float ACCEL_X_SCALE = (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET)) equals to 256/250 in Razor_AHRS.ino.
• In Razor_AHRS.ino, the calibration code accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE gives 256.
• Later, ROS code should get somehow the value of accel[0] as float(words[3]) in imuMsg.linear_acceleration.x = -float(words[3]) * accel_factor, so accel_factor = 9.806 / 256.0 makes imuMsg.linear_acceleration.x equal to -9.806.

If this is correct, I would say that your better results with 256 could be probably obtained equivalently without modifying the code, by modifying your accelerometers calibration values ACCEL_X_MIN, ACCEL_X_MAX, etc.

Let me know if there is something wrong...

[fabolhak]

Hello,
thank you for the quick reply and the detailed explanation. At first I was skeptical, because of the hardcoded default calibration parameter 250. But I also did the calculation with different values and the mathematics seem to be correct.

So probably my calibration wasn't the best.

Thank you again :)

[KristofRobot]

@lebarsfa - I am merging this, although I have no way to test this myself. However, I am relying on the testing of @tyzaizl and @fabolhak

Thank you for your contribution!