Diff: Output.cpp

Revision:

0:9a72d42c0da3

Child:

1:e27c4c0b71d8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Output.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,75 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "Razor_AHRS.h"
+
+#if 1
+void WriteBytes(Serial& s, char* b, int count)
+{
+    for ( int i = 0; i < count; ++i )
+        s.putc(b[i]);
+}
+
+// Output angles: yaw, pitch, roll
+void IMU::output_angles()
+{
+  if (output_mode == OUTPUT__MODE_ANGLES_BINARY)
+  {
+    float ypr[3];  
+    ypr[0] = TO_DEG(yaw);
+    ypr[1] = TO_DEG(pitch);
+    ypr[2] = TO_DEG(roll);
+    WriteBytes(pc, (char*)ypr, 12); // No new-line
+  }
+  else if (output_mode == OUTPUT__MODE_ANGLES_TEXT)
+  {
+      pc.printf("#YPR=%f,%f,%f" NEW_LINE,TO_DEG(yaw),TO_DEG(pitch),TO_DEG(roll));
+  }
+}
+
+void IMU::output_calibration(int calibration_sensor)
+{
+  if (calibration_sensor == 0)  // Accelerometer
+  {
+    // Output MIN/MAX values
+    pc.printf("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];
+      pc.printf("%+5i/%+5i%s", accel_min[i], accel_max[i], (i < 2 ? "  " : "                    "));
+    }
+    pc.printf("%c" ,13,10);
+  }
+  else if (calibration_sensor == 1)  // Magnetometer
+  {
+    // Output MIN/MAX values
+    pc.printf("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];
+      pc.printf("%+4i/%+4i%s", magnetom_min[i], magnetom_max[i], (i < 2 ? "  " :  "                    "));
+    }
+    pc.printf("%c" ,13,10);
+  }
+  else if (calibration_sensor == 2)  // Gyroscope
+  {
+    // Average gyro values
+    for (int i = 0; i < 3; i++)
+      gyro_average[i] += gyro[i];
+    gyro_num_samples++;
+      
+    // Output current and averaged gyroscope values
+    pc.printf("gyro x,y,z (current/average) = ");
+    for (int i = 0; i < 3; i++) {
+      pc.printf("%+5i/%+5i%s",gyro[i], (int16_t)(gyro_average[i] / gyro_num_samples), (i < 2 ? "  " : "                    "));
+    }
+    pc.printf("%c" ,13,10);
+  }
+}
+
+void IMU::output_sensors()
+{
+  pc.printf("#ACC=%+5i %+5i %+5i ", accel[0],    accel[1],    accel[2]);
+  pc.printf("#MAG=%+4i %+4i %+4i ", magnetom[0], magnetom[1], magnetom[2]);
+  pc.printf("#GYR=%+5i %+5i %+5i",  gyro[0],     gyro[1],     gyro[2]);
+  pc.printf("%c" ,13,10);
+}
+#endif