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@0:9a72d42c0da3, 2011-12-27 (annotated)

Committer:
lpetre
Date:
Tue Dec 27 17:20:06 2011 +0000
Revision:
0:9a72d42c0da3
Child:
1:e27c4c0b71d8

        

        
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

#if 1





lpetre

0:9a72d42c0da3


     5

void WriteBytes(Serial& s, char* b, int count)





lpetre

0:9a72d42c0da3


     6

{





lpetre

0:9a72d42c0da3


     7

    for ( int i = 0; i < count; ++i )





lpetre

0:9a72d42c0da3


     8

        s.putc(b[i]);





lpetre

0:9a72d42c0da3


     9

}





lpetre

0:9a72d42c0da3


    10







lpetre

0:9a72d42c0da3


    11

// Output angles: yaw, pitch, roll





lpetre

0:9a72d42c0da3


    12

void IMU::output_angles()





lpetre

0:9a72d42c0da3


    13

{





lpetre

0:9a72d42c0da3


    14

  if (output_mode == OUTPUT__MODE_ANGLES_BINARY)





lpetre

0:9a72d42c0da3


    15

  {





lpetre

0:9a72d42c0da3


    16

    float ypr[3];  





lpetre

0:9a72d42c0da3


    17

    ypr[0] = TO_DEG(yaw);





lpetre

0:9a72d42c0da3


    18

    ypr[1] = TO_DEG(pitch);





lpetre

0:9a72d42c0da3


    19

    ypr[2] = TO_DEG(roll);





lpetre

0:9a72d42c0da3


    20

    WriteBytes(pc, (char*)ypr, 12); // No new-line





lpetre

0:9a72d42c0da3


    21

  }





lpetre

0:9a72d42c0da3


    22

  else if (output_mode == OUTPUT__MODE_ANGLES_TEXT)





lpetre

0:9a72d42c0da3


    23

  {





lpetre

0:9a72d42c0da3


    24

      pc.printf("#YPR=%f,%f,%f" NEW_LINE,TO_DEG(yaw),TO_DEG(pitch),TO_DEG(roll));





lpetre

0:9a72d42c0da3


    25

  }





lpetre

0:9a72d42c0da3


    26

}





lpetre

0:9a72d42c0da3


    27







lpetre

0:9a72d42c0da3


    28

void IMU::output_calibration(int calibration_sensor)





lpetre

0:9a72d42c0da3


    29

{





lpetre

0:9a72d42c0da3


    30

  if (calibration_sensor == 0)  // Accelerometer





lpetre

0:9a72d42c0da3


    31

  {





lpetre

0:9a72d42c0da3


    32

    // Output MIN/MAX values





lpetre

0:9a72d42c0da3


    33

    pc.printf("accel x,y,z (min/max) = ");





lpetre

0:9a72d42c0da3


    34

    for (int i = 0; i < 3; i++) {





lpetre

0:9a72d42c0da3


    35

      if (accel[i] < accel_min[i]) accel_min[i] = accel[i];





lpetre

0:9a72d42c0da3


    36

      if (accel[i] > accel_max[i]) accel_max[i] = accel[i];





lpetre

0:9a72d42c0da3


    37

      pc.printf("%+5i/%+5i%s", accel_min[i], accel_max[i], (i < 2 ? "  " : "                    "));





lpetre

0:9a72d42c0da3


    38

    }





lpetre

0:9a72d42c0da3


    39

    pc.printf("%c" ,13,10);





lpetre

0:9a72d42c0da3


    40

  }





lpetre

0:9a72d42c0da3


    41

  else if (calibration_sensor == 1)  // Magnetometer





lpetre

0:9a72d42c0da3


    42

  {





lpetre

0:9a72d42c0da3


    43

    // Output MIN/MAX values





lpetre

0:9a72d42c0da3


    44

    pc.printf("magn x,y,z (min/max) = ");





lpetre

0:9a72d42c0da3


    45

    for (int i = 0; i < 3; i++) {





lpetre

0:9a72d42c0da3


    46

      if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i];





lpetre

0:9a72d42c0da3


    47

      if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i];





lpetre

0:9a72d42c0da3


    48

      pc.printf("%+4i/%+4i%s", magnetom_min[i], magnetom_max[i], (i < 2 ? "  " :  "                    "));





lpetre

0:9a72d42c0da3


    49

    }





lpetre

0:9a72d42c0da3


    50

    pc.printf("%c" ,13,10);





lpetre

0:9a72d42c0da3


    51

  }





lpetre

0:9a72d42c0da3


    52

  else if (calibration_sensor == 2)  // Gyroscope





lpetre

0:9a72d42c0da3


    53

  {





lpetre

0:9a72d42c0da3


    54

    // Average gyro values





lpetre

0:9a72d42c0da3


    55

    for (int i = 0; i < 3; i++)





lpetre

0:9a72d42c0da3


    56

      gyro_average[i] += gyro[i];





lpetre

0:9a72d42c0da3


    57

    gyro_num_samples++;





lpetre

0:9a72d42c0da3


    58

      





lpetre

0:9a72d42c0da3


    59

    // Output current and averaged gyroscope values





lpetre

0:9a72d42c0da3


    60

    pc.printf("gyro x,y,z (current/average) = ");





lpetre

0:9a72d42c0da3


    61

    for (int i = 0; i < 3; i++) {





lpetre

0:9a72d42c0da3


    62

      pc.printf("%+5i/%+5i%s",gyro[i], (int16_t)(gyro_average[i] / gyro_num_samples), (i < 2 ? "  " : "                    "));





lpetre

0:9a72d42c0da3


    63

    }





lpetre

0:9a72d42c0da3


    64

    pc.printf("%c" ,13,10);





lpetre

0:9a72d42c0da3


    65

  }





lpetre

0:9a72d42c0da3


    66

}





lpetre

0:9a72d42c0da3


    67







lpetre

0:9a72d42c0da3


    68

void IMU::output_sensors()





lpetre

0:9a72d42c0da3


    69

{





lpetre

0:9a72d42c0da3


    70

  pc.printf("#ACC=%+5i %+5i %+5i ", accel[0],    accel[1],    accel[2]);





lpetre

0:9a72d42c0da3


    71

  pc.printf("#MAG=%+4i %+4i %+4i ", magnetom[0], magnetom[1], magnetom[2]);





lpetre

0:9a72d42c0da3


    72

  pc.printf("#GYR=%+5i %+5i %+5i",  gyro[0],     gyro[1],     gyro[2]);





lpetre

0:9a72d42c0da3


    73

  pc.printf("%c" ,13,10);





lpetre

0:9a72d42c0da3


    74

}





lpetre

0:9a72d42c0da3


    75

#endif