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