Tyler Weaver
AHRS library, modified version of Peter Bartz work.
Diff: AHRS.h
- Revision:
- 0:014ee3239c80
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS.h Thu Nov 08 18:57:18 2012 +0000
@@ -0,0 +1,411 @@
+/***************************************************************************************************************
+* Razor AHRS Firmware v1.4.0
+* 9 Degree of Measurement Attitude and Heading Reference System
+* for Sparkfun "9DOF Razor IMU" (SEN-10125 and SEN-10736)
+* and "9DOF Sensor Stick" (SEN-10183, 10321 and SEN-10724)
+*
+* Released under GNU GPL (General Public License) v3.0
+* Copyright (C) 2011 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
+*
+* Infos, updates, bug reports and feedback:
+* http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
+*
+*
+* History:
+* * Original code (http://code.google.com/p/sf9domahrs/) by Doug Weibel and Jose Julio,
+* based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel. Thank you!
+*
+* * Updated code (http://groups.google.com/group/sf_9dof_ahrs_update) by David Malik (david.zsolt.malik@gmail.com)
+* for new Sparkfun 9DOF Razor hardware (SEN-10125).
+*
+* * Updated and extended by Peter Bartz (peter-bartz@gmx.de):
+* * v1.3.0
+* * Cleaned up, streamlined and restructured most of the code to make it more comprehensible.
+* * Added sensor calibration (improves precision and responsiveness a lot!).
+* * Added binary yaw/pitch/roll output.
+* * Added basic serial command interface to set output modes/calibrate sensors/synch stream/etc.
+* * Added support to synch automatically when using Rovering Networks Bluetooth modules (and compatible).
+* * Wrote new easier to use test program (using Processing).
+* * Added support for new version of "9DOF Razor IMU": SEN-10736.
+* --> The output of this code is not compatible with the older versions!
+* --> A Processing sketch to test the tracker is available.
+* * v1.3.1
+* * Initializing rotation matrix based on start-up sensor readings -> orientation OK right away.
+* * Adjusted gyro low-pass filter and output rate settings.
+* * v1.3.2
+* * Adapted code to work with new Arduino 1.0 (and older versions still).
+* * v1.3.3
+* * Improved synching.
+* * v1.4.0
+* * Added support for SparkFun "9DOF Sensor Stick" (versions SEN-10183, SEN-10321 and SEN-10724).
+* * Ported (v1.4.0) to MBED.org by Luke Petre (lpetre@gmail.com)
+* http://mbed.org/users/lpetre/programs/RazorAHRS/
+*
+* TODOs:
+* * Allow optional use of EEPROM for storing and reading calibration values.
+* * Use self-test and temperature-compensation features of the sensors.
+* * Add binary output of unfused sensor data for all 9 axes.
+***************************************************************************************************************/
+
+/*
+ "9DOF Razor IMU" hardware versions: SEN-10125 and SEN-10736
+
+ ATMega328@3.3V, 8MHz
+
+ ADXL345 : Accelerometer
+ HMC5843 : Magnetometer on SEN-10125
+ HMC5883L : Magnetometer on SEN-10736
+ ITG-3200 : Gyro
+
+ Arduino IDE : Select board "Arduino Pro or Pro Mini (3.3v, 8Mhz) w/ATmega328"
+*/
+
+/*
+ "9DOF Sensor Stick" hardware versions: SEN-10183, SEN-10321 and SEN-10724
+
+ ADXL345 : Accelerometer
+ HMC5843 : Magnetometer on SEN-10183 and SEN-10321
+ HMC5883L : Magnetometer on SEN-10724
+ ITG-3200 : Gyro
+*/
+
+/*
+ Axis definition (differs from definition printed on the board!):
+ X axis pointing forward (towards the short edge with the connector holes)
+ Y axis pointing to the right
+ and Z axis pointing down.
+
+ Positive yaw : clockwise
+ Positive roll : right wing down
+ Positive pitch : nose up
+
+ Transformation order: first yaw then pitch then roll.
+*/
+
+/*
+ Commands that the firmware understands:
+
+ "#o<param>" - Set output parameter. The available options are:
+ "#o0" - Disable continuous streaming output.
+ "#o1" - Enable continuous streaming output.
+ "#ob" - Output angles in binary format (yaw/pitch/roll as binary float, so one output frame
+ is 3x4 = 12 bytes long).
+ "#ot" - Output angles in text format (Output frames have form like "#YPR=-142.28,-5.38,33.52",
+ followed by carriage return and line feed [\r\n]).
+ "#os" - Output (calibrated) sensor data of all 9 axes in text format. One frame consist of
+ three lines - one for each sensor.
+ "#oc" - Go to calibration output mode.
+ "#on" - When in calibration mode, go on to calibrate next sensor.
+ "#oe0" - Disable error message output.
+ "#oe1" - Enable error message output.
+
+ "#f" - Request one output frame - useful when continuous output is disabled and updates are
+ required in larger intervals only.
+ "#s<xy>" - Request synch token - useful to find out where the frame boundaries are in a continuous
+ binary stream or to see if tracker is present and answering. The tracker will send
+ "#SYNCH<xy>\r\n" in response (so it's possible to read using a readLine() function).
+ x and y are two mandatory but arbitrary bytes that can be used to find out which request
+ the answer belongs to.
+
+ ("#C" and "#D" - Reserved for communication with optional Bluetooth module.)
+
+ Newline characters are not required. So you could send "#ob#o1#s", which
+ would set binary output mode, enable continuous streaming output and request
+ a synch token all at once.
+
+ The status LED will be on if streaming output is enabled and off otherwise.
+
+ Byte order of binary output is little-endian: least significant byte comes first.
+*/
+#include <mbed.h>
+#include <MODSERIAL.h>
+
+
+/*****************************************************************/
+/*********** USER SETUP AREA! Set your options here! *************/
+/*****************************************************************/
+
+// HARDWARE OPTIONS
+/*****************************************************************/
+// Select your hardware here by uncommenting one line!
+//#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
+//#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
+#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
+
+
+// OUTPUT OPTIONS
+/*****************************************************************/
+// Set your serial port baud rate used to send out data here!
+#define OUTPUT__BAUD_RATE 57600
+
+// Sensor data output interval in milliseconds
+// This may not work, if faster than 20ms (=50Hz)
+// Code is tuned for 20ms, so better leave it like that
+#define OUTPUT__DATA_INTERVAL 20 // in milliseconds
+
+// Output mode
+#define OUTPUT__MODE_CALIBRATE_SENSORS 0 // Outputs sensor min/max values as text for manual calibration
+#define OUTPUT__MODE_ANGLES_TEXT 1 // Outputs yaw/pitch/roll in degrees as text
+#define OUTPUT__MODE_ANGLES_BINARY 2 // Outputs yaw/pitch/roll in degrees as binary float
+#define OUTPUT__MODE_SENSORS_TEXT 3 // Outputs (calibrated) sensor values for all 9 axes as text
+
+// Select if serial continuous streaming output is enabled per default on startup.
+#define OUTPUT__STARTUP_STREAM_ON true // true or false
+
+// Bluetooth
+// You can set this to true, if you have a Rovering Networks Bluetooth Module attached.
+// The connect/disconnect message prefix of the module has to be set to "#".
+// (Refer to manual, it can be set like this: SO,#)
+// When using this, streaming output will only be enabled as long as we're connected. That way
+// receiver and sender are synchronzed easily just by connecting/disconnecting.
+// It is not necessary to set this! It just makes life easier when writing code for
+// the receiving side. The Processing test sketch also works without setting this.
+// NOTE: When using this, OUTPUT__STARTUP_STREAM_ON has no effect!
+#define OUTPUT__HAS_RN_BLUETOOTH false // true or false
+
+
+// SENSOR CALIBRATION
+/*****************************************************************/
+// How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
+// Put MIN/MAX and OFFSET readings for your board here!
+// Accelerometer
+// "accel x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
+#define ACCEL_X_MIN ((float) -263)
+#define ACCEL_X_MAX ((float) 263)
+#define ACCEL_Y_MIN ((float) -257)
+#define ACCEL_Y_MAX ((float) 277)
+#define ACCEL_Z_MIN ((float) -270)
+#define ACCEL_Z_MAX ((float) 251)
+
+// Magnetometer
+// "magn x,y,z (min/max) = X_MIN/X_MAX Y_MIN/Y_MAX Z_MIN/Z_MAX"
+#define MAGN_X_MIN ((float) -600)
+#define MAGN_X_MAX ((float) 600)
+#define MAGN_Y_MIN ((float) -600)
+#define MAGN_Y_MAX ((float) 600)
+#define MAGN_Z_MIN ((float) -600)
+#define MAGN_Z_MAX ((float) 600)
+
+// Gyroscope
+// "gyro x,y,z (current/average) = .../OFFSET_X .../OFFSET_Y .../OFFSET_Z
+#define GYRO_AVERAGE_OFFSET_X ((float) 0.0)
+#define GYRO_AVERAGE_OFFSET_Y ((float) 0.0)
+#define GYRO_AVERAGE_OFFSET_Z ((float) 0.0)
+
+/*
+// Calibration example:
+// "accel x,y,z (min/max) = -278.00/270.00 -254.00/284.00 -294.00/235.00"
+#define ACCEL_X_MIN ((float) -278)
+#define ACCEL_X_MAX ((float) 270)
+#define ACCEL_Y_MIN ((float) -254)
+#define ACCEL_Y_MAX ((float) 284)
+#define ACCEL_Z_MIN ((float) -294)
+#define ACCEL_Z_MAX ((float) 235)
+
+// "magn x,y,z (min/max) = -511.00/581.00 -516.00/568.00 -489.00/486.00"
+#define MAGN_X_MIN ((float) -511)
+#define MAGN_X_MAX ((float) 581)
+#define MAGN_Y_MIN ((float) -516)
+#define MAGN_Y_MAX ((float) 568)
+#define MAGN_Z_MIN ((float) -489)
+#define MAGN_Z_MAX ((float) 486)
+
+//"gyro x,y,z (current/average) = -32.00/-34.82 102.00/100.41 -16.00/-16.38"
+#define GYRO_AVERAGE_OFFSET_X ((float) -34.82)
+#define GYRO_AVERAGE_OFFSET_Y ((float) 100.41)
+#define GYRO_AVERAGE_OFFSET_Z ((float) -16.38)
+*/
+
+
+// DEBUG OPTIONS
+/*****************************************************************/
+// When set to true, gyro drift correction will not be applied
+#define DEBUG__NO_DRIFT_CORRECTION false
+// Print elapsed time after each I/O loop
+#define DEBUG__PRINT_LOOP_TIME false
+
+
+/*****************************************************************/
+/****************** END OF USER SETUP AREA! *********************/
+/*****************************************************************/
+
+
+// Check if hardware version code is defined
+#ifndef HW__VERSION_CODE
+ // Generate compile error
+ #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.pde!
+#endif
+
+//#include <Wire.h>
+
+// Sensor calibration scale and offset values
+#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
+#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
+#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
+#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
+#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
+#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
+
+#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
+#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
+#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
+#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
+#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
+#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
+
+
+// Gain for gyroscope (ITG-3200)
+#define GYRO_GAIN 0.06957 // Same gain on all axes
+#define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
+
+// DCM parameters
+#define Kp_ROLLPITCH 0.02f
+#define Ki_ROLLPITCH 0.00002f
+#define Kp_YAW 1.2f
+#define Ki_YAW 0.00002f
+
+// Stuff
+#define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
+#define TO_RAD(x) (x * 0.01745329252) // *pi/180
+#define TO_DEG(x) (x * 57.2957795131) // *180/pi
+#define NEW_LINE "\r\n"
+
+/**
+* Internal Measurement Unit
+*
+* Uses DCM algorithm to calculate Euler angle values
+*
+* Modified Razr_AHRS 1.4 code
+*/
+class IMU {
+private:
+ // Sensor variables
+ int16_t accel[3]; // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
+ int16_t accel_min[3];
+ int16_t accel_max[3];
+
+ int16_t magnetom[3];
+ int16_t magnetom_min[3];
+ int16_t magnetom_max[3];
+
+ int16_t gyro[3];
+ double temperature;
+ double gyro_average[3];
+ int gyro_num_samples;
+
+ // gyro slope offset values
+ float foffset[3];
+ float slope[3];
+
+ // Euler angles
+ float yaw;
+ float pitch;
+ float roll;
+
+ // DCM variables
+ float MAG_Heading;
+ float Accel_Vector[3]; // Store the acceleration in a vector
+ float Gyro_Vector[3]; // Store the gyros turn rate in a vector
+ float Omega_Vector[3]; // Corrected Gyro_Vector data
+ float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction
+ float Omega_I[3];//= {0, 0, 0}; // Omega Integrator
+ float Omega[3];//= {0, 0, 0};
+ float errorRollPitch[3];// = {0, 0, 0};
+ float errorYaw[3];// = {0, 0, 0};
+ float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
+ float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+
+ // DCM timing in the main loop
+ long timestamp;
+ long timestamp_old;
+ float G_Dt; // Integration time for DCM algorithm
+
+ // More output-state variables
+ int output_mode;
+ bool output_stream_on;
+ bool output_single_on;
+ int curr_calibration_sensor;
+ bool reset_calibration_session_flag;
+ int num_accel_errors;
+ int num_magn_errors;
+ int num_gyro_errors;
+
+ // If set true, an error message will be output if we fail to read sensor data.
+ // Message format: "!ERR: reading <sensor>", followed by "\r\n".
+ bool output_errors;
+
+ DigitalOut statusLed;
+ MODSERIAL pc;
+ I2C Wire;
+ Timer timer;
+
+public:
+ // Compass.cpp
+ void Compass_Heading();
+
+ // DCM.cpp
+ void Normalize();
+ void Drift_correction();
+ void Matrix_update();
+ void Euler_angles();
+
+ // Output.cpp
+ void output_angles();
+ void output_calibration(int calibration_sensor);
+ void output_sensors();
+
+ // AHRS.cpp
+ void read_sensors();
+ void reset_sensor_fusion();
+ void compensate_sensor_errors();
+ void check_reset_calibration_session();
+ void turn_output_stream_on();
+ void turn_output_stream_off();
+ char readChar();
+ void readInput();
+
+ IMU();
+ void loop();
+
+ /**
+ * Loop that calculates values and places them in a array
+ *
+ * @param data array of length=4 to contain
+ * data[0] = temperature (C);
+ * data[1] = yaw (deg)
+ * data[2] = pitch (deg)
+ * data[3] = roll (deg)
+ */
+ void loop(float*);
+
+ // Sensors.cpp
+ void I2C_Init();
+ void Accel_Init();
+ void Read_Accel();
+ void Magn_Init();
+ void Read_Magn();
+ void Gyro_Init();
+
+ /**
+ * Reads from the ITG3200 gyro
+ *
+ * Gyro xyz values in gyro[3]
+ * Temperature value in temperature
+ *
+ * TODO: thermal drift adjustment
+ */
+ void Read_Gyro();
+
+};
+
+float Vector_Dot_Product(float vector1[3], float vector2[3]);
+void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]);
+void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2);
+void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]);
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]);
+void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll);
+float constrain(float in, float min, float max);
+