Tyler Weaver / AHRS

AHRS library, modified version of Peter Bartz work.

Dependencies:   MODSERIAL

Dependents:   AHRS_demo

AHRS.h@1:da3b20b5d38a, 2012-11-08 (annotated)

Committer:
tylerjw
Date:
Thu Nov 08 20:13:35 2012 +0000
Revision:
1:da3b20b5d38a
Parent:
0:014ee3239c80 
Modified AHRS library.

Who changed what in which revision?

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