I'm trying to "calibrate" my MPU9250 data so that the starting position of the MPU9250 is 0. The way I'm trying to tackle this is by creating a function, which will run in setup, that will take 20 readings over 10 seconds into an array and average the results to be stored into a variable which I can then use to subtract from the IMU data as I read it so that I have easier to manage and understand data. This may be an overcomplicated approach, please let me know if there is a simpler one that can be suggested.
I've got as far as creating a for loop which should run 20 times (each loop should last 50 milliseconds) and storing the data into two different arrays for the Yaw and Pitch results as seen below.
void flatconfig() {
int iterations;
for (int i = 0; i <= 20; i++){
int anArrayYaw[iterations];
int anArrayPitch[iterations];
byte arrayIndexYaw = i;
byte arrayIndexPitch = i;
anArrayYaw[arrayIndexYaw] = mpu.getYaw();
anArrayPitch[arrayIndexPitch] = mpu.getPitch();
delay(50);
}
//What i want is something along the lines of
/*
int totalarray;
float average;
average = (totalarray / iterations);
const int flatdif = average;
/*
}
What I'm struggling with is an easy way to average this out without writing it out longhand and adding each value and dividing it by the total.
Is there a syntax that allows me to easily get a total of the values stored in the array so I can simply do divide that by the iterations variable (as demonstrated in the bottom of the code)?
Read about variable scope. Your arrays are only known in the for loop this way.
If you want to average, you don’t need the array. Just add things up in the for loop to get a total and once you exit the for loop divide by the number of samples.
long getAvgYaw()
{
long totalYaw =0;
const byte nbIterations = 20;
for (int i=0; i < nbIterations; i++) {
totalYaw += mpu.getYaw();
delay(50);
}
return totalYaw / nbIterations;
}
(There is probably more to IMU calibration than this..)
Unfortunately, 3D orientation angles are not additive. Euler angles are successively applied and the order matters a great deal. The only exception to the additivity rule is the last angle applied, which is often but not always yaw.
You can store a starting 3D orientation as the reference, then compute the change in 3D orientation from the reference to the current orientation.
If I understand your question correctly this is what I do with my gyro stabilizer for my model aircrafts.
you can look at the copied "setup" from my sketch, where you see the calibration loop. CALIBRATION_COUNT would be 20 in your situation and the delay would be 500 (I use only 1)
Then I added the beginning of the "loop" where you see I take the roll, pitch and yaw and subtract the calibration value. My gyro works just fine.
void setup() {
delay(5); //wait for the capacitor to charge and vcc stabilization
i2c_write_reg(GYRO_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(5); // wait for the sensor to reset
i2c_write_reg(GYRO_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
i2c_write_reg(GYRO_ADDRESS, 0x1A, 0); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
i2c_write_reg(GYRO_ADDRESS, 0x1B, 0x08); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 500 deg/sec
// Clear the 'sleep' bit to start the sensor.
MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
delay(1); // wait for the sensor to startup
gyro_pitch_cal = 0;
gyro_roll_cal = 0;
gyro_yaw_cal = 0;
for (int i = 0; i < CALIBRATION_COUNT ; i++) {
gyro_read_raw(); //Read the gyro output.
gyro_pitch_cal += gyro_pitch; //Add pitch value to gyro_pitch_cal.
gyro_roll_cal += gyro_roll; //Add roll value to gyro_roll_cal.
gyro_yaw_cal += gyro_yaw; //Add yaw value to gyro_yaw_cal.
delay(1); //Wait one milliseconds before the next loop.
}
//Now that we have samples, we need to divide by the sample count to get the average gyro offset.
gyro_pitch_cal /= CALIBRATION_COUNT;
gyro_roll_cal /= CALIBRATION_COUNT;
gyro_yaw_cal /= CALIBRATION_COUNT;
//Configure servo pins as output.
ServoMotor[AILERON].attach(SERVO1_PIN);
ServoMotor[ELEVATOR].attach(SERVO2_PIN);
ServoMotor[THROTTLE].attach(SERVO3_PIN);
ServoMotor[RUDDER].attach(SERVO4_PIN);
ServoMotor[AUX1].attach(SERVO5_PIN);
ServoMotor[AUX2].attach(SERVO6_PIN);
TinyPpmReader.attach(PPM_INPUT_PIN);
}
void loop() {
float rollStabMagnitude;
float pitchStabMagnitude;
static uint16_t Width_us = NEUTRAL_US; /* Static to keep the value at the next loop */
//Let's get the current gyro data and scale it to degrees per second for the pid calculations.
gyro_read_raw();
gyro_pitch -= gyro_pitch_cal; // apply calibration
gyro_roll -= gyro_roll_cal;
gyro_yaw -= gyro_yaw_cal;
gyro_pitch *= GYRO_INVERT_PITCH / 57.14286; //Gyro pid input is deg/sec. gyro_apply_inversion_and_scale
gyro_roll *= GYRO_INVERT_ROLL / 57.14286; //Gyro pid input is deg/sec.
gyro_yaw *= GYRO_INVERT_YAW / 57.14286; //Gyro pid input is deg/sec.
I am indeed using this as part of the control system for a RC scale starship replica. I appreciate there is a lot more complexity to consider. I will not just be doing this with the yaw values but with yaw pitch and roll to accurately give me a base reading and easy to deal with numbers to work with in the PID loop and for when I am monitoring the serial data.
I've implemented the following to retrieve an average reading in the setup
int getAvgYaw() {
int iterations = 20;
int totalyaw;
for (int i = 0; i < iterations; i++){
totalyaw += mpu.getYaw();
delay(50);
}
int average = totalyaw / iterations;
return average;
}
t
he problem is that this keeps returning 0 for some reason...Anything obvious that I'm doing wrong?
J-M-L:
Read about variable scope. Your arrays are only known in the for loop this way.
If you want to average, you don’t need the array. Just add things up in the for loop to get a total and once you exit the for loop divide by the number of samples.
This was very helpful, I've taken your example and dumbed it down into a something I can comprehend a little more as you can see above.
hmeijdam:
you can look at the copied "setup" from my sketch, where you see the calibration loop. CALIBRATION_COUNT would be 20 in your situation and the delay would be 500 (I use only 1)
This is great. Exactly what I'm looking to do. There is actually a calibration function built into the library I'm using but I can't seem to figure out what it's returning and where it's implementing it as it doesn't seem to do what I want it to do....Must be some other form of calibration. Would I be correct in inferring you are not using a library but instead directly reading the data from the sensor? If so are there any resources you could point me towards to learn how to do this myself?
I am very much learning as I go along so I'd like to minimise the use of libraries so I can understand everything I'm using properly. I'd like to do that while also keeping the tasks I take on to a realistic standard so if trying to retrieve the MPU9250 data is a particularly interact or advanced task could you please advice me
If you like to minimize the use of libraries, I recommend you start from the point I leveraged the majority of my sketch from.
Look at the video if you like and then you can download this sketch from the video comments.
I actually replaced reading my PPM input and servo output by libraries as I wanted to use different pins and another microprocessor (LGT8F328), but that makes it less educational for you.
I had to add a 1000uF capacitor between VCC and GND of my board, or the processor would crash when I move the servo's. A lot of EMF going on in an RC plane.
You dumbed my example down too much, now it’s brain dead (kidding)
There were good reasons to what you changed:
I used long type instead of int to be able to fit the sum without overflowing. Read about integral types and how large they can grow.(int versus long)
I initialized the counter to 0 otherwise you’ll start with a random value and the sum will just be bad.
You should also capture the three values in one go as this is how the MPU works rather than have 3 separate functions. You could store the result in a struct and return the struct or just global variables.
Thanks for all your input. I've managed to get things working.
jremington:
You can't, as explained above. 3D orientation angles are not additive.
I'm wrapping my head around eurler's angles and think (though very naively) that I have some what of an understanding.
I'd also like to point out that I have taken out the accelerometer data and calibrated the x, y and z values. My understanding is that these values are yaw pitch and roll but
I can't see how these rules will apply to my application. The starting position of the vehicle will be stood vertically. This will be the way I wish it to stay throughout flight. For this reason I'll be setting that starting position as 0 on the X and Y accelerometer data to serve as a reference point, a point which will also be referenced in my PID loop. The setpoint of the PID loop is 0. I'm aware I could just store the average of the imu data into a variable and then use that variable as the setpoint but as you will see by my oversimplification of things above I like to dumb things down and make them digestible and easy to read for my uneducated brain to comprehend! I can see how Euler's angles work to measure where an object is in 3D space and see the significance of the sequence of movement but I can't see that being a hinderance in reading data in the way i proposed... Feel free to highlight and expose my negligence and follow the "dumbed down" trend I'm setting
J-M-L:
You dumbed my example down too much, now it’s brain dead (kidding)
Haha
Again another area which I have not given much thought to is the data types in relation to how much they can store. I've read up on the data types and get that int can hold 16bits and long 32. That means that an int can store 65535 different values right....? Surely the average of the data or the total of it won't exceed that... Unless I am missing a whole bunch of understanding in how computing or maths works...Very real possibility.
i did also set the totalyaw to 0 when i created in the version of this I plopped into my main mess of code (if you think this is brain dead you should see the accumulation of horror that is the flight controller code!). I've also read all three in the same function for the final and store them in a strut following your suggestion, thanks!
The X, Y and Z acceleration data can be used to measure pitch and roll angles (as long as no forces other than gravity act on the accelerometer), as described here, but there are several different ways to define those angles.
If forces other than gravity act on your gizmo, then you need a full 3D IMU, and you are back to the situation that the Euler angles are not additive.
If you want informed help, tell us about your project.
I use a SimpleKalmanFilter to smooth out magnetometer, accelerometr, and gyro readings. If your using a Uno the SimpleKalmanFIlter will be too processor intensive.
jremington:
If you want informed help, tell us about your project.
The project is essentially a starship replica (an rc rocket of sorts, though I plan to develop my knowledge and the project to automate the manoeuvres starship do). I am using a ducted fan as a source of thrust and have designed a gimbal in which will pivot the motor up to 20 degrees in any direction for thrust vector control. The gimbal is powered by two servos controlling the X and Y positions of the motor. The control system will be a PID loop which will use the IMU as an input and the two servos will be the actuators which the output feeds into to reach the setpoint ( waiting for a bunch of parts so I have yet to test and tune this). Yaw is actually the only axis I don't have control over, no Idea why I started with that, regardless code will work with pitch and roll.
I have yet to get as far as testing the amount of torque, if any, the ducted fan will produce and how to counteract that without adding more hardware and therefore weight. The veins in the EDF are angled opposite to the direction of rotation, I believe this is a design to reduce/remove the torque. Unfortunately my motor gave out on me while testing different nozzles (De laval and aerospike) on how they work with EDF's and which configuration will provide the best thrust so I have yet to get an idea of if this is a significant factor.
I think I see the significance of this information now. Because I am only actuating two axis I don't necessary need information on all 3 and therefore do not need to know the orientation in 3D space, but in a 2d plane. If I find I do need to have control over and data on the yaw to counter the torque as well then I would need to use a 3d IMU and Eurler's angles to determine the position in 3d space (or I'm totally wrong)
Idahowalker:
I use a SimpleKalmanFilter to smooth out magnetometer, accelerometr, and gyro readings. If your using a Uno the SimpleKalmanFIlter will be too processor intensive.
Thanks, I'll take a look into this. Using an ESP32 so got a bit more juice under the hood so should be ok.
Note the updating of the q value with KF_ADC.setProcessNoise( esp_timer_get_time() - TimePast );. Updating the q value before making a calculation improves filter accuracy.
(kidding)