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
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 false // 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) -250) 00176 #define ACCEL_X_MAX ((float) 250) 00177 #define ACCEL_Y_MIN ((float) -250) 00178 #define ACCEL_Y_MAX ((float) 250) 00179 #define ACCEL_Z_MIN ((float) -250) 00180 #define ACCEL_Z_MAX ((float) 250) 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 class IMU { 00276 public: 00277 // Sensor variables 00278 int16_t accel[3]; // Actually stores the NEGATED acceleration (equals gravity, if board not moving). 00279 int16_t accel_min[3]; 00280 int16_t accel_max[3]; 00281 00282 int16_t magnetom[3]; 00283 int16_t magnetom_min[3]; 00284 int16_t magnetom_max[3]; 00285 00286 int16_t gyro[3]; 00287 int16_t gyro_average[3]; 00288 int gyro_num_samples; 00289 00290 // Euler angles 00291 float yaw; 00292 float pitch; 00293 float roll; 00294 00295 // DCM variables 00296 float MAG_Heading; 00297 float Accel_Vector[3]; // Store the acceleration in a vector 00298 float Gyro_Vector[3]; // Store the gyros turn rate in a vector 00299 float Omega_Vector[3]; // Corrected Gyro_Vector data 00300 float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction 00301 float Omega_I[3];//= {0, 0, 0}; // Omega Integrator 00302 float Omega[3];//= {0, 0, 0}; 00303 float errorRollPitch[3];// = {0, 0, 0}; 00304 float errorYaw[3];// = {0, 0, 0}; 00305 float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 00306 float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; 00307 float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 00308 00309 // DCM timing in the main loop 00310 long timestamp; 00311 long timestamp_old; 00312 float G_Dt; // Integration time for DCM algorithm 00313 00314 // More output-state variables 00315 int output_mode; 00316 bool output_stream_on; 00317 bool output_single_on; 00318 int curr_calibration_sensor; 00319 bool reset_calibration_session_flag; 00320 int num_accel_errors; 00321 int num_magn_errors; 00322 int num_gyro_errors; 00323 00324 // If set true, an error message will be output if we fail to read sensor data. 00325 // Message format: "!ERR: reading <sensor>", followed by "\r\n". 00326 bool output_errors; 00327 00328 DigitalOut statusLed; 00329 MODSERIAL pc; 00330 I2C Wire; 00331 Timer timer; 00332 00333 public: 00334 // Compass.cpp 00335 void Compass_Heading(); 00336 00337 // DCM.cpp 00338 void Normalize(); 00339 void Drift_correction(); 00340 void Matrix_update(); 00341 void Euler_angles(); 00342 00343 // Output.cpp 00344 void output_angles(); 00345 void output_calibration(int calibration_sensor); 00346 void output_sensors(); 00347 00348 00349 // Razor_AHRS.cpp 00350 void read_sensors(); 00351 void reset_sensor_fusion(); 00352 void compensate_sensor_errors(); 00353 void check_reset_calibration_session(); 00354 void turn_output_stream_on(); 00355 void turn_output_stream_off(); 00356 char readChar(); 00357 void readInput(); 00358 00359 IMU(); 00360 void loop(); 00361 00362 00363 // Sensors.cpp 00364 void I2C_Init(); 00365 void Accel_Init(); 00366 void Read_Accel(); 00367 void Magn_Init(); 00368 void Read_Magn(); 00369 void Gyro_Init(); 00370 void Read_Gyro(); 00371 00372 }; 00373 00374 float Vector_Dot_Product(float vector1[3], float vector2[3]); 00375 void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]); 00376 void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2); 00377 void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]); 00378 void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]); 00379 void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll); 00380 float constrain(float in, float min, float max); 00381
Generated on Sun Jul 17 2022 22:09:51 by 1.7.2