Luke Petre / Mbed 2 deprecated RazorAHRS

Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724

Dependencies:   mbed

Output.cpp@1:e27c4c0b71d8, 2011-12-28 (annotated)

Committer:
lpetre
Date:
Wed Dec 28 17:13:14 2011 +0000
Revision:
1:e27c4c0b71d8
Parent:
0:9a72d42c0da3 
Removed custom settings

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lpetre 0:9a72d42c0da3 1 /* This file is part of the Razor AHRS Firmware */
lpetre 0:9a72d42c0da3 2 #include "Razor_AHRS.h"
lpetre 0:9a72d42c0da3 3
lpetre 0:9a72d42c0da3 4 void WriteBytes(Serial& s, char* b, int count)
lpetre 0:9a72d42c0da3 5 {
lpetre 0:9a72d42c0da3 6 for ( int i = 0; i < count; ++i )
lpetre 0:9a72d42c0da3 7 s.putc(b[i]);
lpetre 0:9a72d42c0da3 8 }
lpetre 0:9a72d42c0da3 9
lpetre 0:9a72d42c0da3 10 // Output angles: yaw, pitch, roll
lpetre 0:9a72d42c0da3 11 void IMU::output_angles()
lpetre 0:9a72d42c0da3 12 {
lpetre 0:9a72d42c0da3 13 if (output_mode == OUTPUT__MODE_ANGLES_BINARY)
lpetre 0:9a72d42c0da3 14 {
lpetre 0:9a72d42c0da3 15 float ypr[3];
lpetre 0:9a72d42c0da3 16 ypr[0] = TO_DEG(yaw);
lpetre 0:9a72d42c0da3 17 ypr[1] = TO_DEG(pitch);
lpetre 0:9a72d42c0da3 18 ypr[2] = TO_DEG(roll);
lpetre 0:9a72d42c0da3 19 WriteBytes(pc, (char*)ypr, 12); // No new-line
lpetre 0:9a72d42c0da3 20 }
lpetre 0:9a72d42c0da3 21 else if (output_mode == OUTPUT__MODE_ANGLES_TEXT)
lpetre 0:9a72d42c0da3 22 {
lpetre 0:9a72d42c0da3 23 pc.printf("#YPR=%f,%f,%f" NEW_LINE,TO_DEG(yaw),TO_DEG(pitch),TO_DEG(roll));
lpetre 0:9a72d42c0da3 24 }
lpetre 0:9a72d42c0da3 25 }
lpetre 0:9a72d42c0da3 26
lpetre 0:9a72d42c0da3 27 void IMU::output_calibration(int calibration_sensor)
lpetre 0:9a72d42c0da3 28 {
lpetre 0:9a72d42c0da3 29 if (calibration_sensor == 0) // Accelerometer
lpetre 0:9a72d42c0da3 30 {
lpetre 0:9a72d42c0da3 31 // Output MIN/MAX values
lpetre 0:9a72d42c0da3 32 pc.printf("accel x,y,z (min/max) = ");
lpetre 0:9a72d42c0da3 33 for (int i = 0; i < 3; i++) {
lpetre 0:9a72d42c0da3 34 if (accel[i] < accel_min[i]) accel_min[i] = accel[i];
lpetre 0:9a72d42c0da3 35 if (accel[i] > accel_max[i]) accel_max[i] = accel[i];
lpetre 0:9a72d42c0da3 36 pc.printf("%+5i/%+5i%s", accel_min[i], accel_max[i], (i < 2 ? " " : " "));
lpetre 0:9a72d42c0da3 37 }
lpetre 0:9a72d42c0da3 38 pc.printf("%c" ,13,10);
lpetre 0:9a72d42c0da3 39 }
lpetre 0:9a72d42c0da3 40 else if (calibration_sensor == 1) // Magnetometer
lpetre 0:9a72d42c0da3 41 {
lpetre 0:9a72d42c0da3 42 // Output MIN/MAX values
lpetre 0:9a72d42c0da3 43 pc.printf("magn x,y,z (min/max) = ");
lpetre 0:9a72d42c0da3 44 for (int i = 0; i < 3; i++) {
lpetre 0:9a72d42c0da3 45 if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i];
lpetre 0:9a72d42c0da3 46 if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i];
lpetre 0:9a72d42c0da3 47 pc.printf("%+4i/%+4i%s", magnetom_min[i], magnetom_max[i], (i < 2 ? " " : " "));
lpetre 0:9a72d42c0da3 48 }
lpetre 0:9a72d42c0da3 49 pc.printf("%c" ,13,10);
lpetre 0:9a72d42c0da3 50 }
lpetre 0:9a72d42c0da3 51 else if (calibration_sensor == 2) // Gyroscope
lpetre 0:9a72d42c0da3 52 {
lpetre 0:9a72d42c0da3 53 // Average gyro values
lpetre 0:9a72d42c0da3 54 for (int i = 0; i < 3; i++)
lpetre 0:9a72d42c0da3 55 gyro_average[i] += gyro[i];
lpetre 0:9a72d42c0da3 56 gyro_num_samples++;
lpetre 0:9a72d42c0da3 57
lpetre 0:9a72d42c0da3 58 // Output current and averaged gyroscope values
lpetre 0:9a72d42c0da3 59 pc.printf("gyro x,y,z (current/average) = ");
lpetre 0:9a72d42c0da3 60 for (int i = 0; i < 3; i++) {
lpetre 0:9a72d42c0da3 61 pc.printf("%+5i/%+5i%s",gyro[i], (int16_t)(gyro_average[i] / gyro_num_samples), (i < 2 ? " " : " "));
lpetre 0:9a72d42c0da3 62 }
lpetre 0:9a72d42c0da3 63 pc.printf("%c" ,13,10);
lpetre 0:9a72d42c0da3 64 }
lpetre 0:9a72d42c0da3 65 }
lpetre 0:9a72d42c0da3 66
lpetre 0:9a72d42c0da3 67 void IMU::output_sensors()
lpetre 0:9a72d42c0da3 68 {
lpetre 0:9a72d42c0da3 69 pc.printf("#ACC=%+5i %+5i %+5i ", accel[0], accel[1], accel[2]);
lpetre 0:9a72d42c0da3 70 pc.printf("#MAG=%+4i %+4i %+4i ", magnetom[0], magnetom[1], magnetom[2]);
lpetre 0:9a72d42c0da3 71 pc.printf("#GYR=%+5i %+5i %+5i", gyro[0], gyro[1], gyro[2]);
lpetre 0:9a72d42c0da3 72 pc.printf("%c" ,13,10);
lpetre 0:9a72d42c0da3 73 }