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

CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package razor_imu_9dof
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.2.0 (20-12-2017)
+------------------
+* Adding firmware support for SEN-14001 (9DoF Razor IMU M0) from https://github.com/lebarsfa/razor-9dof-ahrs (`#28 <https://github.com/KristofRobot/razor_imu_9dof/issues/28>`_).
+
+    * Note: 
+		* For SEN-14001 (9DoF Razor IMU M0), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU).
+		* There is also a minor update in the calibration tools provided.
+
 1.1.1 (02-07-2016)
 ------------------
 * Passing razor_config_file as ros parameter in launch file (Daniel Koguciuk)

README.md

@@ -21,24 +21,27 @@ Install and Configure ROS Package
 
 Install Arduino firmware
 -------------------------
-1) Open ``src/Razor_AHRS/Razor_AHRS.ino`` in Arduino IDE. Note: this is a modified version
+1) For SEN-14001 (9DoF Razor IMU M0), you will need to follow the same instructions as for the default firmware on https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide and use an updated version of SparkFun_MPU-9250-DMP_Arduino_Library from https://github.com/lebarsfa/SparkFun_MPU-9250-DMP_Arduino_Library (an updated version of the default firmware is also available on https://github.com/lebarsfa/9DOF_Razor_IMU).
+
+2) Open ``src/Razor_AHRS/Razor_AHRS.ino`` in Arduino IDE. Note: this is a modified version
 of Peter Bartz' original Arduino code (see https://github.com/ptrbrtz/razor-9dof-ahrs). 
 Use this version - it emits linear acceleration and angular velocity data required by the ROS Imu message
 
-2) Select your hardware here by uncommenting the right line in ``src/Razor_AHRS/Razor_AHRS.ino``, e.g.
+3) Select your hardware here by uncommenting the right line in ``src/Razor_AHRS/Razor_AHRS.ino``, e.g.
 
 <pre>
 // HARDWARE OPTIONS
 /*****************************************************************/
 // Select your hardware here by uncommenting one line!
 //#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
 //#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
+#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001"
 //#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
 //#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
-#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
+//#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
 </pre>
 
-3) Upload Arduino sketch to the Sparkfun 9DOF Razor IMU board
+4) Upload Arduino sketch to the Sparkfun 9DOF Razor IMU board
 
 
 Configure
@@ -80,7 +83,7 @@ For best accuracy, follow the tutorial to calibrate the sensors:
 
 http://wiki.ros.org/razor_imu_9dof
 
-A copy of Peter Bartz's magnetometer calibration scripts from https://github.com/ptrbrtz/razor-9dof-ahrs is provided in the ``magnetometer_calibration`` directory.
+An updated version of Peter Bartz's magnetometer calibration scripts from https://github.com/ptrbrtz/razor-9dof-ahrs is provided in the ``magnetometer_calibration`` directory.
 
 Update ``my_razor.yaml`` with the new calibration parameters.
 

magnetometer_calibration/Matlab/magnetometer_calibration/magnetometer_calibration.m

@@ -35,9 +35,9 @@
 % output info
 fprintf('In the Razor_AHRS.ino, under "SENSOR CALIBRATION" find the section that reads "Magnetometer (extended calibration)"\n');
 fprintf('Replace the existing 3 lines with these:\n\n');
-fprintf('#define CALIBRATION__MAGN_USE_EXTENDED true\n');
-fprintf('const float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n', e_center);
-fprintf('const float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n', comp);
+fprintf('boolean CALIBRATION__MAGN_USE_EXTENDED = true;\n');
+fprintf('float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n', e_center);
+fprintf('float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n', comp);
 
 % draw ellipsoid fit
 figure;

magnetometer_calibration/Processing/Magnetometer_calibration/Magnetometer_calibration.pde

