AHRS library, modified version of Peter Bartz work.
Output.cpp@1:da3b20b5d38a, 2012-11-08 (annotated)
- Committer:
- tylerjw
- Date:
- Thu Nov 08 20:13:35 2012 +0000
- Revision:
- 1:da3b20b5d38a
- Parent:
- 0:014ee3239c80
Modified AHRS library.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tylerjw | 0:014ee3239c80 | 1 | /* This file is part of the Razor AHRS Firmware */ |
tylerjw | 0:014ee3239c80 | 2 | #include "AHRS.h" |
tylerjw | 0:014ee3239c80 | 3 | |
tylerjw | 0:014ee3239c80 | 4 | void WriteBytes(Serial& s, char* b, int count) |
tylerjw | 0:014ee3239c80 | 5 | { |
tylerjw | 0:014ee3239c80 | 6 | for ( int i = 0; i < count; ++i ) |
tylerjw | 0:014ee3239c80 | 7 | s.putc(b[i]); |
tylerjw | 0:014ee3239c80 | 8 | } |
tylerjw | 0:014ee3239c80 | 9 | |
tylerjw | 0:014ee3239c80 | 10 | // Output angles: yaw, pitch, roll |
tylerjw | 0:014ee3239c80 | 11 | void IMU::output_angles() |
tylerjw | 0:014ee3239c80 | 12 | { |
tylerjw | 0:014ee3239c80 | 13 | if (output_mode == OUTPUT__MODE_ANGLES_BINARY) |
tylerjw | 0:014ee3239c80 | 14 | { |
tylerjw | 0:014ee3239c80 | 15 | float ypr[3]; |
tylerjw | 0:014ee3239c80 | 16 | ypr[0] = TO_DEG(yaw); |
tylerjw | 0:014ee3239c80 | 17 | ypr[1] = TO_DEG(pitch); |
tylerjw | 0:014ee3239c80 | 18 | ypr[2] = TO_DEG(roll); |
tylerjw | 0:014ee3239c80 | 19 | WriteBytes(pc, (char*)ypr, 12); // No new-line |
tylerjw | 0:014ee3239c80 | 20 | } |
tylerjw | 0:014ee3239c80 | 21 | else if (output_mode == OUTPUT__MODE_ANGLES_TEXT) |
tylerjw | 0:014ee3239c80 | 22 | { |
tylerjw | 0:014ee3239c80 | 23 | pc.printf("#YPR=%f,%f,%f" NEW_LINE,TO_DEG(yaw),TO_DEG(pitch),TO_DEG(roll)); |
tylerjw | 0:014ee3239c80 | 24 | } |
tylerjw | 0:014ee3239c80 | 25 | } |
tylerjw | 0:014ee3239c80 | 26 | |
tylerjw | 0:014ee3239c80 | 27 | void IMU::output_calibration(int calibration_sensor) |
tylerjw | 0:014ee3239c80 | 28 | { |
tylerjw | 0:014ee3239c80 | 29 | if (calibration_sensor == 0) // Accelerometer |
tylerjw | 0:014ee3239c80 | 30 | { |
tylerjw | 0:014ee3239c80 | 31 | // Output MIN/MAX values |
tylerjw | 0:014ee3239c80 | 32 | pc.printf("accel x,y,z (min/max) = "); |
tylerjw | 0:014ee3239c80 | 33 | for (int i = 0; i < 3; i++) { |
tylerjw | 0:014ee3239c80 | 34 | if (accel[i] < accel_min[i]) accel_min[i] = accel[i]; |
tylerjw | 0:014ee3239c80 | 35 | if (accel[i] > accel_max[i]) accel_max[i] = accel[i]; |
tylerjw | 0:014ee3239c80 | 36 | pc.printf("%+5i/%+5i%s", accel_min[i], accel_max[i], (i < 2 ? " " : " ")); |
tylerjw | 0:014ee3239c80 | 37 | } |
tylerjw | 0:014ee3239c80 | 38 | pc.printf("%c" ,13,10); |
tylerjw | 0:014ee3239c80 | 39 | } |
tylerjw | 0:014ee3239c80 | 40 | else if (calibration_sensor == 1) // Magnetometer |
tylerjw | 0:014ee3239c80 | 41 | { |
tylerjw | 0:014ee3239c80 | 42 | // Output MIN/MAX values |
tylerjw | 0:014ee3239c80 | 43 | pc.printf("magn x,y,z (min/max) = "); |
tylerjw | 0:014ee3239c80 | 44 | for (int i = 0; i < 3; i++) { |
tylerjw | 0:014ee3239c80 | 45 | if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i]; |
tylerjw | 0:014ee3239c80 | 46 | if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i]; |
tylerjw | 0:014ee3239c80 | 47 | pc.printf("%+4i/%+4i%s", magnetom_min[i], magnetom_max[i], (i < 2 ? " " : " ")); |
tylerjw | 0:014ee3239c80 | 48 | } |
tylerjw | 0:014ee3239c80 | 49 | pc.printf("%c" ,13,10); |
tylerjw | 0:014ee3239c80 | 50 | } |
tylerjw | 0:014ee3239c80 | 51 | else if (calibration_sensor == 2) // Gyroscope |
tylerjw | 0:014ee3239c80 | 52 | { |
tylerjw | 0:014ee3239c80 | 53 | // Average gyro values |
tylerjw | 0:014ee3239c80 | 54 | gyro_num_samples++; |
tylerjw | 0:014ee3239c80 | 55 | for (int i = 0; i < 3; i++) |
tylerjw | 0:014ee3239c80 | 56 | gyro_average[i] += (double)gyro[i]; |
tylerjw | 0:014ee3239c80 | 57 | |
tylerjw | 0:014ee3239c80 | 58 | |
tylerjw | 0:014ee3239c80 | 59 | // Output current and averaged gyroscope values |
tylerjw | 0:014ee3239c80 | 60 | pc.printf("gyro x,y,z (current/average) = "); |
tylerjw | 0:014ee3239c80 | 61 | for (int i = 0; i < 3; i++) { |
tylerjw | 0:014ee3239c80 | 62 | pc.printf("%+5i/%+5.2f%s",gyro[i], (gyro_average[i] / (double)gyro_num_samples), (i < 2 ? " " : " ")); |
tylerjw | 0:014ee3239c80 | 63 | } |
tylerjw | 0:014ee3239c80 | 64 | pc.printf("T:%f",temperature); |
tylerjw | 0:014ee3239c80 | 65 | pc.printf("%c" ,13,10); |
tylerjw | 0:014ee3239c80 | 66 | } |
tylerjw | 0:014ee3239c80 | 67 | } |
tylerjw | 0:014ee3239c80 | 68 | |
tylerjw | 0:014ee3239c80 | 69 | void IMU::output_sensors() |
tylerjw | 0:014ee3239c80 | 70 | { |
tylerjw | 0:014ee3239c80 | 71 | pc.printf("#ACC=%+5i %+5i %+5i ", accel[0], accel[1], accel[2]); |
tylerjw | 0:014ee3239c80 | 72 | pc.printf("#MAG=%+4i %+4i %+4i ", magnetom[0], magnetom[1], magnetom[2]); |
tylerjw | 0:014ee3239c80 | 73 | pc.printf("#GYR=%+5i %+5i %+5i", gyro[0], gyro[1], gyro[2]); |
tylerjw | 0:014ee3239c80 | 74 | pc.printf("%c" ,13,10); |
tylerjw | 0:014ee3239c80 | 75 | } |