Evan Brown / Mbed 2 deprecated APpart3_E_start

11/18

Dependencies:   mbed

sensor_fusion.cpp@1:491bd986ce22, 2018-11-10 (annotated)

Committer:
tlee6414
Date:
Sat Nov 10 00:23:07 2018 +0000
Revision:
1:491bd986ce22
Parent:
0:f43994f44684
Child:
2:a4d5e7f96e87
yeet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tlee6414 1:491bd986ce22 1 /*
tlee6414 1:491bd986ce22 2 * @author: Natasha Sarkar, 2018
tlee6414 1:491bd986ce22 3 */
tlee6414 1:491bd986ce22 4
tlee6414 1:491bd986ce22 5
tlee6414 1:491bd986ce22 6
tlee6414 1:491bd986ce22 7 #include "mbed.h"
tlee6414 1:491bd986ce22 8 #include "sensor_fusion.h"
tlee6414 1:491bd986ce22 9
tlee6414 1:491bd986ce22 10 MPU6050::MPU6050(PinName sda, PinName scl): i2c_object(sda, scl) {
tlee6414 1:491bd986ce22 11 i2c_object.frequency(400000);
tlee6414 1:491bd986ce22 12 }
tlee6414 1:491bd986ce22 13
tlee6414 1:491bd986ce22 14 void MPU6050::start(void) {
tlee6414 1:491bd986ce22 15 /** TO DO
tlee6414 1:491bd986ce22 16 *
tlee6414 1:491bd986ce22 17 * CONFIGURE THE FOLLOWING REGISTERS ACCORDING TO THE DATASHEET:
tlee6414 1:491bd986ce22 18 *
tlee6414 1:491bd986ce22 19 * PWR_MGMT_1 register to take the IMU out of sleep mode
tlee6414 1:491bd986ce22 20 * ACCEL_CONFIG register to the smallest possible full-scale range (why might we want to do that?)
tlee6414 1:491bd986ce22 21 * GYRO_CONFIG register to the largest possible full-scale range to enable the detection of high-velocity rotations
tlee6414 1:491bd986ce22 22 * CONFIG register to the largest possible bandwidth.
tlee6414 1:491bd986ce22 23 */
tlee6414 1:491bd986ce22 24
tlee6414 1:491bd986ce22 25 /** YOUR CODE GOES BELOW */
tlee6414 1:491bd986ce22 26 //Set 6th bit of PWR_MGMT_1 to 0 to take IMU out of sleep
tlee6414 1:491bd986ce22 27 int mask = 0b11111101;
tlee6414 1:491bd986ce22 28 char* regValue;
tlee6414 1:491bd986ce22 29 read_reg(ADDRESS, PWR_MGMT_1, regValue, 1);
tlee6414 1:491bd986ce22 30 int data = *regValue & mask;
tlee6414 1:491bd986ce22 31 write_reg(ADDRESS, PWR_MGMT_1, data);
tlee6414 1:491bd986ce22 32
tlee6414 1:491bd986ce22 33 //Set 3rd and 4th bit ACCEL_CONFIG to 0 to select smallest range
tlee6414 1:491bd986ce22 34 mask = 0b11100111;
tlee6414 1:491bd986ce22 35 read_reg(ADDRESS, ACCEL_CONFIG, regValue, 1);
tlee6414 1:491bd986ce22 36 data = *regValue & mask;
tlee6414 1:491bd986ce22 37 write_reg(ADDRESS, ACCEL_CONFIG, data);
tlee6414 1:491bd986ce22 38
tlee6414 1:491bd986ce22 39 //Set 3rd and 4th bit of GYRO_CONFIG to 1 to select highest range
tlee6414 1:491bd986ce22 40 mask = 0b00011000;
tlee6414 1:491bd986ce22 41 read_reg(ADDRESS, GYRO_CONFIG, regValue, 1);
tlee6414 1:491bd986ce22 42 data = *regValue | mask;
tlee6414 1:491bd986ce22 43 write_reg(ADDRESS, GYRO_CONFIG, data);
tlee6414 1:491bd986ce22 44
tlee6414 1:491bd986ce22 45 //Set first 3 bits of CONFIG to 0 to select highest bandwidth
tlee6414 1:491bd986ce22 46 mask = 0b00011111;
tlee6414 1:491bd986ce22 47 read_reg(ADDRESS, CONFIG, regValue, 1);
tlee6414 1:491bd986ce22 48 data = *regValue & mask;
tlee6414 1:491bd986ce22 49 write_reg(ADDRESS, CONFIG, data);
tlee6414 1:491bd986ce22 50 }
tlee6414 1:491bd986ce22 51
tlee6414 1:491bd986ce22 52 bool MPU6050::read_raw(float *gx, float *gy, float *gz, float *ax, float *ay, float *az) {
tlee6414 1:491bd986ce22 53 /** TO DO
tlee6414 1:491bd986ce22 54 *
tlee6414 1:491bd986ce22 55 * GET THE RAW READINGS FROM THE ACCELEROMETER/GYRSCOPE
tlee6414 1:491bd986ce22 56 *
tlee6414 1:491bd986ce22 57 * Store the readings in the floats pointed to by the given float pointers.
tlee6414 1:491bd986ce22 58 */
evenbrownie 0:f43994f44684 59
tlee6414 1:491bd986ce22 60 /** YOUR CODE GOES BELOW */
tlee6414 1:491bd986ce22 61 char data[2];
tlee6414 1:491bd986ce22 62 bool a;
tlee6414 1:491bd986ce22 63 bool b;
tlee6414 1:491bd986ce22 64 bool c;
tlee6414 1:491bd986ce22 65 bool d;
tlee6414 1:491bd986ce22 66 bool e;
tlee6414 1:491bd986ce22 67 bool f;
tlee6414 1:491bd986ce22 68
tlee6414 1:491bd986ce22 69 a = read_reg(ADDRESS, GYRO_X, data, 2);
tlee6414 1:491bd986ce22 70 *gx = (float)(short)(data[1] | data[0] << 8);
tlee6414 1:491bd986ce22 71 b = read_reg(ADDRESS, GYRO_Y, data, 2);
tlee6414 1:491bd986ce22 72 *gy = (float)(short)(data[1] | data[0] << 8);
tlee6414 1:491bd986ce22 73 c = read_reg(ADDRESS, GYRO_Z, data, 2);
tlee6414 1:491bd986ce22 74 *gz = (float)(short)(data[1] | data[0] << 8);
tlee6414 1:491bd986ce22 75
tlee6414 1:491bd986ce22 76 d = read_reg(ADDRESS, ACCEL_X, data, 2);
tlee6414 1:491bd986ce22 77 *ax = (float)(short)(data[1] | data[0] << 8);
tlee6414 1:491bd986ce22 78 e = read_reg(ADDRESS, ACCEL_Y, data, 2);
tlee6414 1:491bd986ce22 79 *ay = (float)(short)(data[1] | data[0] << 8);
tlee6414 1:491bd986ce22 80 f = read_reg(ADDRESS, ACCEL_Z, data, 2);
tlee6414 1:491bd986ce22 81 *az = (float)(short)(data[1] | data[0] << 8);
tlee6414 1:491bd986ce22 82
tlee6414 1:491bd986ce22 83 return (a && b && c && d && e && f);
tlee6414 1:491bd986ce22 84 }
tlee6414 1:491bd986ce22 85
tlee6414 1:491bd986ce22 86 bool MPU6050::data_ready(void) {
tlee6414 1:491bd986ce22 87 /** TO DO
tlee6414 1:491bd986ce22 88 *
tlee6414 1:491bd986ce22 89 * CHECK THE INT_STATUS REGISTER TO DETERMINE IF DATA IS READY
tlee6414 1:491bd986ce22 90 *
tlee6414 1:491bd986ce22 91 * Return true if it is ready, false otherwise.
tlee6414 1:491bd986ce22 92 */
tlee6414 1:491bd986ce22 93
tlee6414 1:491bd986ce22 94 /** YOUR CODE GOES BELOW */
tlee6414 1:491bd986ce22 95 char* data;
tlee6414 1:491bd986ce22 96 return (read_reg(ADDRESS, INT_STATUS, data, 1) & 0b00000001);
tlee6414 1:491bd986ce22 97 }
tlee6414 1:491bd986ce22 98
tlee6414 1:491bd986ce22 99 bool MPU6050::write_reg(int addr, int reg, char buf) {
tlee6414 1:491bd986ce22 100 /** TO DO
tlee6414 1:491bd986ce22 101 *
tlee6414 1:491bd986ce22 102 * IMPELEMENT THIS FUNCTION
tlee6414 1:491bd986ce22 103 *
tlee6414 1:491bd986ce22 104 * See the documentation in sensor_fusion.h for detail.
tlee6414 1:491bd986ce22 105 */
tlee6414 1:491bd986ce22 106
tlee6414 1:491bd986ce22 107 /** YOUR CODE GOES BELOW */
tlee6414 1:491bd986ce22 108 char data[2] = {reg, buf};
tlee6414 1:491bd986ce22 109 return i2c_object.write(addr, data, 2);
tlee6414 1:491bd986ce22 110 }
tlee6414 1:491bd986ce22 111
tlee6414 1:491bd986ce22 112 bool MPU6050::read_reg(char addr, char reg, char *buf, int length) {
tlee6414 1:491bd986ce22 113 /** TO DO
tlee6414 1:491bd986ce22 114 *
tlee6414 1:491bd986ce22 115 * IMPLEMENT THIS FUNCTION
tlee6414 1:491bd986ce22 116 *
tlee6414 1:491bd986ce22 117 * See the documentation in sensor_fusion.h for detail.
tlee6414 1:491bd986ce22 118 */
tlee6414 1:491bd986ce22 119
tlee6414 1:491bd986ce22 120 /** YOUR CODE GOES BELOW */
tlee6414 1:491bd986ce22 121
tlee6414 1:491bd986ce22 122 return (i2c_object.write(addr, &reg, 1, true) &&
tlee6414 1:491bd986ce22 123 i2c_object.read(addr, buf, length, false));
tlee6414 1:491bd986ce22 124 }
tlee6414 1:491bd986ce22 125