@@ -366,9 +366,9 @@ void outputCalibration() {
   // Output calibration
   System.out.printf("In the Razor_AHRS.ino, under 'SENSOR CALIBRATION' find the section that reads 'Magnetometer (extended calibration)'\n");
   System.out.printf("Replace the existing 3 lines with these:\n\n");
-  System.out.printf("#define CALIBRATION__MAGN_USE_EXTENDED true\n");
-  System.out.printf("const float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n", center.get(0), center.get(1), center.get(2));
-  System.out.printf("const float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n",
+  System.out.printf("boolean CALIBRATION__MAGN_USE_EXTENDED = true;\n");
+  System.out.printf("float magn_ellipsoid_center[3] = {%.6g, %.6g, %.6g};\n", center.get(0), center.get(1), center.get(2));
+  System.out.printf("float magn_ellipsoid_transform[3][3] = {{%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}, {%.6g, %.6g, %.6g}};\n",
     comp.get(0), comp.get(1), comp.get(2), comp.get(3), comp.get(4), comp.get(5), comp.get(6), comp.get(7), comp.get(8));
   println("\n");
 }

package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>razor_imu_9dof</name>
-  <version>1.1.1</version>
+  <version>1.2.0</version>
   <description>
      razor_imu_9dof is a package that provides a ROS driver
      for the Sparkfun Razor IMU 9DOF. It also provides Arduino

src/Razor_AHRS/Compass.ino

@@ -2,12 +2,12 @@
 
 void Compass_Heading()
 {
-  float mag_x;
-  float mag_y;
-  float cos_roll;
-  float sin_roll;
-  float cos_pitch;
-  float sin_pitch;
+  float mag_x = 0;
+  float mag_y = 0;
+  float cos_roll = 0;
+  float sin_roll = 0;
+  float cos_pitch = 0;
+  float sin_pitch = 0;
 
   cos_roll = cos(roll);
   sin_roll = sin(roll);

src/Razor_AHRS/DCM.ino

@@ -32,14 +32,14 @@ void Normalize(void)
 /**************************************************/
 void Drift_correction(void)
 {
-  float mag_heading_x;
-  float mag_heading_y;
-  float errorCourse;
+  float mag_heading_x = 0;
+  float mag_heading_y = 0;
+  float errorCourse = 0;
   //Compensation the Roll, Pitch and Yaw drift. 
   static float Scaled_Omega_P[3];
   static float Scaled_Omega_I[3];
-  float Accel_magnitude;
-  float Accel_weight;
+  float Accel_magnitude = 0;
+  float Accel_weight = 0;
 
 
   //*****Roll and Pitch***************
@@ -85,27 +85,30 @@ void Matrix_update(void)
   Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
   Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
 
-#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
-  Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
-  Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
-  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][2]=0;
-#else // Use drift correction
-  Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
-  Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
-  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
-  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
-  Update_Matrix[2][2]=0;
-#endif
+  if (DEBUG__NO_DRIFT_CORRECTION) // Do not use drift correction
+  {
+	Update_Matrix[0][0] = 0;
+	Update_Matrix[0][1] = -G_Dt*Gyro_Vector[2];//-z
+	Update_Matrix[0][2] = G_Dt*Gyro_Vector[1];//y
+	Update_Matrix[1][0] = G_Dt*Gyro_Vector[2];//z
+	Update_Matrix[1][1] = 0;
+	Update_Matrix[1][2] = -G_Dt*Gyro_Vector[0];
+	Update_Matrix[2][0] = -G_Dt*Gyro_Vector[1];
+	Update_Matrix[2][1] = G_Dt*Gyro_Vector[0];
+	Update_Matrix[2][2] = 0;
+  }
+  else // Use drift correction
+  {
+	Update_Matrix[0][0] = 0;
+	Update_Matrix[0][1] = -G_Dt*Omega_Vector[2];//-z
+	Update_Matrix[0][2] = G_Dt*Omega_Vector[1];//y
+	Update_Matrix[1][0] = G_Dt*Omega_Vector[2];//z
+	Update_Matrix[1][1] = 0;
+	Update_Matrix[1][2] = -G_Dt*Omega_Vector[0];//-x
+	Update_Matrix[2][0] = -G_Dt*Omega_Vector[1];//-y
+	Update_Matrix[2][1] = G_Dt*Omega_Vector[0];//x
+	Update_Matrix[2][2] = 0;
+  }
 
   Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
 
@@ -120,8 +123,7 @@ void Matrix_update(void)
 
 void Euler_angles(void)
 {
-  pitch = -asin(DCM_Matrix[2][0]);
+  if ((DCM_Matrix[2][0] < -1)||(DCM_Matrix[2][0] > 1)) num_math_errors++; else pitch = -asin(DCM_Matrix[2][0]); // Attempt to prevent nan problems...
   roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
   yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
 }
-

src/Razor_AHRS/Output.ino

@@ -9,14 +9,14 @@ void output_angles()
     ypr[0] = TO_DEG(yaw);
     ypr[1] = TO_DEG(pitch);
     ypr[2] = TO_DEG(roll);
-    Serial.write((byte*) ypr, 12);  // No new-line
+    LOG_PORT.write((byte*) ypr, 12);  // No new-line
   }
   else if (output_format == OUTPUT__FORMAT_TEXT)
   {
-    Serial.print("#YPR=");
-    Serial.print(TO_DEG(yaw)); Serial.print(",");
-    Serial.print(TO_DEG(pitch)); Serial.print(",");
-    Serial.print(TO_DEG(roll)); Serial.println();
+    LOG_PORT.print("#YPR=");
+    LOG_PORT.print(TO_DEG(yaw)); LOG_PORT.print(",");
+    LOG_PORT.print(TO_DEG(pitch)); LOG_PORT.print(",");
+    LOG_PORT.print(TO_DEG(roll)); LOG_PORT.println();
   }
 }
 
