Output.cpp

00001 /* This file is part of the Razor AHRS Firmware */
00002 #include "Razor_AHRS.h"
00003 
00004 void WriteBytes(Serial& s, char* b, int count)
00005 {
00006     for ( int i = 0; i < count; ++i )
00007         s.putc(b[i]);
00008 }
00009 
00010 // Output angles: yaw, pitch, roll
00011 void IMU::output_angles()
00012 {
00013   if (output_mode == OUTPUT__MODE_ANGLES_BINARY)
00014   {
00015     float ypr[3];  
00016     ypr[0] = TO_DEG(yaw);
00017     ypr[1] = TO_DEG(pitch);
00018     ypr[2] = TO_DEG(roll);
00019     WriteBytes(pc, (char*)ypr, 12); // No new-line
00020   }
00021   else if (output_mode == OUTPUT__MODE_ANGLES_TEXT)
00022   {
00023       pc.printf("#YPR=%f,%f,%f" NEW_LINE,TO_DEG(yaw),TO_DEG(pitch),TO_DEG(roll));
00024   }
00025 }
00026 
00027 void IMU::output_calibration(int calibration_sensor)
00028 {
00029   if (calibration_sensor == 0)  // Accelerometer
00030   {
00031     // Output MIN/MAX values
00032     pc.printf("accel x,y,z (min/max) = ");
00033     for (int i = 0; i < 3; i++) {
00034       if (accel[i] < accel_min[i]) accel_min[i] = accel[i];
00035       if (accel[i] > accel_max[i]) accel_max[i] = accel[i];
00036       pc.printf("%+5i/%+5i%s", accel_min[i], accel_max[i], (i < 2 ? "  " : "                    "));
00037     }
00038     pc.printf("%c" ,13,10);
00039   }
00040   else if (calibration_sensor == 1)  // Magnetometer
00041   {
00042     // Output MIN/MAX values
00043     pc.printf("magn x,y,z (min/max) = ");
00044     for (int i = 0; i < 3; i++) {
00045       if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i];
00046       if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i];
00047       pc.printf("%+4i/%+4i%s", magnetom_min[i], magnetom_max[i], (i < 2 ? "  " :  "                    "));
00048     }
00049     pc.printf("%c" ,13,10);
00050   }
00051   else if (calibration_sensor == 2)  // Gyroscope
00052   {
00053     // Average gyro values
00054     for (int i = 0; i < 3; i++)
00055       gyro_average[i] += gyro[i];
00056     gyro_num_samples++;
00057       
00058     // Output current and averaged gyroscope values
00059     pc.printf("gyro x,y,z (current/average) = ");
00060     for (int i = 0; i < 3; i++) {
00061       pc.printf("%+5i/%+5i%s",gyro[i], (int16_t)(gyro_average[i] / gyro_num_samples), (i < 2 ? "  " : "                    "));
00062     }
00063     pc.printf("%c" ,13,10);
00064   }
00065 }
00066 
00067 void IMU::output_sensors()
00068 {
00069   pc.printf("#ACC=%+5i %+5i %+5i ", accel[0],    accel[1],    accel[2]);
00070   pc.printf("#MAG=%+4i %+4i %+4i ", magnetom[0], magnetom[1], magnetom[2]);
00071   pc.printf("#GYR=%+5i %+5i %+5i",  gyro[0],     gyro[1],     gyro[2]);
00072   pc.printf("%c" ,13,10);
00073 }