Aaron Berk
SparkFun 9DOF Razor IMU with AHRS [http://code.google.com/p/sf9domahrs/]
dof9RazorImuAhrs.cpp@0:ef171fe2a7e5, 2010-06-17 (annotated)
- Committer:
- aberk
- Date:
- Thu Jun 17 10:05:46 2010 +0000
- Revision:
- 0:ef171fe2a7e5
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| aberk | 0:ef171fe2a7e5 | 1 | //****************************************************************************/ |
| aberk | 0:ef171fe2a7e5 | 2 | // Description: |
| aberk | 0:ef171fe2a7e5 | 3 | // |
| aberk | 0:ef171fe2a7e5 | 4 | // Read attitude and heading reference system [AHRS] data from the SparkFun |
| aberk | 0:ef171fe2a7e5 | 5 | // 9DOF Razor Inertial Measurement Unit [IMU]. |
| aberk | 0:ef171fe2a7e5 | 6 | // |
| aberk | 0:ef171fe2a7e5 | 7 | // AHRS code: |
| aberk | 0:ef171fe2a7e5 | 8 | // |
| aberk | 0:ef171fe2a7e5 | 9 | // http://code.google.com/p/sf9domahrs/ |
| aberk | 0:ef171fe2a7e5 | 10 | // |
| aberk | 0:ef171fe2a7e5 | 11 | // 9DOF Razor IMU: |
| aberk | 0:ef171fe2a7e5 | 12 | // |
| aberk | 0:ef171fe2a7e5 | 13 | // http://www.sparkfun.com/commerce/product_info.php?products_id=9623 |
| aberk | 0:ef171fe2a7e5 | 14 | //****************************************************************************/ |
| aberk | 0:ef171fe2a7e5 | 15 | |
| aberk | 0:ef171fe2a7e5 | 16 | //****************************************************************************/ |
| aberk | 0:ef171fe2a7e5 | 17 | // Includes |
| aberk | 0:ef171fe2a7e5 | 18 | //****************************************************************************/ |
| aberk | 0:ef171fe2a7e5 | 19 | #include "dof9RazorImuAhrs.h" |
| aberk | 0:ef171fe2a7e5 | 20 | |
| aberk | 0:ef171fe2a7e5 | 21 | dof9RazorImuAhrs::dof9RazorImuAhrs(PinName tx, PinName rx) { |
| aberk | 0:ef171fe2a7e5 | 22 | |
| aberk | 0:ef171fe2a7e5 | 23 | razor = new Serial(tx, rx); |
| aberk | 0:ef171fe2a7e5 | 24 | razor->baud(BAUD_RATE); |
| aberk | 0:ef171fe2a7e5 | 25 | |
| aberk | 0:ef171fe2a7e5 | 26 | } |
| aberk | 0:ef171fe2a7e5 | 27 | |
| aberk | 0:ef171fe2a7e5 | 28 | void dof9RazorImuAhrs::update(void) { |
| aberk | 0:ef171fe2a7e5 | 29 | |
| aberk | 0:ef171fe2a7e5 | 30 | //Make sure we get to the start of a line. |
| aberk | 0:ef171fe2a7e5 | 31 | while (razor->getc() != '\n'); |
| aberk | 0:ef171fe2a7e5 | 32 | |
| aberk | 0:ef171fe2a7e5 | 33 | #if PRINT_EULER == 1 |
| aberk | 0:ef171fe2a7e5 | 34 | razor->scanf("!ANG:%f,%f,%f", &roll, &pitch, &yaw); |
| aberk | 0:ef171fe2a7e5 | 35 | #endif |
| aberk | 0:ef171fe2a7e5 | 36 | |
| aberk | 0:ef171fe2a7e5 | 37 | #if PRINT_ANALOGS == 1 |
| aberk | 0:ef171fe2a7e5 | 38 | razor->scanf(",AN:%f,%f,%f", &gyro_x, &gyro_y, &gyro_z); |
| aberk | 0:ef171fe2a7e5 | 39 | razor->scanf(",%f,%f,%f", &acc_x, &acc_y, &acc_z); |
| aberk | 0:ef171fe2a7e5 | 40 | razor->scanf(",%f,%f,%f", &mag_x, &mag_y, &mag_z); |
| aberk | 0:ef171fe2a7e5 | 41 | #endif |
| aberk | 0:ef171fe2a7e5 | 42 | |
| aberk | 0:ef171fe2a7e5 | 43 | } |
| aberk | 0:ef171fe2a7e5 | 44 | |
| aberk | 0:ef171fe2a7e5 | 45 | float dof9RazorImuAhrs::getRoll(void){ |
| aberk | 0:ef171fe2a7e5 | 46 | |
| aberk | 0:ef171fe2a7e5 | 47 | return roll; |
| aberk | 0:ef171fe2a7e5 | 48 | |
| aberk | 0:ef171fe2a7e5 | 49 | } |
| aberk | 0:ef171fe2a7e5 | 50 | |
| aberk | 0:ef171fe2a7e5 | 51 | float dof9RazorImuAhrs::getPitch(void){ |
| aberk | 0:ef171fe2a7e5 | 52 | |
| aberk | 0:ef171fe2a7e5 | 53 | return pitch; |
| aberk | 0:ef171fe2a7e5 | 54 | |
| aberk | 0:ef171fe2a7e5 | 55 | } |
| aberk | 0:ef171fe2a7e5 | 56 | |
| aberk | 0:ef171fe2a7e5 | 57 | float dof9RazorImuAhrs::getYaw(void){ |
| aberk | 0:ef171fe2a7e5 | 58 | |
| aberk | 0:ef171fe2a7e5 | 59 | return yaw; |
| aberk | 0:ef171fe2a7e5 | 60 | |
| aberk | 0:ef171fe2a7e5 | 61 | } |
| aberk | 0:ef171fe2a7e5 | 62 | |
| aberk | 0:ef171fe2a7e5 | 63 | float dof9RazorImuAhrs::getGyroX(void){ |
| aberk | 0:ef171fe2a7e5 | 64 | |
| aberk | 0:ef171fe2a7e5 | 65 | return gyro_x; |
| aberk | 0:ef171fe2a7e5 | 66 | |
| aberk | 0:ef171fe2a7e5 | 67 | } |
| aberk | 0:ef171fe2a7e5 | 68 | |
| aberk | 0:ef171fe2a7e5 | 69 | float dof9RazorImuAhrs::getGyroY(void){ |
| aberk | 0:ef171fe2a7e5 | 70 | |
| aberk | 0:ef171fe2a7e5 | 71 | return gyro_y; |
| aberk | 0:ef171fe2a7e5 | 72 | |
| aberk | 0:ef171fe2a7e5 | 73 | } |
| aberk | 0:ef171fe2a7e5 | 74 | |
| aberk | 0:ef171fe2a7e5 | 75 | float dof9RazorImuAhrs::getGyroZ(void){ |
| aberk | 0:ef171fe2a7e5 | 76 | |
| aberk | 0:ef171fe2a7e5 | 77 | return gyro_z; |
| aberk | 0:ef171fe2a7e5 | 78 | |
| aberk | 0:ef171fe2a7e5 | 79 | } |
| aberk | 0:ef171fe2a7e5 | 80 | |
| aberk | 0:ef171fe2a7e5 | 81 | float dof9RazorImuAhrs::getAccX(void){ |
| aberk | 0:ef171fe2a7e5 | 82 | |
| aberk | 0:ef171fe2a7e5 | 83 | return acc_x; |
| aberk | 0:ef171fe2a7e5 | 84 | |
| aberk | 0:ef171fe2a7e5 | 85 | } |
| aberk | 0:ef171fe2a7e5 | 86 | |
| aberk | 0:ef171fe2a7e5 | 87 | float dof9RazorImuAhrs::getAccY(void){ |
| aberk | 0:ef171fe2a7e5 | 88 | |
| aberk | 0:ef171fe2a7e5 | 89 | return acc_x; |
| aberk | 0:ef171fe2a7e5 | 90 | |
| aberk | 0:ef171fe2a7e5 | 91 | } |
| aberk | 0:ef171fe2a7e5 | 92 | |
| aberk | 0:ef171fe2a7e5 | 93 | float dof9RazorImuAhrs::getAccZ(void){ |
| aberk | 0:ef171fe2a7e5 | 94 | |
| aberk | 0:ef171fe2a7e5 | 95 | return acc_z; |
| aberk | 0:ef171fe2a7e5 | 96 | |
| aberk | 0:ef171fe2a7e5 | 97 | } |
| aberk | 0:ef171fe2a7e5 | 98 | |
| aberk | 0:ef171fe2a7e5 | 99 | float dof9RazorImuAhrs::getMagX(void){ |
| aberk | 0:ef171fe2a7e5 | 100 | |
| aberk | 0:ef171fe2a7e5 | 101 | return mag_x; |
| aberk | 0:ef171fe2a7e5 | 102 | |
| aberk | 0:ef171fe2a7e5 | 103 | } |
| aberk | 0:ef171fe2a7e5 | 104 | |
| aberk | 0:ef171fe2a7e5 | 105 | float dof9RazorImuAhrs::getMagY(void){ |
| aberk | 0:ef171fe2a7e5 | 106 | |
| aberk | 0:ef171fe2a7e5 | 107 | return mag_y; |
| aberk | 0:ef171fe2a7e5 | 108 | |
| aberk | 0:ef171fe2a7e5 | 109 | } |
| aberk | 0:ef171fe2a7e5 | 110 | |
| aberk | 0:ef171fe2a7e5 | 111 | float dof9RazorImuAhrs::getMagZ(void){ |
| aberk | 0:ef171fe2a7e5 | 112 | |
| aberk | 0:ef171fe2a7e5 | 113 | return mag_z; |
| aberk | 0:ef171fe2a7e5 | 114 | |
| aberk | 0:ef171fe2a7e5 | 115 | } |