@@ -25,29 +25,29 @@ void output_calibration(int calibration_sensor)
   if (calibration_sensor == 0)  // Accelerometer
   {
     // Output MIN/MAX values
-    Serial.print("accel x,y,z (min/max) = ");
+    LOG_PORT.print("accel x,y,z (min/max) = ");
     for (int i = 0; i < 3; i++) {
       if (accel[i] < accel_min[i]) accel_min[i] = accel[i];
       if (accel[i] > accel_max[i]) accel_max[i] = accel[i];
-      Serial.print(accel_min[i]);
-      Serial.print("/");
-      Serial.print(accel_max[i]);
-      if (i < 2) Serial.print("  ");
-      else Serial.println();
+      LOG_PORT.print(accel_min[i]);
+      LOG_PORT.print("/");
+      LOG_PORT.print(accel_max[i]);
+      if (i < 2) LOG_PORT.print("  ");
+      else LOG_PORT.println();
     }
   }
   else if (calibration_sensor == 1)  // Magnetometer
   {
     // Output MIN/MAX values
-    Serial.print("magn x,y,z (min/max) = ");
+    LOG_PORT.print("magn x,y,z (min/max) = ");
     for (int i = 0; i < 3; i++) {
       if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i];
       if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i];
-      Serial.print(magnetom_min[i]);
-      Serial.print("/");
-      Serial.print(magnetom_max[i]);
-      if (i < 2) Serial.print("  ");
-      else Serial.println();
+      LOG_PORT.print(magnetom_min[i]);
+      LOG_PORT.print("/");
+      LOG_PORT.print(magnetom_max[i]);
+      if (i < 2) LOG_PORT.print("  ");
+      else LOG_PORT.println();
     }
   }
   else if (calibration_sensor == 2)  // Gyroscope
@@ -58,56 +58,56 @@ void output_calibration(int calibration_sensor)
     gyro_num_samples++;
 
     // Output current and averaged gyroscope values
