Evan Brown / Mbed 2 deprecated APpart3_E_start_2

completed code

Dependencies:   mbed

sensor_fusion.cpp@5:4af75af374cc, 2018-11-22 (annotated)

Committer:
evenbrownie
Date:
Thu Nov 22 22:40:15 2018 +0000
Revision:
5:4af75af374cc
Parent:
2:a4d5e7f96e87 
blah

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 #include "mbed.h"
tlee6414 1:491bd986ce22 6 #include "sensor_fusion.h"
tlee6414 1:491bd986ce22 7
tlee6414 1:491bd986ce22 8 MPU6050::MPU6050(PinName sda, PinName scl): i2c_object(sda, scl) {
tlee6414 1:491bd986ce22 9 i2c_object.frequency(400000);
tlee6414 1:491bd986ce22 10 }
tlee6414 1:491bd986ce22 11
tlee6414 1:491bd986ce22 12 void MPU6050::start(void) {
tlee6414 2:a4d5e7f96e87 13 write_reg(ADDRESS, PWR_MGMT_1, 0x00);
tlee6414 2:a4d5e7f96e87 14 write_reg(ADDRESS, GYRO_CONFIG, 0x03 << 3);
tlee6414 2:a4d5e7f96e87 15 write_reg(ADDRESS, ACCEL_CONFIG, 0x00);
tlee6414 2:a4d5e7f96e87 16 write_reg(ADDRESS, CONFIG, 0x00);
tlee6414 1:491bd986ce22 17 }
tlee6414 1:491bd986ce22 18
tlee6414 1:491bd986ce22 19 bool MPU6050::read_raw(float *gx, float *gy, float *gz, float *ax, float *ay, float *az) {
tlee6414 2:a4d5e7f96e87 20 char raw_gyro[6];
tlee6414 2:a4d5e7f96e87 21 char raw_accel[6];
tlee6414 2:a4d5e7f96e87 22 if (read_reg(ADDRESS, GYRO_X, raw_gyro, 6)) {
tlee6414 2:a4d5e7f96e87 23 read_reg(ADDRESS, ACCEL_X, raw_accel, 6);
tlee6414 2:a4d5e7f96e87 24 *gx = float(short(raw_gyro[0] << 8 | raw_gyro[1])) + 13;
tlee6414 2:a4d5e7f96e87 25 *gy = float(short(raw_gyro[2] << 8 | raw_gyro[3])) - 18;
tlee6414 2:a4d5e7f96e87 26 *gz = float(short(raw_gyro[4] << 8 | raw_gyro[5])) + 80;
tlee6414 2:a4d5e7f96e87 27 *ax = float(short(raw_accel[0] << 8 | raw_accel[1])) - 1700;
tlee6414 2:a4d5e7f96e87 28 *ay = float(short(raw_accel[2] << 8 | raw_accel[3])) + 100;
tlee6414 2:a4d5e7f96e87 29 *az = float(short(raw_accel[4] << 8 | raw_accel[5])) - 14400;
tlee6414 2:a4d5e7f96e87 30 return true;
tlee6414 2:a4d5e7f96e87 31 }
tlee6414 2:a4d5e7f96e87 32 return false;
tlee6414 1:491bd986ce22 33 }
tlee6414 1:491bd986ce22 34
tlee6414 1:491bd986ce22 35 bool MPU6050::data_ready(void) {
tlee6414 2:a4d5e7f96e87 36 char int_status;
tlee6414 2:a4d5e7f96e87 37 read_reg(ADDRESS, INT_STATUS, &int_status, 1);
tlee6414 2:a4d5e7f96e87 38 return int_status & 1;
tlee6414 1:491bd986ce22 39 }
tlee6414 1:491bd986ce22 40
tlee6414 2:a4d5e7f96e87 41 bool MPU6050::write_reg(int addr, char reg, char buf) {
tlee6414 1:491bd986ce22 42 char data[2] = {reg, buf};
tlee6414 2:a4d5e7f96e87 43 return MPU6050::i2c_object.write(addr, data, 2) == 0;
tlee6414 1:491bd986ce22 44 }
tlee6414 1:491bd986ce22 45
tlee6414 2:a4d5e7f96e87 46 bool MPU6050::read_reg(int addr, char reg, char *buf, int length) {
tlee6414 2:a4d5e7f96e87 47 return i2c_object.write(addr, &reg, 1, true) == 0 &&
tlee6414 2:a4d5e7f96e87 48 i2c_object.read(addr, buf, length) == 0;
tlee6414 1:491bd986ce22 49 }