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
- Committer:
- lpetre
- Date:
- 2011-12-27
- Revision:
- 0:9a72d42c0da3
- Child:
- 1:e27c4c0b71d8
File content as of revision 0:9a72d42c0da3:
/* 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