Hello, I am using the MPU-6050 gyroscope and accelermeter for a control system project. Using the example and library from Jeff Rowberg and I2Cdev, I have used the following code to get the angles from the MPU.
Everything works fine and seems accurate, but there is one thing I would like to change. The Yaw angle will reset after it passes 180 or -180 degrees.
In summary, it goes from 0 => 179 => -179 => 0 => 179... and so on.
I would like the Yaw to be compounded so it goes from 0 => 179 => 359 => 539 => 719.
I can't seem to find a way with the current library to change this (though I could have missed it) and when I try to do this with if statements I end up having 5 of them and still can't get it to work properly. Any help with this would be greatly appreciated!
MPU6050_DMP6.ino (15.7 KB)
359 => 539 => 719
It makes no sense at all to have yaw outside the range of 0-360.
Yaw is defined as the angle from due North.
If you want to compound body rotations, read and integrate the gyro values directly, or sum the changes in yaw angle as an independent variable.
jremington:
It makes no sense at all to have yaw outside the range of 0-360.
Yaw is defined as the angle from due North.
I know that's the conventional way but when you have a PID control system and the PID controller sees that a system went from 180 degrees to -180 degrees then the system freaks out bad. To fix this I need a second yaw value just for the PID controller.
I am a very novice coder and when I have tried to sum the yaw to a new yawCompouned value, I have failed. I need the help of a more experienced coder.
To fix this I need a second yaw value just for the PID controller.
No, you don't. It is convenient to have the yaw angle in the range of 0-359 (integer values) and work with that, but that is not necessary.
For the purpose of PID regulation of the heading, you use a separate normalization of the difference between the heading setpoint and the current heading (bearing), if you like using the following routines:
// routine to calculate heading error in degrees, taking into account compass wrap
int heading_error(int bearing, int current_heading)
{
int error = current_heading - bearing;
if (error > 180) error -= 360;
if (error < -180) error += 360;
return error;
}
// routine to correct for compass wrap
int wrap360(int direction) {
while (direction > 359) direction -= 360;
while (direction < 0) direction += 360;
return direction;
}
Then, the PID heading correction calculation for an autonomous motorboat might look like this:
// abbreviated PID loop
gps_last_fix = millis();
gps_latlon(&lat, &lon);
gps_parse_enable(); //re-enable parsing
gps_bearing = (int) (course_to(lat, lon, lat2, lon2, &distance)+0.5);
dist_to_go = distance + 0.5;
// To take into account wind and currents,
// substitute calculated GPS bearing for point_bearing
point_bearing = gps_bearing;
// get IMU heading
imu_heading = wrap360(IMU_yaw() + yaw_offset); //yaw offset is magnetic versus true North
// heading error and PID steering
error = heading_error(point_bearing, imu_heading);
//error is positive if current_heading > bearing (compass direction)
//positive bias acts to reduce left motor speed, so bear left
bias = (kp*error)/10; //Kp in tenths (Ki, Kd not necessary in water)
// the motor routines internally limit the argument to {-255, 255}
set_m1_speed(motorSpeed-bias); //left motor
set_m2_speed(motorSpeed+bias); //right motor
// end PID nav loop
'