I cannot believe that I'm the only one who noticed that! I would write it off as a faulty device but I've got two brand new once here that produce identical output with the same unmodified firmware (V1.9)
Here is the description of the problem:
1. Initialize IMU level
2. Slowly turn 90 in roll
3. Return to horizontal orientation.
4. Slowly turn -90 in roll
5. Return to horizontal orientation.
6. Repeat for pitch and yaw (yaw rotate 360 in 90 deg increments)
Each axis output should correspond to the number of degrees turned. Instead I get the following:
Roll reading: actual roll: - 90 deg -> RLL output: - 120
+ 90 deg -> RLL output: + 120
Pitch reading: actual pitch: -90 deg -> PCH output: -50 (goes from 0 to -89 and then decreases to -50)
+90 deg -> PCH output: +50 (goes from 0 to 89 and then decreases to 50)
Yaw reading: actual yaw: 0 deg -> YAW output: 180
90 deg -> YAW output: 108
180 deg -> YAW output: 45
270 deg -> YAW output: -40
360 deg -> YAW output: -180
Replies
the tan,sin,cos are multiplied by 1.9--- delete this 1.9 so in the last part in DCM
void Euler_angles(void)
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
pitch = asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
yaw = 0;
#else
pitch = asin(DCM_Matrix[2][0]);
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
#endif
}
Artem,
I just wanted to confirm that the two simple changes (GRAVITY=8192 and the two gain changes) are all that is required for you an ArduIMU 1.9.8 (neglecting YAW improvements - I don't use a GPS). I will retest my measurements - do you go all the way near 90 deg or just to maybe 45 deg.
Do you have any expectations of whether it will drift much as long as the rotation rates are low?
I appreciate your expertise in this - don't you think that whoever is responsible for the code should make your changes and release it.
It is Nov 20, 2012 and I am testing a V3 board. I added Artem's two fixes in the ArduIMU.pde :
1)
#if BOARD_VERSION == 3
#define SERIAL_MUX_PIN 7
#define RED_LED_PIN 5
#define BLUE_LED_PIN 6
#define YELLOW_LED_PIN 5 // Yellow led is not used on ArduIMU v3
// MPU6000 4g range => g = 4096
#define GRAVITY 8192
...
2)
// WAS #define Kp_ROLLPITCH 0.015
// WAS #define Ki_ROLLPITCH 0.000010
#define Kp_ROLLPITCH 1.515/GRAVITY
#define Ki_ROLLPITCH 0.00101/GRAVITY
and the roll/pitch seem smooth but I still get wrong values. At roll = 30, I get about 45 on printout and for pitch = 30, I get about 40 deg on the Serial Monitor printout. Anybody else get the same? I know I am actually compiling the new source just by changing the Version printout text slightly e.g. "1.9X" I started with the 1.9.8 download.
OK here is the problem description and the solution:
Problem #1:
As mentioned above the accelerometer calibration value was incorrect and should be 8192
// MPU6000 4g range => g = 8192
#define GRAVITY 8192 // This equivalent to 1G in the raw data coming from the accelerometer
Problem #2: DCM algorithm is implemented incorrectly for MPU-6000
With the previous accelerometer the output for 1G (9.8m/s2) was around 101 so roughly 10 times the raw ADC output. So in order to accelerate the computations Kp and Ki were adjusted accordingly (divided by 10) and then all calculations were done with the raw ADC data. Not a pretty way to write code but it worked and marginally accelerated computations.
However when you raw output was changed by a factor 83.51 for MPU6000 this approach would not work anymore. It didn't work with the old accelerometer calibration value (4096) and the new value only made matters worse.
For example: if the old accelerometer would output value of 1.03 the software would assume it to be 0.1m/s2 now when the new chipset would output the same value of 1.03 the software still infers the value of 0.1m/s2 however actual value measured is 0.0012m/s2. (the output for 0.1m/s2 from MPU6000 would be 83.51).
To write it correctly you would have to change the following lines in the DCM code:
Accel_Vector[0]=read_adc(3); // acc x
Accel_Vector[1]=read_adc(4); // acc y
Accel_Vector[2]=read_adc(5); // acc z
To Accel_Scale(read_adc(x)) version and adjust the weight parameters accordingly. This way it will work fine with any other accelerometer chip and makes the code much easier to read.
Alternative solution, which I implemented, which still utilizes the marginal acceleration of manipulating the raw ADC values is to adjust the values of Kp and Ki by the factor of 9.81/ACC_MAX_SCALE (10 to make it compatible with the existing algorithm), so by 83.51.
So the solution would be to change to lines in the initialization code to:
#define Kp_ROLLPITCH 1.515/GRAVITY
#define Ki_ROLLPITCH 0.00101/GRAVITY
This is tested and verified to work correctly.
Problem #3:
I just noticed this one while browsing through the code and haven't done anything about it yet:
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
Can anybody see an issue here? I'll give you a clue, look and the units. Does that formula provide a result in m/s2 ? ...I think not! :)
Luckily this piece of code doesn't affect anything unless you using GPS and then GPS speed is used to compensate for centrifugal force. Which I don't have hooked up right now and cannot test. However if this is an issue for your application I suggest you change it to the correct value and follow it through in the code.
Artem,
I left you a long reply last night, but I must of accidentally deleted it as I edited it a few times and it was very late. So let me try again. There are different hardware revisions of the MPU6000 out there. The early ones were version C and later ones are version D.
A while back I had expanded on the imu v1.9 to make it into a head tracker and a gimbal stabilizer and control. I have a CLI system for manaul calibration of the mag offsets. Everything was working fine on my 2 imu3 boards, so I never encountered a problem. After seeing your discussion I remembered that the APM2 had this same issue with the accelerometer scaling. It was solved by asking the chip for it's product code and setting the proper scaling according to the chip installed. I couldn't find the codes in the data sheet, so I just ported it over from ArduPlane 2.40 to my 1.9 based gimbal controller. It will print out the product code on initialization. Both my boards reported 0x14 which is version C. I'll bet you have a version D. Version C needs the 4096 selected and ver D needs 8192 for the accel scaling. You can download the gimbal controller zip file here. Be sure and read the wiki for instructions. You can run it without any RC input or servos and print out euler angles. After loading the code start the Arduino serial monitor and reset the board. It will show you the product code. Look in the MPU6000.h tab in the #definitions for the code definitions. My boards were both 0x14.
My comment about the mag calibration was only concerning yaw. Version 1.9 has no mag offset calibration. My version has manual calibration using cli input. You need to rotate the board flat on a nonmetallic table to get the x & y offsets, then turn it on its' side 90 deg and rotate again to get the z offsets.
Try it out and let me know if it fixes the problem on your board. The cli is always on. Type 'set up' to see the main menu, type 'go' to print out data. Also look at the user selectable options in the defines.h tab.
arduimu_v3_gimbal_control.01.zip
Artem,
Your yaw values are off due to lack of magnetometer offset calibration with imu 1.9. Try my version and do the calibration procedure and you will be amazed with the yaw performance. Just stay away from large ferrous metal objects like cars or steel desk tops.