Tyler Weaver / AHRS

AHRS library, modified version of Peter Bartz work.

Dependencies:   MODSERIAL

Dependents:   AHRS_demo

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?

UserRevisionLine numberNew 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 }