Fixed the 'cold boot' issue with 14001 razor board

.gitignore

@@ -1,2 +1,4 @@
 *~
 config/my_razor.yaml
+*.pyc
+*.swp

config/razor.yaml

@@ -1,34 +1,48 @@
-## USB port
-port: /dev/ttyUSB0
+port: /dev/ttyACM0
+frame_header: "base_imu_link"
 
 
-##### Calibration ####
-### accelerometer
 accel_x_min: -250.0
 accel_x_max: 250.0
 accel_y_min: -250.0
 accel_y_max: 250.0
 accel_z_min: -250.0
 accel_z_max: 250.0
 
-### magnetometer
-# standard calibration
 magn_x_min: -600.0
 magn_x_max: 600.0
 magn_y_min: -600.0
 magn_y_max: 600.0
 magn_z_min: -600.0
 magn_z_max: 600.0
 
-# extended calibration
+publish_magnetometer: true
+
+# extended mag calibration
 calibration_magn_use_extended: false
 magn_ellipsoid_center: [0, 0, 0]
 magn_ellipsoid_transform: [[0, 0, 0], [0, 0, 0], [0, 0, 0]]
 
 # AHRS to robot calibration
 imu_yaw_calibration: 0.0
 
-### gyroscope
 gyro_average_offset_x: 0.0
 gyro_average_offset_y: 0.0
 gyro_average_offset_z: 0.0
+
+# covariance
+orientation_covariance: [0.0025, 0,      0,
+                         0,      0.0025, 0,
+                         0,      0,      0.0025]
+
+velocity_covariance: [0.002, 0,     0,
+                      0,     0.002, 0,
+                      0,     0,     0.002]
+
+acceleration_covariance: [0.04, 0,    0,
+                          0,    0.04, 0,
+                          0,    0,    0.04]
+
+magnetic_field_covariance: [0.00, 0,    0,
+                            0,    0.00, 0,
+                            0,    0,    0.00]

launch/razor-pub.launch

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="razor_config_file" default="$(find razor_imu_9dof)/config/my_razor.yaml"/>
+  <arg name="razor_config_file" default="$(find razor_imu_9dof)/config/razor.yaml"/>
   <node pkg="razor_imu_9dof" type="imu_node.py" name="imu_node" output="screen">
     <rosparam file="$(arg razor_config_file)" command="load"/>
   </node>

nodes/lib/serial_commands.py

@@ -0,0 +1,50 @@
+STOP_DATASTREAM = '#o0'
+START_DATASTREAM = '#o1'
+
+GET_CALIBRATION_VALUES ='#p'
+
+SET_BINARY_FORMAT = '#ob'
+SET_TEXT_FORMAT = '#ot'
+SET_TEXT_EXTENDED_FORMAT_NO_MAG = '#ox' # Use this one in the ROS driver WITHOUT magnetometer
+SET_TEXT_EXTENDED_FORMAT_WITH_MAG = '#om' # Use this one in the ROS driver WITH magnetometer
+SET_CALIBRATION_OUTPUT_MODE = '#oc'
+NEXT_SENSOR_CALIBRATION = '#on'
+
+LINE_START_NO_MAG = "#YPRAG="
+LINE_START_WITH_MAG = "#YPRAGM="
+
+ENABLE_ERROR_MESSAGE = '#oe1'
+DISABLE_ERROR_MESSAGE = '#oe0'
+OUTPUT_ERROR_COUNT = '#oec'
+OUTPUT_MATH_ERROR_COUNT = '#oem'
+
+SET_CALIB_ACC_X_MIN = '#caxm'
+SET_CALIB_ACC_X_MAX = '#caxM'
+SET_CALIB_ACC_Y_MIN = '#caym'
+SET_CALIB_ACC_Y_MAX = '#cayM'
+SET_CALIB_ACC_Z_MIN = '#cazm'
+SET_CALIB_ACC_Z_MAX = '#cazM'
+
+SET_MAG_X_MIN = '#cmxm'
+SET_MAG_X_MAX = '#cmxM'
+SET_MAG_Y_MIN = '#cmym'
+SET_MAG_Y_MAX = '#cmyM'
+SET_MAG_Z_MIN = '#cmzm'
+SET_MAG_Z_MAX = '#cmzM'
+
+SET_MAG_ELLIPSOID_CENTER_0 = '#ccx'
+SET_MAG_ELLIPSOID_CENTER_1 = '#ccy'
+SET_MAG_ELLIPSOID_CENTER_2 = '#ccz'
+SET_MAG_ELLIPSOID_TRANSFORM_0_0 = '#ctxX'
+SET_MAG_ELLIPSOID_TRANSFORM_0_1 = '#ctxY'
+SET_MAG_ELLIPSOID_TRANSFORM_0_2 = '#ctxZ'
+SET_MAG_ELLIPSOID_TRANSFORM_1_0 = '#ctyX'
+SET_MAG_ELLIPSOID_TRANSFORM_1_1 = '#ctyY'
+SET_MAG_ELLIPSOID_TRANSFORM_1_2 = '#ctyZ'
+SET_MAG_ELLIPSOID_TRANSFORM_2_0 = '#ctzX'
+SET_MAG_ELLIPSOID_TRANSFORM_2_1 = '#ctzY'
+SET_MAG_ELLIPSOID_TRANSFORM_2_2 = '#ctzZ'
+
+SET_GYRO_AVERAGE_OFFSET_X = '#cgx'
+SET_GYRO_AVERAGE_OFFSET_Y = '#cgy'
+SET_GYRO_AVERAGE_OFFSET_Z = '#cgz'

