Luke Petre
Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724
Razor_AHRS.h@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?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| lpetre | 0:9a72d42c0da3 | 1 | /*************************************************************************************************************** |
| lpetre | 0:9a72d42c0da3 | 2 | * Razor AHRS Firmware v1.4.0 |
| lpetre | 0:9a72d42c0da3 | 3 | * 9 Degree of Measurement Attitude and Heading Reference System |
| lpetre | 0:9a72d42c0da3 | 4 | * for Sparkfun "9DOF Razor IMU" (SEN-10125 and SEN-10736) |
| lpetre | 0:9a72d42c0da3 | 5 | * and "9DOF Sensor Stick" (SEN-10183, 10321 and SEN-10724) |
| lpetre | 0:9a72d42c0da3 | 6 | * |
| lpetre | 0:9a72d42c0da3 | 7 | * Released under GNU GPL (General Public License) v3.0 |
| lpetre | 0:9a72d42c0da3 | 8 | * Copyright (C) 2011 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin |
| lpetre | 0:9a72d42c0da3 | 9 | * |
| lpetre | 0:9a72d42c0da3 | 10 | * Infos, updates, bug reports and feedback: |
| lpetre | 0:9a72d42c0da3 | 11 | * http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs |
| lpetre | 0:9a72d42c0da3 | 12 | * |
| lpetre | 0:9a72d42c0da3 | 13 | * |
| lpetre | 0:9a72d42c0da3 | 14 | * History: |
| lpetre | 0:9a72d42c0da3 | 15 | * * Original code (http://code.google.com/p/sf9domahrs/) by Doug Weibel and Jose Julio, |
| lpetre | 0:9a72d42c0da3 | 16 | * based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel. Thank you! |
| lpetre | 0:9a72d42c0da3 | 17 | * |
| lpetre | 0:9a72d42c0da3 | 18 | * * Updated code (http://groups.google.com/group/sf_9dof_ahrs_update) by David Malik (david.zsolt.malik@gmail.com) |
| lpetre | 0:9a72d42c0da3 | 19 | * for new Sparkfun 9DOF Razor hardware (SEN-10125). |
| lpetre | 0:9a72d42c0da3 | 20 | * |
| lpetre | 0:9a72d42c0da3 | 21 | * * Updated and extended by Peter Bartz (peter-bartz@gmx.de): |
| lpetre | 0:9a72d42c0da3 | 22 | * * v1.3.0 |
| lpetre | 0:9a72d42c0da3 | 23 | * * Cleaned up, streamlined and restructured most of the code to make it more comprehensible. |
| lpetre | 0:9a72d42c0da3 | 24 | * * Added sensor calibration (improves precision and responsiveness a lot!). |
| lpetre | 0:9a72d42c0da3 | 25 | * * Added binary yaw/pitch/roll output. |
| lpetre | 0:9a72d42c0da3 | 26 | * * Added basic serial command interface to set output modes/calibrate sensors/synch stream/etc. |
| lpetre | 0:9a72d42c0da3 | 27 | * * Added support to synch automatically when using Rovering Networks Bluetooth modules (and compatible). |
| lpetre | 0:9a72d42c0da3 | 28 | * * Wrote new easier to use test program (using Processing). |
| lpetre | 0:9a72d42c0da3 | 29 | * * Added support for new version of "9DOF Razor IMU": SEN-10736. |
| lpetre | 0:9a72d42c0da3 | 30 | * --> The output of this code is not compatible with the older versions! |
| lpetre | 0:9a72d42c0da3 | 31 | * --> A Processing sketch to test the tracker is available. |
| lpetre | 0:9a72d42c0da3 | 32 | * * v1.3.1 |
| lpetre | 0:9a72d42c0da3 | 33 | * * Initializing rotation matrix based on start-up sensor readings -> orientation OK right away. |
| lpetre | 0:9a72d42c0da3 | 34 | * * Adjusted gyro low-pass filter and output rate settings. |
| lpetre | 0:9a72d42c0da3 | 35 | * * v1.3.2 |
| lpetre | 0:9a72d42c0da3 | 36 | * * Adapted code to work with new Arduino 1.0 (and older versions still). |
| lpetre | 0:9a72d42c0da3 | 37 | * * v1.3.3 |
| lpetre | 0:9a72d42c0da3 | 38 | * * Improved synching. |
| lpetre | 0:9a72d42c0da3 | 39 | * * v1.4.0 |
| lpetre | 0:9a72d42c0da3 | 40 | * * Added support for SparkFun "9DOF Sensor Stick" (versions SEN-10183, SEN-10321 and SEN-10724). |
| lpetre | 0:9a72d42c0da3 | 41 | * |
| lpetre | 0:9a72d42c0da3 | 42 | * TODOs: |
| lpetre | 0:9a72d42c0da3 | 43 | * * Allow optional use of EEPROM for storing and reading calibration values. |
| lpetre | 0:9a72d42c0da3 | 44 | * * Use self-test and temperature-compensation features of the sensors. |
| lpetre | 0:9a72d42c0da3 | 45 | * * Add binary output of unfused sensor data for all 9 axes. |
| lpetre | 0:9a72d42c0da3 | 46 | ***************************************************************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 47 | |
| lpetre | 0:9a72d42c0da3 | 48 | /* |
| lpetre | 0:9a72d42c0da3 | 49 | "9DOF Razor IMU" hardware versions: SEN-10125 and SEN-10736 |
| lpetre | 0:9a72d42c0da3 | 50 | |
| lpetre | 0:9a72d42c0da3 | 51 | ATMega328@3.3V, 8MHz |
| lpetre | 0:9a72d42c0da3 | 52 | |
| lpetre | 0:9a72d42c0da3 | 53 | ADXL345 : Accelerometer |
| lpetre | 0:9a72d42c0da3 | 54 | HMC5843 : Magnetometer on SEN-10125 |
| lpetre | 0:9a72d42c0da3 | 55 | HMC5883L : Magnetometer on SEN-10736 |
| lpetre | 0:9a72d42c0da3 | 56 | ITG-3200 : Gyro |
| lpetre | 0:9a72d42c0da3 | 57 | |
| lpetre | 0:9a72d42c0da3 | 58 | Arduino IDE : Select board "Arduino Pro or Pro Mini (3.3v, 8Mhz) w/ATmega328" |
| lpetre | 0:9a72d42c0da3 | 59 | */ |
| lpetre | 0:9a72d42c0da3 | 60 | |
| lpetre | 0:9a72d42c0da3 | 61 | /* |
| lpetre | 0:9a72d42c0da3 | 62 | "9DOF Sensor Stick" hardware versions: SEN-10183, SEN-10321 and SEN-10724 |
| lpetre | 0:9a72d42c0da3 | 63 | |
| lpetre | 0:9a72d42c0da3 | 64 | ADXL345 : Accelerometer |
| lpetre | 0:9a72d42c0da3 | 65 | HMC5843 : Magnetometer on SEN-10183 and SEN-10321 |
| lpetre | 0:9a72d42c0da3 | 66 | HMC5883L : Magnetometer on SEN-10724 |
| lpetre | 0:9a72d42c0da3 | 67 | ITG-3200 : Gyro |
| lpetre | 0:9a72d42c0da3 | 68 | */ |
| lpetre | 0:9a72d42c0da3 | 69 | |
| lpetre | 0:9a72d42c0da3 | 70 | /* |
| lpetre | 0:9a72d42c0da3 | 71 | Axis definition (differs from definition printed on the board!): |
| lpetre | 0:9a72d42c0da3 | 72 | X axis pointing forward (towards the short edge with the connector holes) |
| lpetre | 0:9a72d42c0da3 | 73 | Y axis pointing to the right |
| lpetre | 0:9a72d42c0da3 | 74 | and Z axis pointing down. |
| lpetre | 0:9a72d42c0da3 | 75 | |
| lpetre | 0:9a72d42c0da3 | 76 | Positive yaw : clockwise |
| lpetre | 0:9a72d42c0da3 | 77 | Positive roll : right wing down |
| lpetre | 0:9a72d42c0da3 | 78 | Positive pitch : nose up |
| lpetre | 0:9a72d42c0da3 | 79 | |
| lpetre | 0:9a72d42c0da3 | 80 | Transformation order: first yaw then pitch then roll. |
| lpetre | 0:9a72d42c0da3 | 81 | */ |
| lpetre | 0:9a72d42c0da3 | 82 | |
| lpetre | 0:9a72d42c0da3 | 83 | /* |
| lpetre | 0:9a72d42c0da3 | 84 | Commands that the firmware understands: |
| lpetre | 0:9a72d42c0da3 | 85 | |
| lpetre | 0:9a72d42c0da3 | 86 | "#o<param>" - Set output parameter. The available options are: |
| lpetre | 0:9a72d42c0da3 | 87 | "#o0" - Disable continuous streaming output. |
| lpetre | 0:9a72d42c0da3 | 88 | "#o1" - Enable continuous streaming output. |
| lpetre | 0:9a72d42c0da3 | 89 | "#ob" - Output angles in binary format (yaw/pitch/roll as binary float, so one output frame |
| lpetre | 0:9a72d42c0da3 | 90 | is 3x4 = 12 bytes long). |
| lpetre | 0:9a72d42c0da3 | 91 | "#ot" - Output angles in text format (Output frames have form like "#YPR=-142.28,-5.38,33.52", |
| lpetre | 0:9a72d42c0da3 | 92 | followed by carriage return and line feed [\r\n]). |
| lpetre | 0:9a72d42c0da3 | 93 | "#os" - Output (calibrated) sensor data of all 9 axes in text format. One frame consist of |
| lpetre | 0:9a72d42c0da3 | 94 | three lines - one for each sensor. |
| lpetre | 0:9a72d42c0da3 | 95 | "#oc" - Go to calibration output mode. |
| lpetre | 0:9a72d42c0da3 | 96 | "#on" - When in calibration mode, go on to calibrate next sensor. |
| lpetre | 0:9a72d42c0da3 | 97 | "#oe0" - Disable error message output. |
| lpetre | 0:9a72d42c0da3 | 98 | "#oe1" - Enable error message output. |
| lpetre | 0:9a72d42c0da3 | 99 | |
| lpetre | 0:9a72d42c0da3 | 100 | "#f" - Request one output frame - useful when continuous output is disabled and updates are |
| lpetre | 0:9a72d42c0da3 | 101 | required in larger intervals only. |
| lpetre | 0:9a72d42c0da3 | 102 | "#s<xy>" - Request synch token - useful to find out where the frame boundaries are in a continuous |
| lpetre | 0:9a72d42c0da3 | 103 | binary stream or to see if tracker is present and answering. The tracker will send |
| lpetre | 0:9a72d42c0da3 | 104 | "#SYNCH<xy>\r\n" in response (so it's possible to read using a readLine() function). |
| lpetre | 0:9a72d42c0da3 | 105 | x and y are two mandatory but arbitrary bytes that can be used to find out which request |
| lpetre | 0:9a72d42c0da3 | 106 | the answer belongs to. |
| lpetre | 0:9a72d42c0da3 | 107 | |
| lpetre | 0:9a72d42c0da3 | 108 | ("#C" and "#D" - Reserved for communication with optional Bluetooth module.) |
| lpetre | 0:9a72d42c0da3 | 109 | |
| lpetre | 0:9a72d42c0da3 | 110 | Newline characters are not required. So you could send "#ob#o1#s", which |
| lpetre | 0:9a72d42c0da3 | 111 | would set binary output mode, enable continuous streaming output and request |
| lpetre | 0:9a72d42c0da3 | 112 | a synch token all at once. |
| lpetre | 0:9a72d42c0da3 | 113 | |
| lpetre | 0:9a72d42c0da3 | 114 | The status LED will be on if streaming output is enabled and off otherwise. |
| lpetre | 0:9a72d42c0da3 | 115 | |
| lpetre | 0:9a72d42c0da3 | 116 | Byte order of binary output is little-endian: least significant byte comes first. |
| lpetre | 0:9a72d42c0da3 | 117 | */ |
| lpetre | 0:9a72d42c0da3 | 118 | #include <mbed.h> |
| lpetre | 0:9a72d42c0da3 | 119 | #include <MODSERIAL.h> |
| lpetre | 0:9a72d42c0da3 | 120 | |
| lpetre | 0:9a72d42c0da3 | 121 | |
| lpetre | 0:9a72d42c0da3 | 122 | /*****************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 123 | /*********** USER SETUP AREA! Set your options here! *************/ |
| lpetre | 0:9a72d42c0da3 | 124 | /*****************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 125 | |
| lpetre | 0:9a72d42c0da3 | 126 | // HARDWARE OPTIONS |
| lpetre | 0:9a72d42c0da3 | 127 | /*****************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 128 | // Select your hardware here by uncommenting one line! |
| lpetre | 0:9a72d42c0da3 | 129 | //#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer) |
| lpetre | 0:9a72d42c0da3 | 130 | #define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer) |
| lpetre | 0:9a72d42c0da3 | 131 | //#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer) |
| lpetre | 0:9a72d42c0da3 | 132 | //#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer) |
| lpetre | 0:9a72d42c0da3 | 133 | //#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer) |
| lpetre | 0:9a72d42c0da3 | 134 | |
| lpetre | 0:9a72d42c0da3 | 135 | |
| lpetre | 0:9a72d42c0da3 | 136 | // OUTPUT OPTIONS |
| lpetre | 0:9a72d42c0da3 | 137 | /*****************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 138 | // Set your serial port baud rate used to send out data here! |
| lpetre | 0:9a72d42c0da3 | 139 | #define OUTPUT__BAUD_RATE 57600 |
| lpetre | 0:9a72d42c0da3 | 140 | |
| lpetre | 0:9a72d42c0da3 | 141 | // Sensor data output interval in milliseconds |
| lpetre | 0:9a72d42c0da3 | 142 | // This may not work, if faster than 20ms (=50Hz) |
| lpetre | 0:9a72d42c0da3 | 143 | // Code is tuned for 20ms, so better leave it like that |
| lpetre | 0:9a72d42c0da3 | 144 | #define OUTPUT__DATA_INTERVAL 20 // in milliseconds |
| lpetre | 0:9a72d42c0da3 | 145 | |
| lpetre | 0:9a72d42c0da3 | 146 | // Output mode |
| lpetre | 0:9a72d42c0da3 | 147 | #define OUTPUT__MODE_CALIBRATE_SENSORS 0 // Outputs sensor min/max values as text for manual calibration |
| lpetre | 0:9a72d42c0da3 | 148 | #define OUTPUT__MODE_ANGLES_TEXT 1 // Outputs yaw/pitch/roll in degrees as text |
| lpetre | 0:9a72d42c0da3 | 149 | #define OUTPUT__MODE_ANGLES_BINARY 2 // Outputs yaw/pitch/roll in degrees as binary float |
| lpetre | 0:9a72d42c0da3 | 150 | #define OUTPUT__MODE_SENSORS_TEXT 3 // Outputs (calibrated) sensor values for all 9 axes as text |
| lpetre | 0:9a72d42c0da3 | 151 | |
| lpetre | 0:9a72d42c0da3 | 152 | // Select if serial continuous streaming output is enabled per default on startup. |
| lpetre | 0:9a72d42c0da3 | 153 | #define OUTPUT__STARTUP_STREAM_ON false // true or false |
| lpetre | 0:9a72d42c0da3 | 154 | |
| lpetre | 0:9a72d42c0da3 | 155 | // Bluetooth |
| lpetre | 0:9a72d42c0da3 | 156 | // You can set this to true, if you have a Rovering Networks Bluetooth Module attached. |
| lpetre | 0:9a72d42c0da3 | 157 | // The connect/disconnect message prefix of the module has to be set to "#". |
| lpetre | 0:9a72d42c0da3 | 158 | // (Refer to manual, it can be set like this: SO,#) |
| lpetre | 0:9a72d42c0da3 | 159 | // When using this, streaming output will only be enabled as long as we're connected. That way |
| lpetre | 0:9a72d42c0da3 | 160 | // receiver and sender are synchronzed easily just by connecting/disconnecting. |
| lpetre | 0:9a72d42c0da3 | 161 | // It is not necessary to set this! It just makes life easier when writing code for |
| lpetre | 0:9a72d42c0da3 | 162 | // the receiving side. The Processing test sketch also works without setting this. |
| lpetre | 0:9a72d42c0da3 | 163 | // NOTE: When using this, OUTPUT__STARTUP_STREAM_ON has no effect! |
| lpetre | 0:9a72d42c0da3 | 164 | #define OUTPUT__HAS_RN_BLUETOOTH false // true or false |
| lpetre | 0:9a72d42c0da3 | 165 | |
| lpetre | 0:9a72d42c0da3 | 166 | |
| lpetre | 0:9a72d42c0da3 | 167 | // SENSOR CALIBRATION |
| lpetre | 0:9a72d42c0da3 | 168 | /*****************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 169 | // How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs |
| lpetre | 0:9a72d42c0da3 | 170 | // Put MIN/MAX and OFFSET readings for your board here! |
| lpetre | 0:9a72d42c0da3 | 171 | // Accelerometer |
| lpetre | 0:9a72d42c0da3 | 172 | // "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX" |
| lpetre | 0:9a72d42c0da3 | 173 | #define ACCEL_X_MIN ((float) -250) |
| lpetre | 0:9a72d42c0da3 | 174 | #define ACCEL_X_MAX ((float) 250) |
| lpetre | 0:9a72d42c0da3 | 175 | #define ACCEL_Y_MIN ((float) -250) |
| lpetre | 0:9a72d42c0da3 | 176 | #define ACCEL_Y_MAX ((float) 250) |
| lpetre | 0:9a72d42c0da3 | 177 | #define ACCEL_Z_MIN ((float) -250) |
| lpetre | 0:9a72d42c0da3 | 178 | #define ACCEL_Z_MAX ((float) 250) |
| lpetre | 0:9a72d42c0da3 | 179 | |
| lpetre | 0:9a72d42c0da3 | 180 | // Magnetometer |
| lpetre | 0:9a72d42c0da3 | 181 | // "magn x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX" |
| lpetre | 0:9a72d42c0da3 | 182 | #define MAGN_X_MIN ((float) -600) |
| lpetre | 0:9a72d42c0da3 | 183 | #define MAGN_X_MAX ((float) 600) |
| lpetre | 0:9a72d42c0da3 | 184 | #define MAGN_Y_MIN ((float) -600) |
| lpetre | 0:9a72d42c0da3 | 185 | #define MAGN_Y_MAX ((float) 600) |
| lpetre | 0:9a72d42c0da3 | 186 | #define MAGN_Z_MIN ((float) -600) |
| lpetre | 0:9a72d42c0da3 | 187 | #define MAGN_Z_MAX ((float) 600) |
| lpetre | 0:9a72d42c0da3 | 188 | |
| lpetre | 0:9a72d42c0da3 | 189 | // Gyroscope |
| lpetre | 0:9a72d42c0da3 | 190 | // "gyro x,y,z (current/average) = .../OFFSET_X .../OFFSET_Y .../OFFSET_Z |
| lpetre | 0:9a72d42c0da3 | 191 | #define GYRO_AVERAGE_OFFSET_X ((float) 0.0) |
| lpetre | 0:9a72d42c0da3 | 192 | #define GYRO_AVERAGE_OFFSET_Y ((float) 0.0) |
| lpetre | 0:9a72d42c0da3 | 193 | #define GYRO_AVERAGE_OFFSET_Z ((float) 0.0) |
| lpetre | 0:9a72d42c0da3 | 194 | |
| lpetre | 0:9a72d42c0da3 | 195 | /* |
| lpetre | 0:9a72d42c0da3 | 196 | // Calibration example: |
| lpetre | 0:9a72d42c0da3 | 197 | // "accel x,y,z (min/max) = -278.00/270.00 -254.00/284.00 -294.00/235.00" |
| lpetre | 0:9a72d42c0da3 | 198 | #define ACCEL_X_MIN ((float) -278) |
| lpetre | 0:9a72d42c0da3 | 199 | #define ACCEL_X_MAX ((float) 270) |
| lpetre | 0:9a72d42c0da3 | 200 | #define ACCEL_Y_MIN ((float) -254) |
| lpetre | 0:9a72d42c0da3 | 201 | #define ACCEL_Y_MAX ((float) 284) |
| lpetre | 0:9a72d42c0da3 | 202 | #define ACCEL_Z_MIN ((float) -294) |
| lpetre | 0:9a72d42c0da3 | 203 | #define ACCEL_Z_MAX ((float) 235) |
| lpetre | 0:9a72d42c0da3 | 204 | |
| lpetre | 0:9a72d42c0da3 | 205 | // "magn x,y,z (min/max) = -511.00/581.00 -516.00/568.00 -489.00/486.00" |
| lpetre | 0:9a72d42c0da3 | 206 | #define MAGN_X_MIN ((float) -511) |
| lpetre | 0:9a72d42c0da3 | 207 | #define MAGN_X_MAX ((float) 581) |
| lpetre | 0:9a72d42c0da3 | 208 | #define MAGN_Y_MIN ((float) -516) |
| lpetre | 0:9a72d42c0da3 | 209 | #define MAGN_Y_MAX ((float) 568) |
| lpetre | 0:9a72d42c0da3 | 210 | #define MAGN_Z_MIN ((float) -489) |
| lpetre | 0:9a72d42c0da3 | 211 | #define MAGN_Z_MAX ((float) 486) |
| lpetre | 0:9a72d42c0da3 | 212 | |
| lpetre | 0:9a72d42c0da3 | 213 | //"gyro x,y,z (current/average) = -32.00/-34.82 102.00/100.41 -16.00/-16.38" |
| lpetre | 0:9a72d42c0da3 | 214 | #define GYRO_AVERAGE_OFFSET_X ((float) -34.82) |
| lpetre | 0:9a72d42c0da3 | 215 | #define GYRO_AVERAGE_OFFSET_Y ((float) 100.41) |
| lpetre | 0:9a72d42c0da3 | 216 | #define GYRO_AVERAGE_OFFSET_Z ((float) -16.38) |
| lpetre | 0:9a72d42c0da3 | 217 | */ |
| lpetre | 0:9a72d42c0da3 | 218 | |
| lpetre | 0:9a72d42c0da3 | 219 | |
| lpetre | 0:9a72d42c0da3 | 220 | // DEBUG OPTIONS |
| lpetre | 0:9a72d42c0da3 | 221 | /*****************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 222 | // When set to true, gyro drift correction will not be applied |
| lpetre | 0:9a72d42c0da3 | 223 | #define DEBUG__NO_DRIFT_CORRECTION false |
| lpetre | 0:9a72d42c0da3 | 224 | // Print elapsed time after each I/O loop |
| lpetre | 0:9a72d42c0da3 | 225 | #define DEBUG__PRINT_LOOP_TIME false |
| lpetre | 0:9a72d42c0da3 | 226 | |
| lpetre | 0:9a72d42c0da3 | 227 | |
| lpetre | 0:9a72d42c0da3 | 228 | /*****************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 229 | /****************** END OF USER SETUP AREA! *********************/ |
| lpetre | 0:9a72d42c0da3 | 230 | /*****************************************************************/ |
| lpetre | 0:9a72d42c0da3 | 231 | |
| lpetre | 0:9a72d42c0da3 | 232 | |
| lpetre | 0:9a72d42c0da3 | 233 | // Check if hardware version code is defined |
| lpetre | 0:9a72d42c0da3 | 234 | #ifndef HW__VERSION_CODE |
| lpetre | 0:9a72d42c0da3 | 235 | // Generate compile error |
| lpetre | 0:9a72d42c0da3 | 236 | #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.pde! |
| lpetre | 0:9a72d42c0da3 | 237 | #endif |
| lpetre | 0:9a72d42c0da3 | 238 | |
| lpetre | 0:9a72d42c0da3 | 239 | //#include <Wire.h> |
| lpetre | 0:9a72d42c0da3 | 240 | |
| lpetre | 0:9a72d42c0da3 | 241 | // Sensor calibration scale and offset values |
| lpetre | 0:9a72d42c0da3 | 242 | #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) |
| lpetre | 0:9a72d42c0da3 | 243 | #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f) |
| lpetre | 0:9a72d42c0da3 | 244 | #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f) |
| lpetre | 0:9a72d42c0da3 | 245 | #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET)) |
| lpetre | 0:9a72d42c0da3 | 246 | #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET)) |
| lpetre | 0:9a72d42c0da3 | 247 | #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET)) |
| lpetre | 0:9a72d42c0da3 | 248 | |
| lpetre | 0:9a72d42c0da3 | 249 | #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f) |
| lpetre | 0:9a72d42c0da3 | 250 | #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f) |
| lpetre | 0:9a72d42c0da3 | 251 | #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f) |
| lpetre | 0:9a72d42c0da3 | 252 | #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET)) |
| lpetre | 0:9a72d42c0da3 | 253 | #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET)) |
| lpetre | 0:9a72d42c0da3 | 254 | #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)) |
| lpetre | 0:9a72d42c0da3 | 255 | |
| lpetre | 0:9a72d42c0da3 | 256 | |
| lpetre | 0:9a72d42c0da3 | 257 | // Gain for gyroscope (ITG-3200) |
| lpetre | 0:9a72d42c0da3 | 258 | #define GYRO_GAIN 0.06957 // Same gain on all axes |
| lpetre | 0:9a72d42c0da3 | 259 | #define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second |
| lpetre | 0:9a72d42c0da3 | 260 | |
| lpetre | 0:9a72d42c0da3 | 261 | // DCM parameters |
| lpetre | 0:9a72d42c0da3 | 262 | #define Kp_ROLLPITCH 0.02f |
| lpetre | 0:9a72d42c0da3 | 263 | #define Ki_ROLLPITCH 0.00002f |
| lpetre | 0:9a72d42c0da3 | 264 | #define Kp_YAW 1.2f |
| lpetre | 0:9a72d42c0da3 | 265 | #define Ki_YAW 0.00002f |
| lpetre | 0:9a72d42c0da3 | 266 | |
| lpetre | 0:9a72d42c0da3 | 267 | // Stuff |
| lpetre | 0:9a72d42c0da3 | 268 | #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration |
| lpetre | 0:9a72d42c0da3 | 269 | #define TO_RAD(x) (x * 0.01745329252) // *pi/180 |
| lpetre | 0:9a72d42c0da3 | 270 | #define TO_DEG(x) (x * 57.2957795131) // *180/pi |
| lpetre | 0:9a72d42c0da3 | 271 | #define NEW_LINE "\r\n" |
| lpetre | 0:9a72d42c0da3 | 272 | |
| lpetre | 0:9a72d42c0da3 | 273 | class IMU { |
| lpetre | 0:9a72d42c0da3 | 274 | public: |
| lpetre | 0:9a72d42c0da3 | 275 | // Sensor variables |
| lpetre | 0:9a72d42c0da3 | 276 | int16_t accel[3]; // Actually stores the NEGATED acceleration (equals gravity, if board not moving). |
| lpetre | 0:9a72d42c0da3 | 277 | int16_t accel_min[3]; |
| lpetre | 0:9a72d42c0da3 | 278 | int16_t accel_max[3]; |
| lpetre | 0:9a72d42c0da3 | 279 | |
| lpetre | 0:9a72d42c0da3 | 280 | int16_t magnetom[3]; |
| lpetre | 0:9a72d42c0da3 | 281 | int16_t magnetom_min[3]; |
| lpetre | 0:9a72d42c0da3 | 282 | int16_t magnetom_max[3]; |
| lpetre | 0:9a72d42c0da3 | 283 | |
| lpetre | 0:9a72d42c0da3 | 284 | int16_t gyro[3]; |
| lpetre | 0:9a72d42c0da3 | 285 | int16_t gyro_average[3]; |
| lpetre | 0:9a72d42c0da3 | 286 | int gyro_num_samples; |
| lpetre | 0:9a72d42c0da3 | 287 | |
| lpetre | 0:9a72d42c0da3 | 288 | // Euler angles |
| lpetre | 0:9a72d42c0da3 | 289 | float yaw; |
| lpetre | 0:9a72d42c0da3 | 290 | float pitch; |
| lpetre | 0:9a72d42c0da3 | 291 | float roll; |
| lpetre | 0:9a72d42c0da3 | 292 | |
| lpetre | 0:9a72d42c0da3 | 293 | // DCM variables |
| lpetre | 0:9a72d42c0da3 | 294 | float MAG_Heading; |
| lpetre | 0:9a72d42c0da3 | 295 | float Accel_Vector[3]; // Store the acceleration in a vector |
| lpetre | 0:9a72d42c0da3 | 296 | float Gyro_Vector[3]; // Store the gyros turn rate in a vector |
| lpetre | 0:9a72d42c0da3 | 297 | float Omega_Vector[3]; // Corrected Gyro_Vector data |
| lpetre | 0:9a72d42c0da3 | 298 | float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction |
| lpetre | 0:9a72d42c0da3 | 299 | float Omega_I[3];//= {0, 0, 0}; // Omega Integrator |
| lpetre | 0:9a72d42c0da3 | 300 | float Omega[3];//= {0, 0, 0}; |
| lpetre | 0:9a72d42c0da3 | 301 | float errorRollPitch[3];// = {0, 0, 0}; |
| lpetre | 0:9a72d42c0da3 | 302 | float errorYaw[3];// = {0, 0, 0}; |
| lpetre | 0:9a72d42c0da3 | 303 | float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; |
| lpetre | 0:9a72d42c0da3 | 304 | float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; |
| lpetre | 0:9a72d42c0da3 | 305 | float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; |
| lpetre | 0:9a72d42c0da3 | 306 | |
| lpetre | 0:9a72d42c0da3 | 307 | // DCM timing in the main loop |
| lpetre | 0:9a72d42c0da3 | 308 | long timestamp; |
| lpetre | 0:9a72d42c0da3 | 309 | long timestamp_old; |
| lpetre | 0:9a72d42c0da3 | 310 | float G_Dt; // Integration time for DCM algorithm |
| lpetre | 0:9a72d42c0da3 | 311 | |
| lpetre | 0:9a72d42c0da3 | 312 | // More output-state variables |
| lpetre | 0:9a72d42c0da3 | 313 | int output_mode; |
| lpetre | 0:9a72d42c0da3 | 314 | bool output_stream_on; |
| lpetre | 0:9a72d42c0da3 | 315 | bool output_single_on; |
| lpetre | 0:9a72d42c0da3 | 316 | int curr_calibration_sensor; |
| lpetre | 0:9a72d42c0da3 | 317 | bool reset_calibration_session_flag; |
| lpetre | 0:9a72d42c0da3 | 318 | int num_accel_errors; |
| lpetre | 0:9a72d42c0da3 | 319 | int num_magn_errors; |
| lpetre | 0:9a72d42c0da3 | 320 | int num_gyro_errors; |
| lpetre | 0:9a72d42c0da3 | 321 | |
| lpetre | 0:9a72d42c0da3 | 322 | // If set true, an error message will be output if we fail to read sensor data. |
| lpetre | 0:9a72d42c0da3 | 323 | // Message format: "!ERR: reading <sensor>", followed by "\r\n". |
| lpetre | 0:9a72d42c0da3 | 324 | bool output_errors; |
| lpetre | 0:9a72d42c0da3 | 325 | |
| lpetre | 0:9a72d42c0da3 | 326 | DigitalOut statusLed; |
| lpetre | 0:9a72d42c0da3 | 327 | MODSERIAL pc; |
| lpetre | 0:9a72d42c0da3 | 328 | I2C Wire; |
| lpetre | 0:9a72d42c0da3 | 329 | Timer timer; |
| lpetre | 0:9a72d42c0da3 | 330 | |
| lpetre | 0:9a72d42c0da3 | 331 | public: |
| lpetre | 0:9a72d42c0da3 | 332 | IMU() |
| lpetre | 0:9a72d42c0da3 | 333 | : gyro_num_samples(0) |
| lpetre | 0:9a72d42c0da3 | 334 | , yaw(0) |
| lpetre | 0:9a72d42c0da3 | 335 | , pitch(0) |
| lpetre | 0:9a72d42c0da3 | 336 | , roll(0) |
| lpetre | 0:9a72d42c0da3 | 337 | , MAG_Heading(0) |
| lpetre | 0:9a72d42c0da3 | 338 | , timestamp(0) |
| lpetre | 0:9a72d42c0da3 | 339 | , timestamp_old(0) |
| lpetre | 0:9a72d42c0da3 | 340 | , G_Dt(0) |
| lpetre | 0:9a72d42c0da3 | 341 | , output_mode(-1) // Select your startup output mode here!// Select your startup output mode here! |
| lpetre | 0:9a72d42c0da3 | 342 | , output_stream_on(false) |
| lpetre | 0:9a72d42c0da3 | 343 | , output_single_on(true) |
| lpetre | 0:9a72d42c0da3 | 344 | , curr_calibration_sensor(0) |
| lpetre | 0:9a72d42c0da3 | 345 | , reset_calibration_session_flag(true) |
| lpetre | 0:9a72d42c0da3 | 346 | , num_accel_errors(0) |
| lpetre | 0:9a72d42c0da3 | 347 | , num_magn_errors(0) |
| lpetre | 0:9a72d42c0da3 | 348 | , num_gyro_errors(0) |
| lpetre | 0:9a72d42c0da3 | 349 | , output_errors(true) |
| lpetre | 0:9a72d42c0da3 | 350 | , statusLed(LED1) |
| lpetre | 0:9a72d42c0da3 | 351 | , pc(USBTX, USBRX) |
| lpetre | 0:9a72d42c0da3 | 352 | , Wire(p28, p27) |
| lpetre | 0:9a72d42c0da3 | 353 | { |
| lpetre | 0:9a72d42c0da3 | 354 | accel[0] = accel_min[0] = accel_max[0] = magnetom[0] = magnetom_min[0] = magnetom_max[0] = gyro[0] = gyro_average[0] = 0; |
| lpetre | 0:9a72d42c0da3 | 355 | accel[1] = accel_min[1] = accel_max[1] = magnetom[1] = magnetom_min[1] = magnetom_max[1] = gyro[1] = gyro_average[1] = 0; |
| lpetre | 0:9a72d42c0da3 | 356 | accel[2] = accel_min[2] = accel_max[2] = magnetom[2] = magnetom_min[2] = magnetom_max[2] = gyro[2] = gyro_average[2] = 0; |
| lpetre | 0:9a72d42c0da3 | 357 | |
| lpetre | 0:9a72d42c0da3 | 358 | Accel_Vector[0] = Gyro_Vector[0] = Omega_Vector[0] = Omega_P[0] = Omega_I[0] = Omega[0] = errorRollPitch[0] = errorYaw[0] = 0; |
| lpetre | 0:9a72d42c0da3 | 359 | Accel_Vector[1] = Gyro_Vector[1] = Omega_Vector[1] = Omega_P[1] = Omega_I[1] = Omega[1] = errorRollPitch[1] = errorYaw[1] = 0; |
| lpetre | 0:9a72d42c0da3 | 360 | Accel_Vector[2] = Gyro_Vector[2] = Omega_Vector[2] = Omega_P[2] = Omega_I[2] = Omega[2] = errorRollPitch[2] = errorYaw[2] = 0; |
| lpetre | 0:9a72d42c0da3 | 361 | |
| lpetre | 0:9a72d42c0da3 | 362 | DCM_Matrix[0][0] = 1; DCM_Matrix[0][1] = 0; DCM_Matrix[0][2] = 0; |
| lpetre | 0:9a72d42c0da3 | 363 | DCM_Matrix[1][0] = 0; DCM_Matrix[1][1] = 1; DCM_Matrix[1][2] = 0; |
| lpetre | 0:9a72d42c0da3 | 364 | DCM_Matrix[2][0] = 0; DCM_Matrix[2][1] = 0; DCM_Matrix[2][2] = 1; |
| lpetre | 0:9a72d42c0da3 | 365 | |
| lpetre | 0:9a72d42c0da3 | 366 | Update_Matrix[0][0] = 0; Update_Matrix[0][1] = 1; Update_Matrix[0][2] = 2; |
| lpetre | 0:9a72d42c0da3 | 367 | Update_Matrix[1][0] = 3; Update_Matrix[1][1] = 4; Update_Matrix[1][2] = 5; |
| lpetre | 0:9a72d42c0da3 | 368 | Update_Matrix[2][0] = 6; Update_Matrix[2][1] = 7; Update_Matrix[2][2] = 8; |
| lpetre | 0:9a72d42c0da3 | 369 | |
| lpetre | 0:9a72d42c0da3 | 370 | Temporary_Matrix[0][0] = 0; Temporary_Matrix[0][1] = 0; Temporary_Matrix[0][2] = 0; |
| lpetre | 0:9a72d42c0da3 | 371 | Temporary_Matrix[1][0] = 0; Temporary_Matrix[1][1] = 0; Temporary_Matrix[1][2] = 0; |
| lpetre | 0:9a72d42c0da3 | 372 | Temporary_Matrix[2][0] = 0; Temporary_Matrix[2][1] = 0; Temporary_Matrix[2][2] = 0; |
| lpetre | 0:9a72d42c0da3 | 373 | } |
| lpetre | 0:9a72d42c0da3 | 374 | |
| lpetre | 0:9a72d42c0da3 | 375 | // Compass.cpp |
| lpetre | 0:9a72d42c0da3 | 376 | void Compass_Heading(); |
| lpetre | 0:9a72d42c0da3 | 377 | |
| lpetre | 0:9a72d42c0da3 | 378 | // DCM.cpp |
| lpetre | 0:9a72d42c0da3 | 379 | void Normalize(); |
| lpetre | 0:9a72d42c0da3 | 380 | void Drift_correction(); |
| lpetre | 0:9a72d42c0da3 | 381 | void Matrix_update(); |
| lpetre | 0:9a72d42c0da3 | 382 | void Euler_angles(); |
| lpetre | 0:9a72d42c0da3 | 383 | |
| lpetre | 0:9a72d42c0da3 | 384 | // Output.cpp |
| lpetre | 0:9a72d42c0da3 | 385 | void output_angles(); |
| lpetre | 0:9a72d42c0da3 | 386 | void output_calibration(int calibration_sensor); |
| lpetre | 0:9a72d42c0da3 | 387 | void output_sensors(); |
| lpetre | 0:9a72d42c0da3 | 388 | |
| lpetre | 0:9a72d42c0da3 | 389 | |
| lpetre | 0:9a72d42c0da3 | 390 | // Razor_AHRS.cpp |
| lpetre | 0:9a72d42c0da3 | 391 | void read_sensors(); |
| lpetre | 0:9a72d42c0da3 | 392 | void reset_sensor_fusion(); |
| lpetre | 0:9a72d42c0da3 | 393 | void compensate_sensor_errors(); |
| lpetre | 0:9a72d42c0da3 | 394 | void check_reset_calibration_session(); |
| lpetre | 0:9a72d42c0da3 | 395 | void turn_output_stream_on(); |
| lpetre | 0:9a72d42c0da3 | 396 | void turn_output_stream_off(); |
| lpetre | 0:9a72d42c0da3 | 397 | char readChar(); |
| lpetre | 0:9a72d42c0da3 | 398 | void setup(); |
| lpetre | 0:9a72d42c0da3 | 399 | void loop(); |
| lpetre | 0:9a72d42c0da3 | 400 | |
| lpetre | 0:9a72d42c0da3 | 401 | |
| lpetre | 0:9a72d42c0da3 | 402 | // Sensors.cpp |
| lpetre | 0:9a72d42c0da3 | 403 | void I2C_Init(); |
| lpetre | 0:9a72d42c0da3 | 404 | void Accel_Init(); |
| lpetre | 0:9a72d42c0da3 | 405 | void Read_Accel(); |
| lpetre | 0:9a72d42c0da3 | 406 | void Magn_Init(); |
| lpetre | 0:9a72d42c0da3 | 407 | void Read_Magn(); |
| lpetre | 0:9a72d42c0da3 | 408 | void Gyro_Init(); |
| lpetre | 0:9a72d42c0da3 | 409 | void Read_Gyro(); |
| lpetre | 0:9a72d42c0da3 | 410 | |
| lpetre | 0:9a72d42c0da3 | 411 | }; |
| lpetre | 0:9a72d42c0da3 | 412 | |
| lpetre | 0:9a72d42c0da3 | 413 | float Vector_Dot_Product(float vector1[3], float vector2[3]); |
| lpetre | 0:9a72d42c0da3 | 414 | void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]); |
| lpetre | 0:9a72d42c0da3 | 415 | void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2); |
| lpetre | 0:9a72d42c0da3 | 416 | void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]); |
| lpetre | 0:9a72d42c0da3 | 417 | void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]); |
| lpetre | 0:9a72d42c0da3 | 418 | void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll); |
| lpetre | 0:9a72d42c0da3 | 419 | float constrain(float in, float min, float max); |
| lpetre | 0:9a72d42c0da3 | 420 |