Open phhoef opened 4 years ago
phhoef
commented
4 years ago Small Update:
When using the old implementation of dmpGetYawPitchRoll function by setting #define USE_OLD_DMPGETYAWPITCHROLL, the deviation between both positions is smaller.
flat: -0.000646795262583
side: -0.007850996218622
intensite
commented
4 years ago I have a similar situation, where I mount the MPU-6050 vertically in a model rocket.
I noticed that it works best when you run the calibration program (to get the offsets) while the device lays flat. Then I supply these offsets to the library with the following commands:
// supply your own gyro offsets here
mpu.setXGyroOffset(_CONF.X_GYRO_OFFSETS);
mpu.setYGyroOffset(_CONF.Y_GYRO_OFFSETS);
mpu.setZGyroOffset(_CONF.Z_GYRO_OFFSETS);
mpu.setXAccelOffset(_CONF.X_ACCEL_OFFSETS);
mpu.setYAccelOffset(_CONF.Y_ACCEL_OFFSETS);
mpu.setZAccelOffset(_CONF.Z_ACCEL_OFFSETS); And I make sure NOT to call the auto calibration after when the device is mounted vertically in the rocket.
// Calibration Time: generate offsets and calibrate our MPU6050
//mpu.CalibrateAccel(6); // Stephen Disabled for now (Seems to be affected when the board is started horizontally)
//mpu.CalibrateGyro(6); // Stephen Disabled for now (Seems to be affected when the board is started horizontally)This is MY personal experience and I am far from an expert here. Maybe the master @jrowberg could chime-in.
Paulie92
commented
4 years ago I have the same experience. I am working on project of artificial horizon and I have the MPU mounted vertically on the PCB. I am also working with the fix offsets.
Problem is that the library working well only if the default position of the chip is horizontally. I was trying to swap the X and Z of the "dmpGetYawPitchRoll" but it does not works. I assume that the DMP have to know which axis is the vertical and as default it is probably the Z, but I don't know how to change it.
intensite
commented
4 years ago Please post back here if you find something new on this topic. I will do the same.
ZHomeSlice
commented
4 years ago @intensite Mounting Vertically... The Gyro Calibration could be done at any angle. On the other hand, the Accelerometer calibration requires gravity to be in a known direction. in order to calibrate the accelerometer, we must remove gravity from the measurements so I programmed the calibration routine to remove gravity while the MPU6050 was flat. This doesn't mean that by changing the program you couldn't remove gravity from another angle.
Lind 3315 in MPU6050.cpp
if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity
you could try:
if ((ReadAddress == 0x3B)&&(i == 0)) Reading -= 16384; //remove Gravity
or
if ((ReadAddress == 0x3B)&&(i == 1)) Reading -= 16384; //remove Gravity
0 (Zero) is for X and 1 (one) is for Y
@Paulie92 & @intensite This is to point you in (hopefully) the right direction. I am not any good at Quaternian math!
This is the Yaw Pitch and Roll math that we are using:
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan2(gravity -> y , gravity -> z);
if (gravity -> z < 0) {
if(data[1] > 0) {
data[1] = PI - data[1];
} else {
data[1] = -PI - data[1];
}
}
return 0;
}Notice that Yaw doesn't use gravity in its equation but Pitch and Roll do. (I Believe) This math assumes that gravity is by default positioned below the MPU6050 in this way angles are based primarily on acceleration rather than gyro measurements. Note: without gravity for reference, you will experience drift in all directions rather than just yaw. I would modify this math to use 100% gyro for measurements. Just a thought: When your rocket launches, it will accelerate and spoof the accelerometer into thinking gravity is still down towards the bottom of the rocket while the rocket is accelerating in any direction. Now with this observation and these comments, I am adding this disclaimer. I do not fully understand Quaternion math.
Z
phhoef
commented
4 years ago @ZHomeSlice I saw a similar answer of yours in another thread, but I still don't understand it completely. I am calibrating the MPU, when it lays on a leveling device like shown in the first pic. This suits to your assumption in code, that gravity is in Z direction, right? So, I assume the calibration works properly.
Now, when I change position of the MPU, so that it lays on the side, the angle becomes weird like described in the first post.
I would have expect, that the assumption, that gravity is in Z direction is only important for the calibration, but not for the measurement once the calibration has been finished.
In addition, it seems that the angle becomes worse with the new dmpGetYawPitchRoll method.
The new dmpGetYawPitchRoll tries to take care, if the MPU lies on the back, right? That's why you try to distinguish between gravity < 0. But, as far as I understand all this, this might be the problem in my case. When MPU lies on the side, gravity is max on Y axis ...Could this become a problem?
ZHomeSlice
commented
4 years ago @phhoef
This suits to your assumption in code, that gravity is in Z direction, right?
This is correct I remove gravity from the Z direction so that the resulting calculations will become as close to Zero as possible. I land this by using a PID feedback loop which instantly adds or subtracts values from the set offsets and requests a new reading. After getting close on the first cycle my routine steps to Tighter PID multipliers then processes this information again. You can set the number of steps. At some point, after about 6~10 steps the calibration will have very little influence on offsets landing the calibration to as close as possible to the desired calibration.
So, I assume the calibration works properly.
Yes. Calibration is performed prior to even installing the Embedded DMP into the MPU The calibration focuses on Rates of change and not the math involved in determining the actual orientation of the MPU. The result of the calibration routine is that all rates except Z are producing a value of close to Zero as possible and Z is at 1G of acceleration
Also, note that further calibration occurs on V6.12 the Gyro calibration built into the Embedded DMP software has a routine that refines the gyro offsets behind the scenes when the MPU is still for a short period of time (I believe 8 seconds).
I would have expect, that the assumption, that gravity is in Z direction is only important for the calibration, but not for the measurement once the calibration has been finished. In addition, it seems that the angle becomes worse with the new dmpGetYawPitchRoll method. The new dmpGetYawPitchRoll tries to take care, if the MPU lies on the back, right? That's why you try to distinguish between gravity < 0. But, as far as I understand all this, this might be the problem in my case. When MPU lies on the side, gravity is max on Y axis ...Could this become a problem?
This ould assume that with the new orientation the Y is now the new Z access. if we are wanting the bottom to always have the reference direction towards gravity. to adjust the math you will want to consult a Quaternion expert :)
There is a lot of documentation on quaternion math out there. I would assume that the specific equations we have created are based on normalized conditions such as the MPU setting flat and Gravity being present all the time. using eDMP (Embedded Digital Motion Processing) Quaternion values are then calculated by checking the acceleration/rotation rates over time. This accumulated Quaternion value is presented in the FIFO buffer for our use. it should be able to be converted into any angular measurement.
Z
phhoef
commented
4 years ago @ZHomeSlice thanks for your answer. Sorry, I have to ask again, as I haven't understood it yet. You say, that after calibration, it's still necessary that the gravity is in Z direction? I would have expected, that it does not have any impact in which orientation the device is, as long as the angle between surface and Y axis is the same. Maybe I just misunderstood your answer ... In my case it's sort of a mobile device and of course at calibration time, I can assure the direction, but at use-time I cannot. I save the calibration offsets and set it when the Arduino has been started.
What I still do not understand is, why the new and old dmpGetYawPitchRoll returns different angles. Though, the old method also has a bigger angle, when the device is put on one side.
ZHomeSlice
commented
4 years ago Calibration uses gravity only to remove it. If we were in a zero g environment accelerometer calibration would work perfectly without removing gravity.
the calibration offset values that are generated have no reference to gravity. Calibration complete, all the rates in a 0G situation with no motion would be zero Note for the math Note that Yaw has no reference to gravity. without a reference point yaw will drift. Pitch and Roll can be calculated using angles and gravity.
you may consider trying to use the Euler equations they might work better for your situation
Z
Paulie92
commented
4 years ago I think the calibration of acelerometer have no effect there. As you once set fix offset to ensure, that all 3 axis returning approx. 16384 when oriented vertically with this code, and the rest 2 are approx. zero.
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Serial.print(ax); Serial.print("\t");
Serial.print(ay); Serial.print("\t");
Serial.print(az); Serial.print("\t");In this case, when I have the MPU oriented horizontally (Z axis vertically) the quarterion function giving values w: 1, x:0, y: 0, z:0,. When I roll the MPU around X (horizontal) axis only the y value is changing and the rest are still. When I set the MPU vertical so the X is now vertical and rolling the MPU in the same direction as before, now around Z axis, which became now horizontal as X was before, both of x and z values are changing.
I want to have the same quarterion values with vertically orientation of MPU as with the horizontally and I am affraid it is not possible, because this values comes directly from the FIFO buffer without any math operations, so I assume that the DMP have some setting telling it which axis is the default vertical (where should be the gravity measured) and it is probably fix to Z.
Regarding this discussion: https://www.i2cdevlib.com/forums/topic/397-change-default-axes-orientation/ it could be done, but it doesn't works for me and I think it can not work, as the quarterion values are acting weird when it is vertically and the dmpGetYawPitchRoll is based on this values.
intensite
commented
4 years ago @ZHomeSlice thanks for your answer.
I am seriously considering relying almost exclusively on the gyros and your comments on the effect of the rapid acceleration reinforce this impression (although I was under the impression the IMU could support 16G).
First I do need the 3 axis P/Y/R (see image) and also, since the flight time will only be a few seconds anyway, I could probably integrate the rate of rotations and filter the drifts in order to get the orientation in degrees. Not as easy as using the DMP however :-(
Quick question: I noticed on your Github profile that you are working on a alternate library "Simple_MPU6050", is it more advanced that the one included in i2cdevlib ? If so is it a drop-in replacement?
This is the flight computer mounted on the test rig. (The yellow arrow is the MPU-6050)
Thanks anyway to take the time to answer our questions.
phhoef
commented
4 years ago @Paulie92 You're saying, that the problem is rooted in the firmware of the MPU, right? But, what I still do not understand, that the angle is at least ok (not too accurate) when on side, when using the old method. My problem is, that I cannot predict which axis is in direction of the gravity. At calibration time, it's fine, but at runtime it can be changed when the user holding the device in another position ...
ZHomeSlice
commented
4 years ago First of all. Calibration does not use the DMP firmware. Calibrating is only attempting to zero or all sensors. This includes the accelerometer detecting gravity.
Now for the digital motion processing. 200 samples a second are read from the 6 sensors Quaternion values are generated a noise filter is applied. 100 readings are available from the DMP. The Quaternion values should be accurate. Note that the Quaternion values are generated from an initial starting position rates over time are used to get actual positions. Gravity is also used as it is a constant eliminating drift.
Now we come to the math to get us yaw pitch and roll
https://en.m.wikipedia.org/wiki/Quaternions_and_spatial_rotation
I believe the conversion to YPR is where the math is failing you.
I'm not sure but you might find Euler angles more suited for your rocket. Using Euler Start the rocket laying flat pointing north then rotate the rocket to its launch position. Imagine the rocket having wings with the MPU as the pilot. The pitch and yaw guide the rocket to its destination while roll is the spin of your rocket. You may be able to start your rocket vertically. I'm not an expert on any of this math and I haven't used Euler for any of my projects to date.
Z
Paulie92
commented
4 years ago I found what I assumed that DMP have to know to work properly, when the MPU is not in the default orientation (Z is the vertically axis). On this link: https://www.dropbox.com/s/91yioqg11bhgyvk/DMP.zip you can find the official InvenSense MotionDriver datasheet, where all the functions of DMP are described. As I assumed, the first thing after the DMP image is loaded is the settings of the MPU orientation on the board or device with dmp_set_orientation() function.
You can also find the driver code on the Dropbox so the "only" thing is to implement it into the library, what I unfortunatelly as an amateur don't know how to realize now.
I also find that the MPU9250 library from SparkFun uses exactly the original InvenSense code and also the dmp_set_orientation() is imlemented there, but the example code from this library doesn't work for me - giving weird numbers all the time with MPU9250 even MPU6050.
So does anyone have an idea how to implement this dmp_set_orientation() to jrowberg library?
ZHomeSlice
commented
4 years ago Let's see if we can get close Note that the firmware is loaded into the MPU and just prior to this Also, note that the firmware in the example has been pre-configured for the flat orientation
Here is how orientation is defigned:
/* The sensors can be mounted onto the board in any orientation. The mounting
* matrix seen below tells the MPL how to rotate the raw data from the
* driver(s).
* TODO: The following matrices refer to the configuration on internal test
* boards at Invensense. If needed, please modify the matrices to match the
* chip-to-body matrix for your particular set up.
*/
static struct platform_data_s gyro_pdata = {
.orientation = { 1, 0, 0,
0, 1, 0,
0, 0, 1}
};This line is where it all starts:
dmp_set_orientation( inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
// these are the memory locations for the V6.12 that we need got this from dmpKey.h
#define KEY_CFG_25 (0)
#define KEY_CFG_24 (KEY_CFG_25 + 1)
#define KEY_CFG_26 (KEY_CFG_24 + 1)
#define KEY_CFG_27 (KEY_CFG_26 + 1)
#define KEY_CFG_21 (KEY_CFG_27 + 1)
#define KEY_CFG_20 (KEY_CFG_21 + 1)
#define KEY_CFG_TAP4 (KEY_CFG_20 + 1)
#define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1)
#define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1)
#define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1)
#define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1)
#define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1)
#define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1)
#define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1)
#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1)
#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1)
#define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1)
#define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1)
#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1)
#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1)
#define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1)
#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1)
#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1)
#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1)
#define KEY_FCFG_1 (KEY_CFG_23 + 1)
#define KEY_FCFG_3 (KEY_FCFG_1 + 1)
#define KEY_FCFG_2 (KEY_FCFG_3 + 1)
#define KEY_CFG_3D (KEY_FCFG_2 + 1)
#define KEY_CFG_3B (KEY_CFG_3D + 1)
#define KEY_CFG_3C (KEY_CFG_3B + 1)
#define KEY_FCFG_5 (KEY_CFG_3C + 1)
#define KEY_FCFG_4 (KEY_FCFG_5 + 1)
#define KEY_FCFG_7 (KEY_FCFG_4 + 1)
/**
* @brief Push gyro and accel orientation to the DMP.
* The orientation is represented here as the output of
* @e inv_orientation_matrix_to_scalar.
* @param[in] orient Gyro and accel orientation in body frame.
* @return 0 if successful.
*/
int dmp_set_orientation(unsigned short orient)
{
unsigned char gyro_regs[3], accel_regs[3];
#define DINA4C 0x4c
#define DINACD 0xcd
#define DINA6C 0x6c
#define DINA0C 0x0c
#define DINAC9 0xc9
#define DINA2C 0x2c
#define DINA36 0x36
#define DINA56 0x56
#define DINA76 0x76
#define DINA26 0x26
#define DINA46 0x46
#define DINA66 0x66
const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C};
const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C};
const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76};
const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66};
gyro_regs[0] = gyro_axes[orient & 3];
gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
accel_regs[0] = accel_axes[orient & 3];
accel_regs[1] = accel_axes[(orient >> 3) & 3];
accel_regs[2] = accel_axes[(orient >> 6) & 3];
/* Chip-to-body, axes only. */
if (mpu_write_mem(FCFG_1, 3, gyro_regs))
return -1;
if (mpu_write_mem(FCFG_2, 3, accel_regs))
return -1;
memcpy(gyro_regs, gyro_sign, 3);
memcpy(accel_regs, accel_sign, 3);
if (orient & 4) {
gyro_regs[0] |= 1;
accel_regs[0] |= 1;
}
if (orient & 0x20) {
gyro_regs[1] |= 1;
accel_regs[1] |= 1;
}
if (orient & 0x100) {
gyro_regs[2] |= 1;
accel_regs[2] |= 1;
}
/* Chip-to-body, sign only. */
if (mpu_write_mem(FCFG_3, 3, gyro_regs))
return -1;
if (mpu_write_mem(FCFG_7, 3, accel_regs))
return -1;
dmp.orient = orient;
return 0;
}
/**
* @brief Write to the DMP memory.
* This function prevents I2C writes past the bank boundaries. The DMP memory
* is only accessible when the chip is awake.
* @param[in] mem_addr Memory location (bank << 8 | start address)
* @param[in] length Number of bytes to write.
* @param[in] data Bytes to write to memory.
* @return 0 if successful.
*/
int mpu_write_mem(unsigned short mem_addr, unsigned short length,
unsigned char *data)
{
unsigned char tmp[2];
if (!data)
return -1;
if (!st.chip_cfg.sensors)
return -1;
tmp[0] = (unsigned char)(mem_addr >> 8);
tmp[1] = (unsigned char)(mem_addr & 0xFF);
/* Check bank boundaries. */
if (tmp[1] + length > st.hw->bank_size)
return -1;
// we will need to change the next 4 lines to follow the MPU6050 code structure
if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
return -1;
if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))
return -1;
return 0;
}we have a function in the MPU6050 library that does this next part.
/**
* @brief Read from the DMP memory.
* This function prevents I2C reads past the bank boundaries. The DMP memory
* is only accessible when the chip is awake.
* @param[in] mem_addr Memory location (bank << 8 | start address)
* @param[in] length Number of bytes to read.
* @param[out] data Bytes read from memory.
* @return 0 if successful.
*/
int mpu_read_mem(unsigned short mem_addr, unsigned short length,
unsigned char *data)
{
unsigned char tmp[2];
if (!data)
return -1;
if (!st.chip_cfg.sensors)
return -1;
tmp[0] = (unsigned char)(mem_addr >> 8);
tmp[1] = (unsigned char)(mem_addr & 0xFF);
/* Check bank boundaries. */
if (tmp[1] + length > st.hw->bank_size)
return -1;
// we will need to change the next 4 lines to follow the MPU6050 code structure
if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
return -1;
if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
return -1;
return 0;
}
in the MPU6050_6Axis_MotionApps_V6_12.h @ 368 we need to insert the code to modify the Firmware.
// this is the most basic initialization I can create. with the intent that we access the register bytes as few times as needed to get the job done.
// for detailed descriptins of all registers and there purpose google "MPU-6000/MPU-6050 Register Map and Descriptions"
uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely
uint8_t val;
uint16_t ival;
// Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41
I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1)); //PWR_MGMT_1: reset with 100ms delay
delay(100);
I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111)); // full SIGNAL_PATH_RESET: with another 100ms delay
delay(100);
I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro
I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00)); // 0000 0000 INT_ENABLE: no Interrupt
I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00)); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead
I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g
I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read
I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro
I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat
if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail
// Here is where we need to modify the firmware we just loaded into memory
I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address
I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18)); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec
I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0)); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo
I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02)); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on
I2Cdev::writeBit(devAddr,0x6A, 2, 1); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte)
setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library
/*
dmpPacketSize += 16;//DMP_FEATURE_6X_LP_QUAT
dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_ACCEL
dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_GYRO
*/
dmpPacketSize = 28;
return 0;
}I hope someone can help you work through this. I haven't the time at the moment to go further. All the information should be gathered to modify the firmware with the above snips I gathered for you.
Z
phhoef
commented
4 years ago I did some tests, and the best angle calculation I was able to find, is the old dmpGetYawPitchRoll method for roll angle.
Based on @Paulie92 last post, it seems that the DMP itself is the problem as the orientation is fixed, right?
Unfortunately, I cannot predict the orientation of the device. It's sort of a handheld device and one could hold it "flat" or on side...
Only possibility, I currently see, is to not use the DMP.
But I think, that's not really an option, as the calculation of the angle is quite heavy load for the Arduino (Due) and needs fixed time intervals (if I understood it correctly) and I cannot guarantee, that dt is always equals, as other things are also running (like drawing the display).
ZHomeSlice
commented
4 years ago Per Application Note Programming Sequence for DMP Hardware Functions: <<< google for document 8 Mounting Matrix Configuration page 37 Mounting Matrix Configuration Definitions: Body Frame = orientation of the end OEM device Chip Frame = orientation of the gyro and accelerometer chips Mounting Matrix = a 3x3 permutation matrix that is applied to a 3x1 sample of (x,y,z) sensor data to convert its orientation from Chip Frame to Body Frame A proper mounting matrix is required to align the chip’s output data orientation with that of the end OEM device.
I believe this is used for Android Orient. But it could be used for other DMP calculations. My last post contains the example code that implements this documents instruction.
Z
ZHomeSlice
commented
4 years ago Also, I messed around with some quaternion math.
void calculateYPR(float *data, Quaternion *q){
float qw = (q -> w);
float qx = (q -> x);
float qy = (q -> y);
float qz = (q -> z);
float qw2 = qw*qw;
float qx2 = qx*qx;
float qy2 = qy*qy;
float qz2 = qz*qz;
float z = atan2(2 *(qx*qy + qw*qz), qw2 + qx2 - qy2 - qz2);
float y = asin(-2 *(qx*qz - qw*qy));
float x = atan2(2 *(qy*qz + qw*qx), qw2 - qx2 - qy2 + qz2);
data[0] = x;
data[1] = y;
data[2] = z;
}
void calculateEuler(float *data, Quaternion *q){
float qw = (q -> w);
float qx = (q -> x);
float qy = (q -> y);
float qz = (q -> z);
float qw2 = qw*qw;
float qx2 = qx*qx;
float qy2 = qy*qy;
float qz2 = qz*qz;
float test= qx*qy + qz*qw;/*
if (test > 0.499) {
data[1] = 360/M_PI*atan2(qx,qw);
data[2] = 90;
data[0] = 0;
return;
}
if (test < -0.499) {
data[1] = -360/M_PI*atan2(qx,qw);
data[2] = -90;
data[0] = 0;
return;
}
*/
float h = atan2(2*qy*qw-2*qx*qz,1-2*qy2-2*qz2);
float a = asin(2*qx*qy+2*qz*qw);
float b = atan2(2*qx*qw-2*qy*qz,1-2*qx2-2*qz2);
data[1] = h;
data[2] = a;
data[0] = b;
}and to use them:
// display YPR angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
// mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.print(q.z);
/*
calculateEuler(euler, &q);
Serial.print("\t\teuler\t");
Serial.print(euler[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180 / M_PI);
Serial.print("\t");
Serial.print(euler[2] * 180 / M_PI);
*/
calculateYPR(ypr, &q);
Serial.print("\t\typr\t");
Serial.print(ypr[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180 / M_PI);
Serial.print("\t");
Serial.print(ypr[2] * 180 / M_PI);
/*
mpu.dmpGetAccel(&aa, fifoBuffer);
Serial.print("\tRaw Accl XYZ\t");
Serial.print(aa.x);
Serial.print("\t");
Serial.print(aa.y);
Serial.print("\t");
Serial.print(aa.z);
mpu.dmpGetGyro(&gy, fifoBuffer);
Serial.print("\tRaw Gyro XYZ\t");
Serial.print(gy.x);
Serial.print("\t");
Serial.print(gy.y);
Serial.print("\t");
Serial.print(gy.z);
*/
Serial.println();
Z
phhoef
commented
4 years ago Thank you very much for your help, Z.
I gave it a try, and unfortunately for me the result is pretty the same as using the dmpGetYawPitchRoll method.
It seems, that the pitch is influencing roll and vice versa. Is that possible?
Thinking about the teapot example, I would have expected, that I can determine the roll angle reliable.
Is it possible, that the quaternion itself is wrong? Do I really have to change the orientation matrix, if I change the orientation of the device? How does Android/iOS solve this issue?
Is another IC used in smartphones?
intensite
commented
4 years ago Have you tried re-calibrating. I sometime have this phenomenon after re-calibration it seems to go away. No scientific reasons just trial and error :-) Also, is it possible calibration is affected by ambient temperature?
phhoef
commented
4 years ago Yes :-)
I just did it once again, to be absolutely sure.
After calibration:
Flat on leveling device: -0.001839298522100
On Side on leveling device: -1.584688544273376
Both values are reproducible.
Then, I calibrated the device, when on side:
Flat on leveling device: 0.470321476459503
On side on leveling device: -0.005335298366845
The roll angle hasn't changed in both tests. I just turned the device to the side.
WTF?
I calibrated again, when the device is flat on leveling instrument.
And now, I rotate it (yaw) and actually the angle is changing?!
After calibration: 0.009036025032401
After rotating about 90°: 3.140199184417725
Is that even possible? That is destroying my understanding completely.
I think that was not happening with dmpGetYawPitchRoll method.
I am not sure, if the ambient temperature has such big influence on the angle. It's lying in front on me on my desk in the office. It's about 20°C ... The device is connected through USB and the battery is charging. This will cause some heat, but I do not expect, that this causes the problem, right? Edit: The problem also exists, when the device is not charging. It's just easier for me when connected to the computer as I can trigger angle measurement through a serial command
Paulie92
commented
4 years ago I tried to implement the dmp_set_orientation to the MPU6050_6Axis_MotionApps_V6_12.h and put the siplified code int the dmpInitialize():
uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely
uint8_t val;
uint16_t ival;
// definitions
#define DINA4C 0x4c
#define DINACD 0xcd
#define DINA6C 0x6c
#define DINA0C 0x0c
#define DINAC9 0xc9
#define DINA2C 0x2c
#define DINA36 0x36
#define DINA56 0x56
#define DINA76 0x76
#define DINA26 0x26
#define DINA46 0x46
#define DINA66 0x66
#define KEY_CFG_25 (0)
#define KEY_CFG_24 (KEY_CFG_25 + 1)
#define KEY_CFG_26 (KEY_CFG_24 + 1)
#define KEY_CFG_27 (KEY_CFG_26 + 1)
#define KEY_CFG_21 (KEY_CFG_27 + 1)
#define KEY_CFG_20 (KEY_CFG_21 + 1)
#define KEY_CFG_TAP4 (KEY_CFG_20 + 1)
#define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1)
#define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1)
#define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1)
#define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1)
#define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1)
#define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1)
#define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1)
#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1)
#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1)
#define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1)
#define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1)
#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1)
#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1)
#define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1)
#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1)
#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1)
#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1)
#define KEY_FCFG_1 (KEY_CFG_23 + 1)
#define KEY_FCFG_3 (KEY_FCFG_1 + 1)
#define KEY_FCFG_2 (KEY_FCFG_3 + 1)
#define KEY_CFG_3D (KEY_FCFG_2 + 1)
#define KEY_CFG_3B (KEY_CFG_3D + 1)
#define KEY_CFG_3C (KEY_CFG_3B + 1)
#define KEY_FCFG_5 (KEY_CFG_3C + 1)
#define KEY_FCFG_4 (KEY_FCFG_5 + 1)
#define KEY_FCFG_7 (KEY_FCFG_4 + 1)
#define KEY_FCFG_FSCALE (KEY_FCFG_7 + 1)
#define KEY_FCFG_AZ (KEY_FCFG_FSCALE + 1)
#define KEY_FCFG_6 (KEY_FCFG_AZ + 1)
#define KEY_FCFG_LSB4 (KEY_FCFG_6 + 1)
#define KEY_CFG_12 (KEY_FCFG_LSB4 + 1)
#define KEY_CFG_14 (KEY_CFG_12 + 1)
#define KEY_CFG_15 (KEY_CFG_14 + 1)
#define KEY_CFG_16 (KEY_CFG_15 + 1)
#define KEY_CFG_18 (KEY_CFG_16 + 1)
#define KEY_CFG_6 (KEY_CFG_18 + 1)
#define KEY_CFG_7 (KEY_CFG_6 + 1)
#define KEY_CFG_4 (KEY_CFG_7 + 1)
#define KEY_CFG_5 (KEY_CFG_4 + 1)
#define KEY_CFG_2 (KEY_CFG_5 + 1)
#define KEY_CFG_3 (KEY_CFG_2 + 1)
#define KEY_CFG_1 (KEY_CFG_3 + 1)
#define KEY_CFG_EXTERNAL (KEY_CFG_1 + 1)
#define KEY_CFG_8 (KEY_CFG_EXTERNAL + 1)
#define KEY_CFG_9 (KEY_CFG_8 + 1)
#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1)
#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1)
#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1)
#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1)
#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1)
#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1)
#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1)
#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1)
#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1)
#define KEY_CFG_LP_QUAT (KEY_FCFG_MAG_MOV + 1)
#define FCFG_2 (1066)
#define FCFG_1 (1062)
#define FCFG_3 (1088)
#define FCFG_7 (1073)
unsigned char gyro_regs[3], accel_regs[3];
const unsigned char gyro_axes[3] = { DINA4C, DINACD, DINA6C };
const unsigned char accel_axes[3] = { DINA0C, DINAC9, DINA2C };
const unsigned char gyro_sign[3] = { DINA36, DINA56, DINA76 };
const unsigned char accel_sign[3] = { DINA26, DINA46, DINA66 };
// Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41
I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1)); //PWR_MGMT_1: reset with 100ms delay
delay(100);
I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111)); // full SIGNAL_PATH_RESET: with another 100ms delay
delay(100);
I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro
I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00)); // 0000 0000 INT_ENABLE: no Interrupt
I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00)); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead
I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g
I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read
I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro
I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat
if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail
// Beginning of the code modification
gyro_regs[0] = gyro_axes[1];
gyro_regs[1] = gyro_axes[2];
gyro_regs[2] = gyro_axes[0];
accel_regs[0] = accel_axes[1];
accel_regs[1] = accel_axes[2];
accel_regs[2] = accel_axes[0];
I2Cdev::writeBytes(devAddr, KEY_FCFG_1, 3, (gyro_regs));
I2Cdev::writeBytes(devAddr, KEY_FCFG_2, 3, (accel_regs));
//////////////////////////////////////////////////////////////////////////////////////////////
gyro_regs[0] = gyro_sign[1]; // direct write insstead od using memcpy function
gyro_regs[1] = gyro_sign[2];
gyro_regs[2] = gyro_sign[0];
accel_regs[0] = accel_sign[1];
accel_regs[1] = accel_sign[2];
accel_regs[2] = accel_sign[0];
unsigned short orient = 10; // refer to ZYX orientation which is 000_001_010
if (orient & 4) {
gyro_regs[0] |= 1;
accel_regs[0] |= 1;
}
if (orient & 0x20) {
gyro_regs[1] |= 1;
accel_regs[1] |= 1;
}
if (orient & 0x100) {
gyro_regs[2] |= 1;
accel_regs[2] |= 1;
}
I2Cdev::writeBytes(devAddr, KEY_FCFG_3, 3, (gyro_regs));
I2Cdev::writeBytes(devAddr, KEY_FCFG_7, 3, (accel_regs));
// End of code modification
I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address
I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18)); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec
I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0)); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo
I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02)); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on
I2Cdev::writeBit(devAddr,0x6A, 2, 1); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte)
setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library
/*
dmpPacketSize += 16;//DMP_FEATURE_6X_LP_QUAT
dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_ACCEL
dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_GYRO
*/
dmpPacketSize = 28;
return 0;
}I didnt't implement the orientation matrix contiguration, only directly push the data into the variable according to my MPU chip orientation. Unfortunately it doesn't work. When I used the KEY_FCFG definition, the rate of serial print decrease to about 2 rows per second on the serial monitor and also the values changes quite slower - when I tilt the MPU for about 45 degrees it took approx. 60sec for the pitch to achieve this value. But no change in the orientation!
When I used the defininition "without key" as in the original example:
`
`
there were absolutly no changes from the original setup. I also tried use of different order of the axis and it has also no effect. As I mentioned I am an amateur, so I am not familiar with the I2C write functions. I am little bit confused with the definitions above where the memory adress is 2 bytes number, what probably cannot be used, because the real adresses where the data were written has different adresses:
//I2C(0x68) writing 3 bytes to 0x1A...C9 2C C.Done.
//I2C(0x68) writing 3 bytes to 0x19...56 76 36. Done.
//I2C(0x68) writing 3 bytes to 0x20...46 66 26. Done.For example the 0x18 (24 in DEC) adress should be FCFG_2 what is 1066.
So currently no positive results on my side.
By the way, I don't assume that the dmp_set_orientation function is used only for the Android orientation as was mentioned before. I tried this with the SparkFun MPU9250 library and this function also affects the Yaw Pitch and Roll, not only Android orientation.
intensite
commented
4 years ago Wow impressive effort @Paulie92 !!
A side question here: You based your work on the MPU6050_6Axis_MotionApps_V6_12.h version of the library where my code uses the version from MPU6050_6Axis_MotionApps20.h
I was under the impression that MPU6050_6Axis_MotionApps20 was more recent/evolved than the others. Do you know what are the differences?
ElreboCM
commented
2 years ago @Paulie92: as far as I can tell you are not trying to set orientation as described in the motion processor user guide on p9f. Are you?
Unfortunately i was not able to locate Orientation Matrix Transformation chart.pdf which should give an indication on how to set the orientation struct properly.
ElreboCM
commented
2 years ago I located now the orientation transformation chart matrix as a part of this package. To me it looks like this only for configuring the position on the pcb. It does not take vertical mounting into consideration.
Depending on the position on the PCB this could be the correct orientation matrix for a vertical position: Y becoming Z and Z becoming Y
{1,0,0 0,0,1 0,1,0}
Captain-ZR
commented
2 years ago @ZHomeSlice Hello, I have tried the method you mentioned for calibration, which can achieve the calibration in the vertical state. But there is another problem, I want to achieve the effect of this vertical state as the initial state, for example, the current quaternion value is 1,0,0,0, but it is not the actual situation, it is calibrated to the horizontal state as the initial state, and the vertical quaternion value is about 0.71,0,0.71,0, not 1,0,0,0. According to what I read, the solution seems to be to modify the coordinate matrix {1, 0, 0, 0, 0, 1, 0, 0, 1}, But I can't find a similar matrix to modify in I2Cdevlib.
ZHomeSlice
commented
2 years ago @Captain-ZR Are you talking about the calibration routine or the orientation matrix?
Captain-ZR
commented
2 years ago @Captain-ZR Are you talking about the calibration routine or the orientation matrix?
@ZHomeSlice I think it's the orientation matrix, I've calibrated it in the vertical state, but it's still calibrated in the horizontal state, and I want to initialize it in the vertical state with a quaternion output of 1,0,0,0. Look at the official code to modify the direction matrix to achieve this effect, but in the I2Cdevlib library I do not seem to find the direction matrix can be set
jaggzh
commented
1 year ago I'm putting mine in a device that's resting under a patient's hand, on a pillow. In my case, Y is mostly the up-down axis (but not exactly).
I'm wondering if it's possible we can calibrate in a way that the acceleration x,y,z vectors sum up to the proper gravitational acceleration?
jaggzh
commented
1 year ago For those interested, the button fits in the left-hand's palm, and looks like this:
The MPU6050 is mounted inside the button itself (ignore the red 3d printing supports):
And here's a closeup where you can see the orientation and axes indicators I put in:
Captain-ZR
commented
1 year ago In the file MPU6050.cpp, about line 3300, there is a MPU6050::PID function,
if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity
this The sentence code can be adapted to the calibration of different installation directions, where i==2 represents calibration on the z-axis, and 0 and 1 represent the x-axis and y-axis. The -= or += in Reading -=16384 means that the direction of gravity is related to the positive direction of the current axis.
ZHomeSlice
commented
1 year ago The embedded DMP is challenging to manipulate so changing the orientation for Z axis with 6 Degrees of freedom is hard. I haven't succeeded to do this as of yet. I did however change the quaternions only to use Gyro ( 3 Axis Low Power Quaternions)
This is part of my Simple_MPU6050.cpp code but it could be easily modified to work with Jeffs MPU6050 code
Here is what needs to be changed:
//here is a copy of the #defines if they give you any trouble
#define CFG_8 (2718)
#define CFG_LP_QUAT (2712)
// Load the eDMP Firmware then make these modifications to the Firmware you just loaded before doing anything else.
// Change eDMP to 3 Axis Low Power Quaternions
uint8_t regs1[4] = {0xA3,0xA3,0xA3,0xA3};
write_mem(CFG_8, 4, regs1); // CFG_8 is a special location in the eDMP firmware and is defined elsewhere for us
uint8_t regs2[4] = {0xC0,0xC2,0xC4,0xC6};
write_mem(CFG_LP_QUAT, 4, regs2); // CFG_LP_QUATis a special location in the eDMP firmware and is defined elsewhere for us
Serial.println(F("3 Axis Low Power Quaternions"));Now acceleration is not part of Quaternions. the gyros will drift.
You can still calculate angles by accessing an instantaneous sample of the accelerometer readings and by using atan2
// Calculate the pitch angle using the atan2 function
double pitch = atan2(ay, az);
// Calculate the roll angle using the atan2 function
double roll = atan2(-ax, sqrt(ay*ay + az*az));
// This outputs the pitch and roll angles in radians
note that Z is now been replaced by your X and vice versa with your orientation.
As for Calibration Which grabs the instantaneous values directly for the gyroscope and Accelerometer, you will want to modify this code specifically (i==2) where X = 0, Y = 1, and Z = 2
if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity on Z
to
if ((ReadAddress == 0x3B)&&(i == 0)) Reading -= 16384; //remove Gravity on X
to properly subtract gravity so that the PID loop can zero out any acceleration from the effects of gravity.
Z
jaggzh
commented
1 year ago Thanks a lot. :) @ZHomeSlice I forked and cloned your lib. I'm looking through it to try to understand it a bit (before going through the changes in my code to try it out. It doesn't seem listed in Arduino's libs, by the way -- maybe you want to add it?)
// Portion of example tuning loop pseudocode.
cali_loop() {
ax ay az = 15000, 50, 200 <== Get Axyz readings
total = 15000+50+200 = 17050 // (Uncalibrated doesn't add up. Deal with in a bit.)
xfrac = 15000/17050 = 0.88
yfrac = 50/17050 = .003
zfrac = 200/17050 = .012
scale_all = 16384 / (float)(total) => 0.96094
/* To-do: See if we can use some divisions trick to avoid floats
To-do: We may not want to scale values so total<16384, but it might be okay.
Scaling them down means they will have a tendency to need to creep upwards
towards their final value. */
if (ReadAddress == 0x3B)) {
if (i == 0)) Reading -= (xfrac*scale_all * 16384); // remove Gravity on X
else if (i == 1)) Reading -= (yfrac*scale_all * 16384); // ...on Y
else if (i == 2)) Reading -= (yfrac*scale_all * 16384); // ...on Z
}
}You may want to calibrate the accelerometers once and then assume that minor deviations are not that big of an issue. then only calibrate the gyro. The orientation of the gyro is not an issue. The only requirement is that it remains still. So by doing the initial calibration with Z facing down. this way you won't have to change anything with the calibration routine.
concept code to try:
int16_t * ActiveOffsets;
if (Z axis is down and the unit is still) (
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
ActiveOffsets = mpu.GetActiveOffsets();
// ActiveOffsets 0,1,2 are Accelerometer offsets
// ActiveOffsets 3,4,5 are Gyro offsets
// Store this MPU6050's offsets in EPROM
)
// Load all the offsets from EPROM
// Set the offsets
mpu.setXAccelOffset(ActiveOffsets[0]);
mpu.setYAccelOffset(ActiveOffsets[1]);
mpu.setZAccelOffset(ActiveOffsets[2]);
mpu.setXGyroOffset(ActiveOffsets[3]);
mpu.setYGyroOffset(ActiveOffsets[4]);
mpu.setZGyroOffset(ActiveOffsets[5]);
if (gyro is still) {
mpu.CalibrateGyro(6); // fine tune and remove drift from the gyro only
}This is the function that gets the active offsets from the MPU6050
int16_t * MPU6050_Base::GetActiveOffsets() {
uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77;
if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)offsets, I2Cdev::readTimeout, wireObj);
else {
I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)offsets, I2Cdev::readTimeout, wireObj);
I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)(offsets+1), I2Cdev::readTimeout, wireObj);
I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)(offsets+2), I2Cdev::readTimeout, wireObj);
}
I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)(offsets+3), I2Cdev::readTimeout, wireObj);
return offsets;
}The accelerometer is less likely to need detailed calibration so just getting it close once in a while should be enough.
You can use direct measurements from the accelerometer for Pitch and Roll calculations because gravity is your reference and you just calculate the angles at that moment of time. You will need to use the gyro only for yaw. using the eDMP with quaternions set to 3 degrees of freedom will likely be the easiest to keep track of the orientation as you need to calculate rotation over time and that takes a lot of samples which the eDMP does automatically.
note: eDMP stands for Embedded Digital Motion Processing and can only achieve 6 Degrees of freedom there is an additional DMP library that adds in the magnetometer (MPU9250) but I have never been able to get it to work on the Arduino platform.
Z
PS. Adding Simple_MPU6050 library to the Arduino's Libs....? I haven't had a lot of time lately to focus on this hobby as of late. I would like to add my Simple_MPU6050 library to the Arduino libs. Is there a tutorial on this? This should probably continue on my Github page
I am not really aware of all the maths behind the calculation of the angle. I mounted the MPU6050 in a housing. I want to calculate the angle between surface and y axis.
I am running the calibration by executing the following functions
as shown in the IMPU_Zero.ino example. I am not sure, if I need all the other stuff of this example? As far as I understood it, the additional lines are just to proof it's working, right?
Now, after calibration finished, the angle is quite good when the housing lies flat on a leveling instrument (rad 0.000688443135004).
But if I flip the housing to the side and calculate the angle again, I get obviously a wrong angle (rad -2.713898897171021).
According to my understanding, the angle should be the same as the y axis hasn't changed. Does anybody can imagine what's wrong?