Diff: sensor_fusion.cpp

Revision:

1:491bd986ce22

Parent:

0:f43994f44684

Child:

2:a4d5e7f96e87

--- a/sensor_fusion.cpp	Thu Nov 08 20:48:47 2018 +0000
+++ b/sensor_fusion.cpp	Sat Nov 10 00:23:07 2018 +0000
@@ -1,71 +1,125 @@
-/* 
- * @author: Natasha Sarkar, 2018
- */
-
-#include "mbed.h"
-#include "sensor_fusion.h"
- 
-MPU6050::MPU6050(PinName sda, PinName scl): i2c_object(sda, scl) {
-    i2c_object.frequency(400000);
-}
- 
-void MPU6050::start(void) {
-    /** TO DO 
-     * 
-     * CONFIGURE THE FOLLOWING REGISTERS ACCORDING TO THE DATASHEET:
-     *
-     * PWR_MGMT_1 register to take the IMU out of sleep mode
-     * ACCEL_CONFIG register to the smallest possible full-scale range (why might we want to do that?)
-     * GYRO_CONFIG register to the largest possible full-scale range to enable the detection of high-velocity rotations
-     * CONFIG register to the largest possible bandwidth.
-     */
-
-    /** YOUR CODE GOES BELOW */ 
-
-}
- 
-bool MPU6050::read_raw(float *gx, float *gy, float *gz, float *ax, float *ay, float *az) {
-    /** TO DO
-     * 
-     * GET THE RAW READINGS FROM THE ACCELEROMETER/GYRSCOPE
-     *
-     * Store the readings in the floats pointed to by the given float pointers.
-     */
-
-    /** YOUR CODE GOES BELOW */ 
-
-}
-
-bool MPU6050::data_ready(void) {
-    /** TO DO
-     * 
-     * CHECK THE INT_STATUS REGISTER TO DETERMINE IF DATA IS READY
-     *
-     * Return true if it is ready, false otherwise.
-     */
-
-    /** YOUR CODE GOES BELOW */ 
-}
-
-bool MPU6050::write_reg(int addr, int reg, char buf) {
-    /** TO DO
-     * 
-     * IMPELEMENT THIS FUNCTION
-     *
-     * See the documentation in sensor_fusion.h for detail.
-     */
-
-    /** YOUR CODE GOES BELOW */ 
-}
- 
-bool MPU6050::read_reg(char addr, char reg, char *buf, int length) {
-    /** TO DO
-     * 
-     * IMPLEMENT THIS FUNCTION
-     * 
-     * See the documentation in sensor_fusion.h for detail.
-     */
-
-    /** YOUR CODE GOES BELOW */ 
-}
+/* 
+ * @author: Natasha Sarkar, 2018
+ */
+ 
+
+
+#include "mbed.h"
+#include "sensor_fusion.h"
+ 
+MPU6050::MPU6050(PinName sda, PinName scl): i2c_object(sda, scl) {
+    i2c_object.frequency(400000);
+}
+ 
+void MPU6050::start(void) {
+    /** TO DO 
+     * 
+     * CONFIGURE THE FOLLOWING REGISTERS ACCORDING TO THE DATASHEET:
+     *
+     * PWR_MGMT_1 register to take the IMU out of sleep mode
+     * ACCEL_CONFIG register to the smallest possible full-scale range (why might we want to do that?)
+     * GYRO_CONFIG register to the largest possible full-scale range to enable the detection of high-velocity rotations
+     * CONFIG register to the largest possible bandwidth.
+     */
+
+    /** YOUR CODE GOES BELOW */ 
+    //Set 6th bit of PWR_MGMT_1 to 0 to take IMU out of sleep
+    int mask = 0b11111101;
+    char* regValue;
+    read_reg(ADDRESS, PWR_MGMT_1, regValue, 1);
+    int data = *regValue & mask;
+    write_reg(ADDRESS, PWR_MGMT_1, data);
+    
+    //Set 3rd and 4th bit ACCEL_CONFIG to 0 to select smallest range
+    mask = 0b11100111;
+    read_reg(ADDRESS, ACCEL_CONFIG, regValue, 1);
+    data = *regValue & mask;
+    write_reg(ADDRESS, ACCEL_CONFIG, data);
+    
+    //Set 3rd and 4th bit of GYRO_CONFIG to 1 to select highest range
+    mask = 0b00011000;
+    read_reg(ADDRESS, GYRO_CONFIG, regValue, 1);
+    data = *regValue | mask;
+    write_reg(ADDRESS, GYRO_CONFIG, data);
+    
+    //Set first 3 bits of CONFIG to 0 to select highest bandwidth
+    mask = 0b00011111;
+    read_reg(ADDRESS, CONFIG, regValue, 1);
+    data = *regValue & mask;
+    write_reg(ADDRESS, CONFIG, data);
+}
+ 
+bool MPU6050::read_raw(float *gx, float *gy, float *gz, float *ax, float *ay, float *az) {
+    /** TO DO
+     * 
+     * GET THE RAW READINGS FROM THE ACCELEROMETER/GYRSCOPE
+     *
+     * Store the readings in the floats pointed to by the given float pointers.
+     */
 
+    /** YOUR CODE GOES BELOW */ 
+    char data[2];
+    bool a;
+    bool b;
+    bool c;
+    bool d;
+    bool e;
+    bool f;
+    
+    a = read_reg(ADDRESS, GYRO_X, data, 2);
+    *gx = (float)(short)(data[1] | data[0] << 8);
+    b = read_reg(ADDRESS, GYRO_Y, data, 2);
+    *gy = (float)(short)(data[1] | data[0] << 8);
+    c = read_reg(ADDRESS, GYRO_Z, data, 2);
+    *gz = (float)(short)(data[1] | data[0] << 8);
+    
+    d = read_reg(ADDRESS, ACCEL_X, data, 2);
+    *ax = (float)(short)(data[1] | data[0] << 8);
+    e = read_reg(ADDRESS, ACCEL_Y, data, 2);
+    *ay = (float)(short)(data[1] | data[0] << 8);
+    f = read_reg(ADDRESS, ACCEL_Z, data, 2);
+    *az = (float)(short)(data[1] | data[0] << 8);
+    
+    return (a && b && c && d && e && f);
+}
+
+bool MPU6050::data_ready(void) {
+    /** TO DO
+     * 
+     * CHECK THE INT_STATUS REGISTER TO DETERMINE IF DATA IS READY
+     *
+     * Return true if it is ready, false otherwise.
+     */
+
+    /** YOUR CODE GOES BELOW */ 
+    char* data;
+    return (read_reg(ADDRESS, INT_STATUS, data, 1) & 0b00000001);
+}
+
+bool MPU6050::write_reg(int addr, int reg, char buf) {
+    /** TO DO
+     * 
+     * IMPELEMENT THIS FUNCTION
+     *
+     * See the documentation in sensor_fusion.h for detail.
+     */
+
+    /** YOUR CODE GOES BELOW */ 
+    char data[2] = {reg, buf};
+    return i2c_object.write(addr, data, 2);
+}
+ 
+bool MPU6050::read_reg(char addr, char reg, char *buf, int length) {
+    /** TO DO
+     * 
+     * IMPLEMENT THIS FUNCTION
+     * 
+     * See the documentation in sensor_fusion.h for detail.
+     */
+
+    /** YOUR CODE GOES BELOW */
+    
+    return (i2c_object.write(addr, &reg, 1, true) &&
+            i2c_object.read(addr, buf, length, false));
+}
+