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

Razor_AHRS.h@1:e27c4c0b71d8, 2011-12-28 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew 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 1:e27c4c0b71d8 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