05. Configuring the FreeIMU Library and FreeIMU Serial Sketch
There are several configurable options in the FreeIMU library that either must be set or are optional. Some are accessible at run time through the Processing sketch or in the FreeIMU.h which is loaded into the Arduino.
- IMU Selection (this is mandatory) and is set in FreeIMU.h. To select your IMU uncomment from the available list of boards (right now the DFROBOT 10DOF IMU is selected:
// Uncomment the appropriated version of FreeIMU you are using
//#define FREEIMU_v01
//#define FREEIMU_v02
//#define FREEIMU_v03
//#define FREEIMU_v035
//#define FREEIMU_v035_MS
//#define FREEIMU_v035_BMP
//#define FREEIMU_v04
// 3rd party boards. Please consider donating or buying a FreeIMU board to support this library development.
//#define SEN_10121 //IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345 SEN-10121 http://www.sparkfun.com/products/10121
//#define SEN_10736 //9 Degrees of Freedom - Razor IMU SEN-10736 http://www.sparkfun.com/products/10736/
//#define SEN_10724 //9 Degrees of Freedom - Sensor Stick SEN-10724 http://www.sparkfun.com/products/10724
//#define SEN_10183 //9 Degrees of Freedom - Sensor Stick SEN-10183 http://www.sparkfun.com/products/10183
//#define ARDUIMU_v3 // DIYDrones ArduIMU+ V3 http://store.diydrones.com/ArduIMU_V3_p/kt-arduimu-30.htm or https://www.sparkfun.com/products/11055
//#define GEN_MPU6050 // Generic MPU6050 breakout board. Compatible with GY-521, SEN-11028 and other MPU6050 wich have the MPU6050 AD0 pin connected to GND.
#define DFROBOT //DFROBOT 10DOF SEN-1040 IMU
//#define MPU9250_5611 //MPU-9250 IMU with MS5611 Altimeter from eBay
//#define GEN_MPU9150
//#define GEN_MPU9250
//#define Altimu10 // Pololu AltIMU v10 - 10 DOF IMU - http://www.pololu.com/product/1269
//#define GY_88 //GY-88 Sensor Board from eBay
//#define GY_87 //GY-87 Sensor Board from eBay, NOTE: Pressusre sensor is BMP180 but BMP085 library should work - You can also enable or disable the magnetometer (currently set for enable) by either commenting or uncommenting the following line:
//#define DISABLE_MAGN // Uncomment this line to disable the magnetometer in the sensor fusion algorithm- Set your magnetic declination
//Magnetic declination angle for Compass
#define MAG_DEC 4 //+4.0 degrees for Israel
#define MAG_DEC -13.1603 //degrees for Flushing, NYYou can determine your magnetic declination by visiting the NOAA National Geophysical Data Center. In addition, if you go to the tab Magnetic Field it will give you the magnetic field data for your location which you can use to determine if you magnetometer readings are in the ballpark. Just remember it will not be exact for a myriad of reasons. Here is a screen shot,
- Set which AHRS algorithm you want to use:
// Set filter type: 1 = Madgwick Gradient Descent, 0 - Madgwick implementation of Mahoney DCM
// in Quaternion form, 3 = Madwick's Original Paper AHRS
#define MARG 1- Algorithm filter factors. Thing the comments are pretty explanatory.
// proportional gain governs rate of convergence to accelerometer/magnetometer
// integral gain governs rate of convergence of gyroscope biases
// set up defines for various boards in my inventory, DFROBOT and Freeimu have
// temperature calibration curves. (3.31.14)
//
//Kp and Ki are used in the MahonyAHRS and betaDef in the MadgwickAHRS
#if defined(DFROBOT)
#define twoKpDef (2.0f * 0.5f)
#define twoKiDef (2.0f * 0.00002f)
#define betaDef 0.1f
#elif defined(FREEIMU_v04)
#define twoKpDef (2.0f * 0.75f) //works with and without mag enabled
#define twoKiDef (2.0f * 0.1625f)
#define betaDef 0.1f
#elif defined(GEN_MPU6050)
#define twoKpDef (2.0f * 0.5f)
#define twoKiDef (2.0f * 0.25f)
#define betaDef 0.1f
#elif defined(GEN_MPU9150)
#define twoKpDef (2.0f * 0.75f)
#define twoKiDef (2.0f * 0.1f)
#define betaDef 0.01f
#elif defined(Altimu10)
//#define twoKpDef (2.0f * 1.01f)
//#define twoKiDef (2.0f * 0.00002f)
#define twoKpDef (2.0f * 2.75f)
#define twoKiDef (2.0f * 0.1625f)
#define betaDef 1.1f
#elif defined(GEN_MPU9250)
#define twoKpDef (2.0f * 1.75f) // was 0.95
#define twoKiDef (2.0f * 0.05f) // was 0.05
#define betaDef 0.0f
#else
#define twoKpDef (2.0f * 0.5f)
#define twoKiDef (2.0f * 0.1f)
#define betaDef 0.1f
#endifIn a post on DIY Drones (http://diydrones.com/forum/topics/freeimu-firmware-on-arduimu?commentId=705844%3AComment%3A1010680) by Seb Madgwick in 2011 he suggested the following for tuning Kp and Ki: Leave Ki as 0 and start with a Kp value of 5. You will want to reduce your Kp value from this by 10 or even 100 times when tuning. The lowest value of Kp you can use is dependent on: gyroscope bias calibration errors, and gyroscope sensitivity calibration errors and expected angular dynamics of application (coupled characteristics).
For the MadgwickAHRS betaDef term, suggest that you read Seb Madgwick’s Internal Report paper.`
After rummaging around the net for awhile I found the following in one of the forums that I also described on one of the issue pages. If you read the paper the Beta term is way less than one and from what I am seeing a good starting point is around 2 and lower. Here is a quote by Seb Madgwick regarding beta based on the method in his original paper:
sebmadgwick wrote: khatarat wrote:...possible explain more about this two variables and also beta and zeta.
I will summarize the algorithm and explain beta’s role:
The filter uses a quaternion to represent orientation. Gyroscope data is represented as the rate of change of a quaternion which, when integrated, provides the estimated orientation of the sensor array. However, this estimate alone will drift over time and so must be ‘corrected’.
The accelerometer and magnetometer provide measurements of the earth’s gravitational and magnetic fields (respectively) which may be compared to the expected measurements of each field (according to the estimated orientation of the sensor array) to compute an error. Using non-linear programming methods, this error can be used to compute a vector in quaternion space that defines the required ‘corrective step’ that must be taken to reduce the error in the quaternion describing the estimated orientation . If this vector is normalised to a unit vector then it may be multiplied by a gain (beta) that defines the rate of ‘correction’ or the ‘rate of convergence’.
If the rate of convergence (beta) is too great then the filter output will be ‘noisy’ as it tries to track all accelerometer and magnetometer data; but if the rate of convergence is too small integral drift may take hold. Therefore an optimal value of beta may be defined as that which ensures the rate of convergence due to the ‘corrective step’ is equal to the rate of divergence due to integral drift. The divergence due to integral drift is the the gyroscope measurement error (gyroMeasError). The units of beta are defined as the ‘magnitude of the rate of change of quaternion’, the units of gyroMeasError are degrees per second; therefore a simple conversion is required (section 3.6 of report). khatarat wrote:... can i use values in your program?
I recommend using zeta=0 initially on the condition that you observe gyroscope bias drift as being negligible. Once you have a beta value that provides suitable convergences, then tune a zeta value to suitable tackle your gyroscope bias drift.
zeta is related to paper ( see his paper) which also has this in the header for his code:
// System constants
#define deltat 0.001f // sampling period in seconds (shown as 1 ms)
#define gyroMeasError 3.14159265358979 * (5.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s)
#define gyroMeasDrift 3.14159265358979 * (0.2f / 180.0f) // gyroscope measurement error in rad/s/s (shown as 0.2f deg/s/s)
#define beta sqrt(3.0f / 4.0f) * gyroMeasError // compute beta
#define zeta sqrt(3.0f / 4.0f) * gyroMeasDrift // compute zetaFor Madwick's original method I am using the following for the MPU-6000 and MPU-6050.
#define gyroMeasError 3.14159265358979 * (.50f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s)
#define gyroMeasDrift 3.14159265358979 * (0.02f / 180.0f) // gyroscope measurement error in rad/s/s (shown as 0.2f deg/s/s)
#define beta1 sqrt(3.0f / 4.0f) * gyroMeasError // compute beta
#define zeta sqrt(3.0f / 4.0f) * gyroMeasDrift // compute zeta
- Other options
//
// Other Options
//
#define temp_break -1000 //original temp_break = -4300;
#define senTemp_break 32
#define temp_corr_on_default 0
#define nsamples 75
#define instability_fix 1The first three defines are used for temperature compensation. For the MPU-6050 the temp_break define sets the temp in counts where the temp compensation should stop, the second define is for other sensors, the third is the default value for temperature compensation on/off.
nsamples is the number of gyro samples that is taken for zeroing the gyro when the unit is steady.
instability_fix is set to 1 to use an new routine for inv_srt as suggested by Tobias Simon in is post on DIY Drones forum, Madgwick IMU/AHRS and Fast Inverse Square Root.
I will post an Appendix on temperature compensation method that I used along with some references.
That should be for configuring the FreeIMU library prior to final upload.
Now lets talk about configuring the Arudino sketch, FreeIMU_serial_ARM_CPU.ino or the FreeIMU_serial.ino.
The options in the Arduino sketch are limited to identifying whether there is a GPS available, serial port GPS is connected to and the baudrate:
GPS is defined when HAS_GPS is set to 1, line 37 of the sketch
#define HAS_GPS 0 //0 – no GPS, 1 – GPS connected
Baudrate is set in the line following the GPS:
#define BaudRate 57600 // Set to the baud rate you want to run at
The serial port for the GPS is set at line 84 if you are using FreeIMU_serial_ARM_cpu.ino
Serial1.begin(GPSBaud);
If you are using a Mega or Uno you will be using the FreeIMU_serial.ino sketch. This version uses AltSoftSerial library (https://www.pjrc.com/teensy/td_libs_AltSoftSerial.html):
| Board | Transmit Pin | Receive Pin | Unusable PWM |
|---|---|---|---|
| Teensy 3.0 & 3.1 | 21 | 20 | 22 |
| Teensy 2.0 | 9 | 10 | (none) |
| Teensy++ 2.0 | 25 | 4 | 26, 27 |
| Arduino Uno | 9 | 8 | 10 |
| Arduino Leonardo | 5 | 13 | (none) |
| Arduino Mega | 46 | 48 | 44, 45 |
| Wiring-S | 5 | 6 | 4 |
| Sanguino | 13 | 14 | 12 |
The next section will walk you through changing the accelerometer, gryo and magnetometer settings in the various libraries.