AHRS.h

00001 /***************************************************************************************************************
00002 * Razor AHRS Firmware v1.4.0
00003 * 9 Degree of Measurement Attitude and Heading Reference System
00004 * for Sparkfun "9DOF Razor IMU" (SEN-10125 and SEN-10736)
00005 * and "9DOF Sensor Stick" (SEN-10183, 10321 and SEN-10724)
00006 *
00007 * Released under GNU GPL (General Public License) v3.0
00008 * Copyright (C) 2011 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
00009 *
00010 * Infos, updates, bug reports and feedback:
00011 *     http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
00012 *
00013 *
00014 * History:
00015 *   * Original code (http://code.google.com/p/sf9domahrs/) by Doug Weibel and Jose Julio,
00016 *     based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel. Thank you!
00017 *
00018 *   * Updated code (http://groups.google.com/group/sf_9dof_ahrs_update) by David Malik (david.zsolt.malik@gmail.com)
00019 *     for new Sparkfun 9DOF Razor hardware (SEN-10125).
00020 *
00021 *   * Updated and extended by Peter Bartz (peter-bartz@gmx.de):
00022 *     * v1.3.0
00023 *       * Cleaned up, streamlined and restructured most of the code to make it more comprehensible.
00024 *       * Added sensor calibration (improves precision and responsiveness a lot!).
00025 *       * Added binary yaw/pitch/roll output.
00026 *       * Added basic serial command interface to set output modes/calibrate sensors/synch stream/etc.
00027 *       * Added support to synch automatically when using Rovering Networks Bluetooth modules (and compatible).
00028 *       * Wrote new easier to use test program (using Processing).
00029 *       * Added support for new version of "9DOF Razor IMU": SEN-10736.
00030 *       --> The output of this code is not compatible with the older versions!
00031 *       --> A Processing sketch to test the tracker is available.
00032 *     * v1.3.1
00033 *       * Initializing rotation matrix based on start-up sensor readings -> orientation OK right away.
00034 *       * Adjusted gyro low-pass filter and output rate settings.
00035 *     * v1.3.2
00036 *       * Adapted code to work with new Arduino 1.0 (and older versions still).
00037 *     * v1.3.3
00038 *       * Improved synching.
00039 *     * v1.4.0
00040 *       * Added support for SparkFun "9DOF Sensor Stick" (versions SEN-10183, SEN-10321 and SEN-10724).
00041 *       * Ported (v1.4.0) to MBED.org by Luke Petre (lpetre@gmail.com)
00042 *          http://mbed.org/users/lpetre/programs/RazorAHRS/
00043 *
00044 * TODOs:
00045 *   * Allow optional use of EEPROM for storing and reading calibration values.
00046 *   * Use self-test and temperature-compensation features of the sensors.
00047 *   * Add binary output of unfused sensor data for all 9 axes.
00048 ***************************************************************************************************************/
00049 
00050 /*
00051   "9DOF Razor IMU" hardware versions: SEN-10125 and SEN-10736
00052 
00053   ATMega328@3.3V, 8MHz
00054 
00055   ADXL345  : Accelerometer
00056   HMC5843  : Magnetometer on SEN-10125
00057   HMC5883L : Magnetometer on SEN-10736
00058   ITG-3200 : Gyro
00059 
00060   Arduino IDE : Select board "Arduino Pro or Pro Mini (3.3v, 8Mhz) w/ATmega328"
00061 */
00062 
00063 /*
00064   "9DOF Sensor Stick" hardware versions: SEN-10183, SEN-10321 and SEN-10724
00065 
00066   ADXL345  : Accelerometer
00067   HMC5843  : Magnetometer on SEN-10183 and SEN-10321
00068   HMC5883L : Magnetometer on SEN-10724
00069   ITG-3200 : Gyro
00070 */
00071 
00072 /*
00073   Axis definition (differs from definition printed on the board!):
00074     X axis pointing forward (towards the short edge with the connector holes)
00075     Y axis pointing to the right
00076     and Z axis pointing down.
00077     
00078   Positive yaw   : clockwise
00079   Positive roll  : right wing down
00080   Positive pitch : nose up
00081   
00082   Transformation order: first yaw then pitch then roll.
00083 */
00084 
00085 /*
00086   Commands that the firmware understands:
00087   
00088   "#o<param>" - Set output parameter. The available options are:
00089       "#o0" - Disable continuous streaming output.
00090       "#o1" - Enable continuous streaming output.
00091       "#ob" - Output angles in binary format (yaw/pitch/roll as binary float, so one output frame
00092               is 3x4 = 12 bytes long).
00093       "#ot" - Output angles in text format (Output frames have form like "#YPR=-142.28,-5.38,33.52",
00094               followed by carriage return and line feed [\r\n]).
00095       "#os" - Output (calibrated) sensor data of all 9 axes in text format. One frame consist of
00096               three lines - one for each sensor.
00097       "#oc" - Go to calibration output mode.
00098       "#on" - When in calibration mode, go on to calibrate next sensor.
00099       "#oe0" - Disable error message output.
00100       "#oe1" - Enable error message output.
00101     
00102   "#f" - Request one output frame - useful when continuous output is disabled and updates are
00103          required in larger intervals only.
00104   "#s<xy>" - Request synch token - useful to find out where the frame boundaries are in a continuous
00105          binary stream or to see if tracker is present and answering. The tracker will send
00106          "#SYNCH<xy>\r\n" in response (so it's possible to read using a readLine() function).
00107          x and y are two mandatory but arbitrary bytes that can be used to find out which request
00108          the answer belongs to.
00109           
00110   ("#C" and "#D" - Reserved for communication with optional Bluetooth module.)
00111   
00112   Newline characters are not required. So you could send "#ob#o1#s", which
00113   would set binary output mode, enable continuous streaming output and request
00114   a synch token all at once.
00115   
00116   The status LED will be on if streaming output is enabled and off otherwise.
00117   
00118   Byte order of binary output is little-endian: least significant byte comes first.
00119 */
00120 #include <mbed.h>
00121 #include <MODSERIAL.h>
00122 
00123 
00124 /*****************************************************************/
00125 /*********** USER SETUP AREA! Set your options here! *************/
00126 /*****************************************************************/
00127 
00128 // HARDWARE OPTIONS
00129 /*****************************************************************/
00130 // Select your hardware here by uncommenting one line!
00131 //#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
00132 //#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
00133 //#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
00134 //#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
00135 #define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
00136 
00137 
00138 // OUTPUT OPTIONS
00139 /*****************************************************************/
00140 // Set your serial port baud rate used to send out data here!
00141 #define OUTPUT__BAUD_RATE 57600
00142 
00143 // Sensor data output interval in milliseconds
00144 // This may not work, if faster than 20ms (=50Hz)
00145 // Code is tuned for 20ms, so better leave it like that
00146 #define OUTPUT__DATA_INTERVAL 20  // in milliseconds
00147 
00148 // Output mode
00149 #define OUTPUT__MODE_CALIBRATE_SENSORS 0 // Outputs sensor min/max values as text for manual calibration
00150 #define OUTPUT__MODE_ANGLES_TEXT 1 // Outputs yaw/pitch/roll in degrees as text
00151 #define OUTPUT__MODE_ANGLES_BINARY 2 // Outputs yaw/pitch/roll in degrees as binary float
00152 #define OUTPUT__MODE_SENSORS_TEXT 3 // Outputs (calibrated) sensor values for all 9 axes as text
00153 
00154 // Select if serial continuous streaming output is enabled per default on startup.
00155 #define OUTPUT__STARTUP_STREAM_ON true // true or false
00156 
00157 // Bluetooth
00158 // You can set this to true, if you have a Rovering Networks Bluetooth Module attached.
00159 // The connect/disconnect message prefix of the module has to be set to "#".
00160 // (Refer to manual, it can be set like this: SO,#)
00161 // When using this, streaming output will only be enabled as long as we're connected. That way
00162 // receiver and sender are synchronzed easily just by connecting/disconnecting.
00163 // It is not necessary to set this! It just makes life easier when writing code for
00164 // the receiving side. The Processing test sketch also works without setting this.
00165 // NOTE: When using this, OUTPUT__STARTUP_STREAM_ON has no effect!
00166 #define OUTPUT__HAS_RN_BLUETOOTH false  // true or false
00167 
00168 
00169 // SENSOR CALIBRATION
00170 /*****************************************************************/
00171 // How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
00172 // Put MIN/MAX and OFFSET readings for your board here!
00173 // Accelerometer
00174 // "accel x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
00175 #define ACCEL_X_MIN ((float) -263)
00176 #define ACCEL_X_MAX ((float) 263)
00177 #define ACCEL_Y_MIN ((float) -257)
00178 #define ACCEL_Y_MAX ((float) 277)
00179 #define ACCEL_Z_MIN ((float) -270)
00180 #define ACCEL_Z_MAX ((float) 251)
00181 
00182 // Magnetometer
00183 // "magn x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
00184 #define MAGN_X_MIN ((float) -600)
00185 #define MAGN_X_MAX ((float) 600)
00186 #define MAGN_Y_MIN ((float) -600)
00187 #define MAGN_Y_MAX ((float) 600)
00188 #define MAGN_Z_MIN ((float) -600)
00189 #define MAGN_Z_MAX ((float) 600)
00190 
00191 // Gyroscope
00192 // "gyro x,y,z (current/average) = .../OFFSET_X  .../OFFSET_Y  .../OFFSET_Z
00193 #define GYRO_AVERAGE_OFFSET_X ((float) 0.0)
00194 #define GYRO_AVERAGE_OFFSET_Y ((float) 0.0)
00195 #define GYRO_AVERAGE_OFFSET_Z ((float) 0.0)
00196 
00197 /*
00198 // Calibration example:
00199 // "accel x,y,z (min/max) = -278.00/270.00  -254.00/284.00  -294.00/235.00"
00200 #define ACCEL_X_MIN ((float) -278)
00201 #define ACCEL_X_MAX ((float) 270)
00202 #define ACCEL_Y_MIN ((float) -254)
00203 #define ACCEL_Y_MAX ((float) 284)
00204 #define ACCEL_Z_MIN ((float) -294)
00205 #define ACCEL_Z_MAX ((float) 235)
00206 
00207 // "magn x,y,z (min/max) = -511.00/581.00  -516.00/568.00  -489.00/486.00"
00208 #define MAGN_X_MIN ((float) -511)
00209 #define MAGN_X_MAX ((float) 581)
00210 #define MAGN_Y_MIN ((float) -516)
00211 #define MAGN_Y_MAX ((float) 568)
00212 #define MAGN_Z_MIN ((float) -489)
00213 #define MAGN_Z_MAX ((float) 486)
00214 
00215 //"gyro x,y,z (current/average) = -32.00/-34.82  102.00/100.41  -16.00/-16.38"
00216 #define GYRO_AVERAGE_OFFSET_X ((float) -34.82)
00217 #define GYRO_AVERAGE_OFFSET_Y ((float) 100.41)
00218 #define GYRO_AVERAGE_OFFSET_Z ((float) -16.38)
00219 */
00220 
00221 
00222 // DEBUG OPTIONS
00223 /*****************************************************************/
00224 // When set to true, gyro drift correction will not be applied
00225 #define DEBUG__NO_DRIFT_CORRECTION false
00226 // Print elapsed time after each I/O loop
00227 #define DEBUG__PRINT_LOOP_TIME false
00228 
00229 
00230 /*****************************************************************/
00231 /****************** END OF USER SETUP AREA!  *********************/
00232 /*****************************************************************/
00233 
00234 
00235 // Check if hardware version code is defined
00236 #ifndef HW__VERSION_CODE
00237   // Generate compile error
00238   #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.pde!
00239 #endif
00240 
00241 //#include <Wire.h>
00242 
00243 // Sensor calibration scale and offset values
00244 #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
00245 #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
00246 #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
00247 #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
00248 #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
00249 #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
00250 
00251 #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
00252 #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
00253 #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
00254 #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
00255 #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
00256 #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
00257 
00258 
00259 // Gain for gyroscope (ITG-3200)
00260 #define GYRO_GAIN 0.06957 // Same gain on all axes
00261 #define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
00262 
00263 // DCM parameters
00264 #define Kp_ROLLPITCH 0.02f
00265 #define Ki_ROLLPITCH 0.00002f
00266 #define Kp_YAW 1.2f
00267 #define Ki_YAW 0.00002f
00268 
00269 // Stuff
00270 #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
00271 #define TO_RAD(x) (x * 0.01745329252)  // *pi/180
00272 #define TO_DEG(x) (x * 57.2957795131)  // *180/pi
00273 #define NEW_LINE "\r\n"
00274 
00275 /**
00276 * Internal Measurement Unit
00277 *
00278 * Uses DCM algorithm to calculate Euler angle values
00279 *
00280 * Modified Razr_AHRS 1.4 code
00281 */
00282 class IMU {
00283 private:
00284     // Sensor variables
00285     int16_t accel[3];  // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
00286     int16_t accel_min[3];
00287     int16_t accel_max[3];
00288 
00289     int16_t magnetom[3];
00290     int16_t magnetom_min[3];
00291     int16_t magnetom_max[3];
00292 
00293     int16_t gyro[3];
00294     double temperature;
00295     double gyro_average[3];
00296     int gyro_num_samples;
00297     
00298     // gyro slope offset values
00299     float foffset[3];
00300     float slope[3];
00301 
00302     // Euler angles
00303     float yaw;
00304     float pitch;
00305     float roll;
00306 
00307     // DCM variables
00308     float MAG_Heading;
00309     float Accel_Vector[3]; // Store the acceleration in a vector
00310     float Gyro_Vector[3];  // Store the gyros turn rate in a vector
00311     float Omega_Vector[3]; // Corrected Gyro_Vector data
00312     float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction
00313     float Omega_I[3];//= {0, 0, 0}; // Omega Integrator
00314     float Omega[3];//= {0, 0, 0};
00315     float errorRollPitch[3];// = {0, 0, 0};
00316     float errorYaw[3];// = {0, 0, 0};
00317     float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
00318     float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
00319     float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
00320 
00321     // DCM timing in the main loop
00322     long timestamp;
00323     long timestamp_old;
00324     float G_Dt; // Integration time for DCM algorithm
00325 
00326     // More output-state variables
00327     int output_mode;
00328     bool output_stream_on;
00329     bool output_single_on;
00330     int curr_calibration_sensor;
00331     bool reset_calibration_session_flag;
00332     int num_accel_errors;
00333     int num_magn_errors;
00334     int num_gyro_errors;
00335 
00336     // If set true, an error message will be output if we fail to read sensor data.
00337     // Message format: "!ERR: reading <sensor>", followed by "\r\n".
00338     bool output_errors;
00339 
00340     DigitalOut statusLed;
00341     MODSERIAL pc;
00342     I2C Wire;
00343     Timer timer;
00344 
00345 public:    
00346     // Compass.cpp
00347     void Compass_Heading();
00348  
00349     // DCM.cpp
00350     void Normalize();
00351     void Drift_correction();
00352     void Matrix_update();
00353     void Euler_angles();
00354     
00355     // Output.cpp
00356     void output_angles();
00357     void output_calibration(int calibration_sensor);
00358     void output_sensors();
00359     
00360     // AHRS.cpp
00361     void read_sensors();
00362     void reset_sensor_fusion();
00363     void compensate_sensor_errors();
00364     void check_reset_calibration_session();
00365     void turn_output_stream_on();
00366     void turn_output_stream_off();
00367     char readChar();
00368     void readInput();
00369     
00370     IMU();
00371     void loop();
00372     
00373     /**
00374     * Loop that calculates values and places them in a array
00375     *
00376     * @param data array of length=4 to contain
00377     *                   data[0] = temperature (C);
00378     *                   data[1] = yaw (deg)
00379     *                   data[2] = pitch (deg)
00380     *                   data[3] = roll (deg)
00381     */
00382     void loop(float*);
00383     
00384     // Sensors.cpp
00385     void I2C_Init();
00386     void Accel_Init();
00387     void Read_Accel();
00388     void Magn_Init();
00389     void Read_Magn();
00390     void Gyro_Init();
00391     
00392     /**
00393     * Reads from the ITG3200 gyro
00394     *
00395     * Gyro xyz values in gyro[3]
00396     * Temperature value in temperature
00397     *
00398     * TODO: thermal drift adjustment
00399     */
00400     void Read_Gyro();
00401     
00402 };
00403 
00404 float Vector_Dot_Product(float vector1[3], float vector2[3]);
00405 void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]);
00406 void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2);
00407 void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]);
00408 void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]);
00409 void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll);
00410 float constrain(float in, float min, float max);
00411