Test_MPU6050 - Tests Yaw angle

The Professional Perfectionists » Code » Test_MPU6050

The Professional Perfectionists / Mbed 2 deprecated Test_MPU6050

Tests Yaw angle

main.cpp@0:98f7c7d55051, 2015-02-15 (annotated)

Committer:: joshwilkins2013
Date:: Sun Feb 15 17:40:56 2015 +0000
Revision:: 0:98f7c7d55051
Child:: 2:8b8ea2b74666

Who changed what in which revision?

User	Revision	Line number	New contents of line
joshwilkins2013	0:98f7c7d55051	1	// SDA --- A4
joshwilkins2013	0:98f7c7d55051	2	// SCL --- A5
joshwilkins2013	0:98f7c7d55051	3
joshwilkins2013	0:98f7c7d55051	4	#include "mbed.h"
joshwilkins2013	0:98f7c7d55051	5	#include "MPU6050.h"
joshwilkins2013	0:98f7c7d55051	6
joshwilkins2013	0:98f7c7d55051	7	float sum = 0;
joshwilkins2013	0:98f7c7d55051	8	uint32_t sumCount = 0;
joshwilkins2013	0:98f7c7d55051	9
joshwilkins2013	0:98f7c7d55051	10	MPU6050 mpu6050;
joshwilkins2013	0:98f7c7d55051	11	Timer t;
joshwilkins2013	0:98f7c7d55051	12	Serial pc(USBTX, USBRX); // tx, rx
joshwilkins2013	0:98f7c7d55051	13
joshwilkins2013	0:98f7c7d55051	14	int main(){
joshwilkins2013	0:98f7c7d55051	15	pc.baud(9600);
joshwilkins2013	0:98f7c7d55051	16
joshwilkins2013	0:98f7c7d55051	17	//Set up I2C
joshwilkins2013	0:98f7c7d55051	18	i2c.frequency(400000); // use fast (400 kHz) I2C
joshwilkins2013	0:98f7c7d55051	19	t.start();
joshwilkins2013	0:98f7c7d55051	20
joshwilkins2013	0:98f7c7d55051	21	// Read the WHO_AM_I register, this is a good test of communication
joshwilkins2013	0:98f7c7d55051	22	uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
joshwilkins2013	0:98f7c7d55051	23	pc.printf("I AM 0x%x\n\r", whoami);
joshwilkins2013	0:98f7c7d55051	24	pc.printf("I SHOULD BE 0x68\n\r");
joshwilkins2013	0:98f7c7d55051	25
joshwilkins2013	0:98f7c7d55051	26	if (whoami == 0x68){
joshwilkins2013	0:98f7c7d55051	27	pc.printf("MPU6050 is online...");
joshwilkins2013	0:98f7c7d55051	28	wait(1);
joshwilkins2013	0:98f7c7d55051	29
joshwilkins2013	0:98f7c7d55051	30	mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
joshwilkins2013	0:98f7c7d55051	31	pc.printf("x-axis self test: acceleration trim within : ");
joshwilkins2013	0:98f7c7d55051	32	pc.printf("%f", SelfTest[0]);
joshwilkins2013	0:98f7c7d55051	33	pc.printf("% of factory value \n\r");
joshwilkins2013	0:98f7c7d55051	34	pc.printf("y-axis self test: acceleration trim within : ");
joshwilkins2013	0:98f7c7d55051	35	pc.printf("%f", SelfTest[1]);
joshwilkins2013	0:98f7c7d55051	36	pc.printf("% of factory value \n\r");
joshwilkins2013	0:98f7c7d55051	37	pc.printf("z-axis self test: acceleration trim within : ");
joshwilkins2013	0:98f7c7d55051	38	pc.printf("%f", SelfTest[2]);
joshwilkins2013	0:98f7c7d55051	39	pc.printf("% of factory value \n\r");
joshwilkins2013	0:98f7c7d55051	40	pc.printf("x-axis self test: gyration trim within : ");
joshwilkins2013	0:98f7c7d55051	41	pc.printf("%f", SelfTest[3]);
joshwilkins2013	0:98f7c7d55051	42	pc.printf("% of factory value \n\r");
joshwilkins2013	0:98f7c7d55051	43	pc.printf("y-axis self test: gyration trim within : ");
joshwilkins2013	0:98f7c7d55051	44	pc.printf("%f", SelfTest[4]);
joshwilkins2013	0:98f7c7d55051	45	pc.printf("% of factory value \n\r");
joshwilkins2013	0:98f7c7d55051	46	pc.printf("z-axis self test: gyration trim within : ");
joshwilkins2013	0:98f7c7d55051	47	pc.printf("%f", SelfTest[5]);
joshwilkins2013	0:98f7c7d55051	48	pc.printf("% of factory value \n\r");
joshwilkins2013	0:98f7c7d55051	49	wait(1);
joshwilkins2013	0:98f7c7d55051	50
joshwilkins2013	0:98f7c7d55051	51	if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
joshwilkins2013	0:98f7c7d55051	52	mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
joshwilkins2013	0:98f7c7d55051	53	mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
joshwilkins2013	0:98f7c7d55051	54	mpu6050.initMPU6050();
joshwilkins2013	0:98f7c7d55051	55	pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
joshwilkins2013	0:98f7c7d55051	56
joshwilkins2013	0:98f7c7d55051	57	pc.printf("\nMPU6050 passed self test... Initializing");
joshwilkins2013	0:98f7c7d55051	58	wait(2);
joshwilkins2013	0:98f7c7d55051	59	} else pc.printf("\nDevice did not the pass self-test!\n\r");
joshwilkins2013	0:98f7c7d55051	60	} else {
joshwilkins2013	0:98f7c7d55051	61	pc.printf("Could not connect to MPU6050: \n\r");
joshwilkins2013	0:98f7c7d55051	62	pc.printf("%#x \n", whoami);
joshwilkins2013	0:98f7c7d55051	63	while(1) ; // Loop forever if communication doesn't happen
joshwilkins2013	0:98f7c7d55051	64	}
joshwilkins2013	0:98f7c7d55051	65
joshwilkins2013	0:98f7c7d55051	66	while(1) {
joshwilkins2013	0:98f7c7d55051	67
joshwilkins2013	0:98f7c7d55051	68	// If data ready bit set, all data registers have new data
joshwilkins2013	0:98f7c7d55051	69	if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
joshwilkins2013	0:98f7c7d55051	70	mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
joshwilkins2013	0:98f7c7d55051	71	mpu6050.getAres();
joshwilkins2013	0:98f7c7d55051	72
joshwilkins2013	0:98f7c7d55051	73	// Now we'll calculate the accleration value into actual g's
joshwilkins2013	0:98f7c7d55051	74	ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
joshwilkins2013	0:98f7c7d55051	75	ay = (float)accelCount[1]*aRes - accelBias[1];
joshwilkins2013	0:98f7c7d55051	76	az = (float)accelCount[2]*aRes - accelBias[2];
joshwilkins2013	0:98f7c7d55051	77
joshwilkins2013	0:98f7c7d55051	78	mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
joshwilkins2013	0:98f7c7d55051	79	mpu6050.getGres();
joshwilkins2013	0:98f7c7d55051	80
joshwilkins2013	0:98f7c7d55051	81	// Calculate the gyro value into actual degrees per second
joshwilkins2013	0:98f7c7d55051	82	gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
joshwilkins2013	0:98f7c7d55051	83	gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
joshwilkins2013	0:98f7c7d55051	84	gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
joshwilkins2013	0:98f7c7d55051	85
joshwilkins2013	0:98f7c7d55051	86	tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
joshwilkins2013	0:98f7c7d55051	87	temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
joshwilkins2013	0:98f7c7d55051	88	}
joshwilkins2013	0:98f7c7d55051	89
joshwilkins2013	0:98f7c7d55051	90	Now = t.read_us();
joshwilkins2013	0:98f7c7d55051	91	deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
joshwilkins2013	0:98f7c7d55051	92	lastUpdate = Now;
joshwilkins2013	0:98f7c7d55051	93
joshwilkins2013	0:98f7c7d55051	94	sum += deltat;
joshwilkins2013	0:98f7c7d55051	95	sumCount++;
joshwilkins2013	0:98f7c7d55051	96
joshwilkins2013	0:98f7c7d55051	97	if(lastUpdate - firstUpdate > 10000000.0f) {
joshwilkins2013	0:98f7c7d55051	98	beta = 0.04; // decrease filter gain after stabilized
joshwilkins2013	0:98f7c7d55051	99	zeta = 0.015; // increasey bias drift gain after stabilized
joshwilkins2013	0:98f7c7d55051	100	}
joshwilkins2013	0:98f7c7d55051	101
joshwilkins2013	0:98f7c7d55051	102	// Pass gyro rate as rad/s
joshwilkins2013	0:98f7c7d55051	103	mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gxPI/180.0f, gyPI/180.0f, gz*PI/180.0f);
joshwilkins2013	0:98f7c7d55051	104
joshwilkins2013	0:98f7c7d55051	105	// Serial print and/or display at 0.5 s rate independent of data rates
joshwilkins2013	0:98f7c7d55051	106	delt_t = t.read_ms() - count;
joshwilkins2013	0:98f7c7d55051	107	if (delt_t > 500) { // update LCD once per half-second independent of read rate
joshwilkins2013	0:98f7c7d55051	108
joshwilkins2013	0:98f7c7d55051	109	yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
joshwilkins2013	0:98f7c7d55051	110	pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
joshwilkins2013	0:98f7c7d55051	111	roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
joshwilkins2013	0:98f7c7d55051	112	pitch *= 180.0f / PI;
joshwilkins2013	0:98f7c7d55051	113	yaw *= 180.0f / PI;
joshwilkins2013	0:98f7c7d55051	114	roll *= 180.0f / PI;
joshwilkins2013	0:98f7c7d55051	115
joshwilkins2013	0:98f7c7d55051	116	pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
joshwilkins2013	0:98f7c7d55051	117	pc.printf("average rate = %f\n\r", (float) sumCount/sum);
joshwilkins2013	0:98f7c7d55051	118
joshwilkins2013	0:98f7c7d55051	119	count = t.read_ms();
joshwilkins2013	0:98f7c7d55051	120	sum = 0;
joshwilkins2013	0:98f7c7d55051	121	sumCount = 0;
joshwilkins2013	0:98f7c7d55051	122	}
joshwilkins2013	0:98f7c7d55051	123	}
joshwilkins2013	0:98f7c7d55051	124
joshwilkins2013	0:98f7c7d55051	125	}