Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724
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 }
Generated on Sun Jul 17 2022 22:09:51 by 1.7.2