src/Razor_AHRS/Output.ino

@@ -103,6 +103,27 @@ void output_both_angles_and_sensors_text()
   LOG_PORT.print(Gyro_Vector[2]); LOG_PORT.println();
 }
 
+void output_both_angles_and_all_sensors_text()
+{
+  LOG_PORT.print("#YPRAGM=");
+  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(",");
+
+  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(",");
+
+  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.print(",");
+
+    //TODO:Check units and calibration
+  LOG_PORT.print(magnetom[0]); LOG_PORT.print(",");
+  LOG_PORT.print(magnetom[1]); LOG_PORT.print(",");
+  LOG_PORT.print(magnetom[2]); LOG_PORT.println();
+}
+
 void output_sensors_binary()
 {
   LOG_PORT.write((byte*) accel, 12);
@@ -145,4 +166,3 @@ void output_sensors()
     }
   }
 }
-

src/Razor_AHRS/Razor_AHRS.ino

@@ -143,6 +143,12 @@
               velocity is in rad/s. (Output frames have form like
               "#YPRAG=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01",
               followed by carriage return and line feed [\r\n]).
+      "#om" - Output angles and linear acceleration, rotational
+              velocity and magnetic field. Angles are in degrees, acceleration is
+              in units of 1.0 = 1/256 G (9.8/256 m/s^2). Rotational
+              velocity is in rad/s. (Output frames have form like
+              "#YPRAGM=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01,1.0,2.0,3.0",
+              followed by carriage return and line feed [\r\n]).
 
       // Sensor calibration
       "#oc" - Go to CALIBRATION output mode.
@@ -208,7 +214,7 @@
 // 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 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)
@@ -238,6 +244,7 @@
 #define OUTPUT__MODE_SENSORS_RAW 3 // Outputs raw (uncalibrated) sensor values for all 9 axes
 #define OUTPUT__MODE_SENSORS_BOTH 4 // Outputs calibrated AND raw sensor values for all 9 axes
 #define OUTPUT__MODE_ANGLES_AG_SENSORS 5 // Outputs yaw/pitch/roll in degrees + linear accel + rot. vel
+#define OUTPUT__MODE_ANGLES_AGM_SENSORS 6 // Outputs yaw/pitch/roll in degrees + linear accel + rot. vel + magnetometer data
 // Output format definitions (do not change)
 #define OUTPUT__FORMAT_TEXT 0 // Outputs data as text
 #define OUTPUT__FORMAT_BINARY 1 // Outputs data as binary float
@@ -629,12 +636,25 @@ char readChar()
 
 void setup()
 {
-  // Init serial output
-  LOG_PORT.begin(OUTPUT__BAUD_RATE);
-
   // Init status LED
   pinMode (STATUS_LED_PIN, OUTPUT);
   digitalWrite(STATUS_LED_PIN, LOW);
+  double time_old = 0;
+  // Init serial output
+  LOG_PORT.end();
+  delay(50);
+  LOG_PORT.begin(OUTPUT__BAUD_RATE);
+  while(!LOG_PORT){
+    if (millis() - time_old > 10000){
+      time_old = millis();
+      digitalWrite(STATUS_LED_PIN, HIGH);
+      delay(50);
+      #if HW__VERSION_CODE == 14001
+        NVIC_SystemReset();      // processor software reset
+      #endif
+      } 
+    }
+
 
   // Init sensors
   delay(50);  // Give sensors enough time to start
@@ -672,6 +692,9 @@ void loop()
 {
   // Read incoming control messages
  #if HW__VERSION_CODE == 14001
+  if (!LOG_PORT)
+    NVIC_SystemReset();      // processor software reset
+
   // Compatibility fix : if bytes are sent 1 by 1 without being read, available() might never return more than 1...
   // Therefore, we need to read bytes 1 by 1 and the command byte needs to be a blocking read...
   if (LOG_PORT.available() >= 1)
@@ -728,6 +751,11 @@ void loop()
           output_mode = OUTPUT__MODE_ANGLES_AG_SENSORS;
           output_format = OUTPUT__FORMAT_TEXT;
         }
+        else if (output_param == 'm') // Output angles + accel + rot. vel as te_x_t
+        {
+          output_mode = OUTPUT__MODE_ANGLES_AGM_SENSORS;
+          output_format = OUTPUT__FORMAT_TEXT;
+        }
         else if (output_param == 's') // Output _s_ensor values
         {
           char values_param = readChar();
@@ -1013,6 +1041,24 @@ void loop()
 
       if (output_stream_on || output_single_on) output_both_angles_and_sensors_text();
     }
+    else if (output_mode == OUTPUT__MODE_ANGLES_AGM_SENSORS)  // Output angles + accel + rot. vel
+    {
+      // Apply sensor calibration
+      compensate_sensor_errors();
+
+#if DEBUG__USE_ONLY_DMP_M0 == true
+    Euler_angles_only_DMP_M0();
+#else
+      // Run DCM algorithm
+      Compass_Heading(); // Calculate magnetic heading
+      Matrix_update();
+      Normalize();
+      Drift_correction();
+      Euler_angles();
+#endif // DEBUG__USE_ONLY_DMP_M0
+
+      if (output_stream_on || output_single_on) output_both_angles_and_all_sensors_text();
+    }
     else  // Output sensor values
     {      
       if (output_stream_on || output_single_on) output_sensors();