New output mode to support ROS IMU use emits YPR + accel + rot. vel.

Arduino/Razor_AHRS/Output.ino

@@ -87,6 +87,22 @@ void output_sensors_text(char raw_or_calibrated)
   Serial.print(gyro[2]); Serial.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(",");
+
+  Serial.print(Accel_Vector[0]); Serial.print(",");
+  Serial.print(Accel_Vector[1]); Serial.print(",");
+  Serial.print(Accel_Vector[2]); Serial.print(",");
+
+  Serial.print(Gyro_Vector[0]); Serial.print(",");
+  Serial.print(Gyro_Vector[1]); Serial.print(",");
+  Serial.print(Gyro_Vector[2]); Serial.println();
+}
+
 void output_sensors_binary()
 {
   Serial.write((byte*) accel, 12);

Arduino/Razor_AHRS/Razor_AHRS.ino

@@ -1,5 +1,5 @@
 /***************************************************************************************************************
-* Razor AHRS Firmware v1.4.2
+* Razor AHRS Firmware v1.4.2.1
 * 9 Degree of Measurement Attitude and Heading Reference System
 * for Sparkfun "9DOF Razor IMU" (SEN-10125 and SEN-10736)
 * and "9DOF Sensor Stick" (SEN-10183, 10321 and SEN-10724)
@@ -44,6 +44,8 @@
 *       * Added static magnetometer soft iron distortion compensation
 *     * v1.4.2
 *       * (No core firmware changes)
+*     * v1.4.2.1
+*       * New output mode to support ROS Imu use emits YPR + accel + rot. vel.
 *
 * TODOs:
 *   * Allow optional use of EEPROM for storing and reading calibration values.
@@ -99,7 +101,12 @@
               is 3x4 = 12 bytes long).
       "#ot" - Output angles in TEXT format (Output frames have form like "#YPR=-142.28,-5.38,33.52",
               followed by carriage return and line feed [\r\n]).
-
+      "#ox" - Output angles and linear acceleration and rotational
+              velocity. 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^2. (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]).
       // Sensor calibration
       "#oc" - Go to CALIBRATION output mode.
       "#on" - When in calibration mode, go on to calibrate NEXT sensor.
@@ -180,6 +187,7 @@
 #define OUTPUT__MODE_SENSORS_CALIB 2 // Outputs calibrated sensor values for all 9 axes
 #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
 // 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
@@ -561,6 +569,11 @@ void loop()
           output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
           reset_calibration_session_flag = true;
         }
+        else if (output_param == 'x') // Go to _c_alibration mode for both sensor and angle comment: Tang
+        {
+          output_mode = OUTPUT__MODE_ANGLES_AG_SENSORS;
+          reset_calibration_session_flag = true;
+        }
         else if (output_param == 's') // Output _s_ensor values
         {
           char values_param = readChar();
@@ -645,6 +658,20 @@ void loop()
 
       if (output_stream_on || output_single_on) output_angles();
     }
+    else if (output_mode == OUTPUT__MODE_ANGLES_AG_SENSORS)  // Output angles + accel + rot. vel
+    {
+      // Apply sensor calibration
+      compensate_sensor_errors();
+
+      // Run DCM algorithm
+      Compass_Heading(); // Calculate magnetic heading
+      Matrix_update();
+      Normalize();
+      Drift_correction();
+      Euler_angles();
+
+      if (output_stream_on || output_single_on) output_both_angles_and_sensors_text();
+    }
     else  // Output sensor values
     {      
       if (output_stream_on || output_single_on) output_sensors();