-    Serial.print("gyro x,y,z (current/average) = ");
+    LOG_PORT.print("gyro x,y,z (current/average) = ");
     for (int i = 0; i < 3; i++) {
-      Serial.print(gyro[i]);
-      Serial.print("/");
-      Serial.print(gyro_average[i] / (float) gyro_num_samples);
-      if (i < 2) Serial.print("  ");
-      else Serial.println();
+      LOG_PORT.print(gyro[i]);
+      LOG_PORT.print("/");
+      LOG_PORT.print(gyro_average[i] / (float) gyro_num_samples);
+      if (i < 2) LOG_PORT.print("  ");
+      else LOG_PORT.println();
     }
   }
 }
 
 void output_sensors_text(char raw_or_calibrated)
 {
-  Serial.print("#A-"); Serial.print(raw_or_calibrated); Serial.print('=');
-  Serial.print(accel[0]); Serial.print(",");
-  Serial.print(accel[1]); Serial.print(",");
-  Serial.print(accel[2]); Serial.println();
+  LOG_PORT.print("#A-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('=');
+  LOG_PORT.print(accel[0]); LOG_PORT.print(",");
+  LOG_PORT.print(accel[1]); LOG_PORT.print(",");
+  LOG_PORT.print(accel[2]); LOG_PORT.println();
 
-  Serial.print("#M-"); Serial.print(raw_or_calibrated); Serial.print('=');
-  Serial.print(magnetom[0]); Serial.print(",");
-  Serial.print(magnetom[1]); Serial.print(",");
-  Serial.print(magnetom[2]); Serial.println();
+  LOG_PORT.print("#M-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('=');
+  LOG_PORT.print(magnetom[0]); LOG_PORT.print(",");
+  LOG_PORT.print(magnetom[1]); LOG_PORT.print(",");
+  LOG_PORT.print(magnetom[2]); LOG_PORT.println();
 
-  Serial.print("#G-"); Serial.print(raw_or_calibrated); Serial.print('=');
-  Serial.print(gyro[0]); Serial.print(",");
-  Serial.print(gyro[1]); Serial.print(",");
-  Serial.print(gyro[2]); Serial.println();
+  LOG_PORT.print("#G-"); LOG_PORT.print(raw_or_calibrated); LOG_PORT.print('=');
+  LOG_PORT.print(gyro[0]); LOG_PORT.print(",");
+  LOG_PORT.print(gyro[1]); LOG_PORT.print(",");
+  LOG_PORT.print(gyro[2]); LOG_PORT.println();
 }
 
 void output_both_angles_and_sensors_text()
 {
-  Serial.print("#YPRAG=");
-  Serial.print(TO_DEG(yaw)); Serial.print(",");
-  Serial.print(TO_DEG(pitch)); Serial.print(",");
-  Serial.print(TO_DEG(roll)); Serial.print(",");
+  LOG_PORT.print("#YPRAG=");
+  LOG_PORT.print(TO_DEG(yaw)); LOG_PORT.print(",");
+  LOG_PORT.print(TO_DEG(pitch)); LOG_PORT.print(",");
+  LOG_PORT.print(TO_DEG(roll)); LOG_PORT.print(",");
 
-  Serial.print(Accel_Vector[0]); Serial.print(",");
-  Serial.print(Accel_Vector[1]); Serial.print(",");
-  Serial.print(Accel_Vector[2]); Serial.print(",");
+  LOG_PORT.print(Accel_Vector[0]); LOG_PORT.print(",");
+  LOG_PORT.print(Accel_Vector[1]); LOG_PORT.print(",");
+  LOG_PORT.print(Accel_Vector[2]); LOG_PORT.print(",");
 
-  Serial.print(Gyro_Vector[0]); Serial.print(",");
-  Serial.print(Gyro_Vector[1]); Serial.print(",");
-  Serial.print(Gyro_Vector[2]); Serial.println();
+  LOG_PORT.print(Gyro_Vector[0]); LOG_PORT.print(",");
+  LOG_PORT.print(Gyro_Vector[1]); LOG_PORT.print(",");
+  LOG_PORT.print(Gyro_Vector[2]); LOG_PORT.println();
 }
 
 void output_sensors_binary()
 {
-  Serial.write((byte*) accel, 12);
-  Serial.write((byte*) magnetom, 12);
-  Serial.write((byte*) gyro, 12);
+  LOG_PORT.write((byte*) accel, 12);
+  LOG_PORT.write((byte*) magnetom, 12);
+  LOG_PORT.write((byte*) gyro, 12);
 }
 
 void output_sensors()