Added scaled read, C++ wrapper and headers

The new files wrap the mpu6050_i2c.c functions in a cpp class. New read function includes scale. Also split separate accel, gyro, and temp scale functions. NOT tested.

Author: NirajPatelRobots

i2c/mpu6050_i2c/mpu6050.cpp

@@ -0,0 +1,51 @@
+#include "mpu6050.hpp"
+#include "mpu6050_i2c.h"
+#include "pico/stdlib.h"
+#include "pico/binary_info.h"
+#include "hardware/i2c.h"
+
+static MPU6050_Scale MPU6050::accel_scale;
+static MPU6050_Scale MPU6050::gyro_scale;
+
+/*MPU6050::MPU6050() :  //TODO: is there a good way to do this
+chip_temp(*temp) {
+    accel = {0., 0., 0.};
+    gyro = {0., 0., 0.};
+    temp = {};
+    MPU6050(accel, gyro, temp);
+} */
+
+MPU6050::MPU6050(float *accel_out, float *gyro_out) :
+ accel(accel_out),  gyro(gyro_out), chip_temp(temp) {
+    accel_scale = MPU_FS_0;
+    gyro_scale = MPU_FS_0;
+    //I2C init
+    i2c_init(i2c_default, 400 * 1000); // Max bus speed 400 kHz
+    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
+    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+}
+
+MPU6050::power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle) {
+    mpu6050_power(CLKSEL, temp_disable, sleep, cycle);
+}
+
+MPU6050::reset(void) {
+    mpu6050_reset();
+}
+
+MPU6050::setscale_accel(uint8_t scale_num) {
+    accel_scale = (MPU6050_Scale)(scale_num & 3);
+    mpu6050_setscale_accel(accel_scale);
+}
+
+MPU6050::setscale_gyro(uint8_t scale_num) {
+    gyro_scale = (MPU6050_Scale)(scale_num & 3);
+    mpu6050_setscale_gyro(gyro_scale);
+}
+
+MPU6050::read(void) {
+    mpu6050_read(accel, gyro, &temp, accel_scale, gyro_scale);
+}
+    

i2c/mpu6050_i2c/mpu6050.hpp

@@ -0,0 +1,15 @@
+/* class for using an MPU6050 IMU sensor on pi pico.
+An abstraction of pico example C code. */
+
+class MPU6050 {
+    float *accel, *gyro, temp;
+public:
+    const float &accel[3], &gyro[3], &chip_temp; // [m/s^2], [rad/s], [C]
+    MPU6050(void);
+    MPU6050(float *accel_out, float *gyro_out);
+    void power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle);
+    void reset(void);
+    void setscale_accel(uint8_t scale_num); //scale 0-3 is 2g, 4g, 8g, or 16g
+    void setscale_gyro(uint8_t scale_num); // scale 0-3 is 250, 500, 1000, or 2000 deg/s
+    void read(void);
+}

i2c/mpu6050_i2c/mpu6050_i2c.c

@@ -36,10 +36,10 @@ const uint8_t REG_PWR_MGMT_1 = 0x6B, REG_ACCEL_OUT = 0x3B, REG_GYRO_OUT = 0x43,
              REG_SIGNAL_PATH_RESET = 0x68, REG_GYRO_CONFIG = 0x1B, REG_ACCEL_CONFIG = 0x1C;
 typedef enum MPU6050_Scale {MPU_FS_0, MPU_FS_1, MPU_FS_2, MPU_FS_3} MPU6050_Scale;
 
-float gyro_scale_deg[] = {250. / 0x7fff, 500. / 0x7fff, 1000. / 0x7fff, 2000. / 0x7fff};
-float gyro_scale_rad[] = {M_PI / 180. * 250. / 0x7fff, M_PI / 180. * 500. / 0x7fff,
+const float gyro_scale_deg[] = {250. / 0x7fff, 500. / 0x7fff, 1000. / 0x7fff, 2000. / 0x7fff};
+const float gyro_scale_rad[] = {M_PI / 180. * 250. / 0x7fff, M_PI / 180. * 500. / 0x7fff,
                           M_PI / 180. *  1000. / 0x7fff, M_PI / 180. *  2000. / 0x7fff};
-float accel_scale[] = {2. / 0x7fff, 4. / 0x7fff, 8. / 0x7fff, 16. / 0x7fff};
+const float accel_scale[] = {2 * 9.81 / 0x7fff, 4 * 9.81 / 0x7fff, 8 * 9.81 / 0x7fff, 16 * 9.81 / 0x7fff};
 
 
 ////////////////////////////////////////////////////lower level functions for i2c
@@ -102,31 +102,45 @@ void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp) {
     mpu6050_readreg16(REG_TEMP_OUT, temp, 1);
 }
 
+void mpu6050_read(float accel[3], float gyro_rad[3], float *temp, MPU6050_Scale accel_scale, MPU6050_Scale gyro_scale) {
+    int16_t buffer[7];
+    mpu6050_readreg16(REG_ACCEL_OUT, buffer, 7); //reads all at same time
+    mpu6050_scale_accel(accel, buffer, accel_scale);
+    if (temp) *temp = mpu6050_scale_temp(buffer[3]);
+    mpu6050_scale_gyro_rad(gyro, buffer + 4, gyro_scale);
+}
+
 // set and use sensitivity
-void mpu6050_setscale(MPU6050_Scale accel_scale, MPU6050_Scale gyro_scale) {
+void mpu6050_setscale_gyro(MPU6050_Scale gyro_scale) {
     mpu6050_writereg(REG_GYRO_CONFIG, (uint8_t)gyro_scale << 3);
+}
+void mpu6050_setscale_accel(MPU6050_Scale accel_scale) {
     mpu6050_writereg(REG_ACCEL_CONFIG, (uint8_t)accel_scale << 3);
 }
-void mpu6050_scale_accel(float *accel, int16_t *accel_raw, MPU6050_Scale scale) {
+void mpu6050_scale_accel(float accel[3], int16_t accel_raw[3], MPU6050_Scale scale) {
     for (int i = 0; i < 3; i++) {
         accel[i] = accel_raw[i] * accel_scale[scale];
     }
 }
-void mpu6050_scale_gyro_deg(float *gyro, int16_t *gyro_raw, MPU6050_Scale scale) {
+void mpu6050_scale_gyro_deg(float gyro[3], int16_t gyro_raw[3], MPU6050_Scale scale) {
     for (int i = 0; i < 3; i++) {
         gyro[i] = gyro_raw[i] * gyro_scale_deg[scale];
     }
 }
-void mpu6050_scale_gyro_rad(float *gyro, int16_t *gyro_raw, MPU6050_Scale scale) {
+void mpu6050_scale_gyro_rad(float gyro[3], int16_t gyro_raw[3], MPU6050_Scale scale) {
     for (int i = 0; i < 3; i++) {
         gyro[i] = gyro_raw[i] * gyro_scale_rad[scale];
     }
 }
+float mpu6050_scale_temp(float temp_raw) {
+    return (temp_raw / 340.0) + 36.53;
+}
+
 //TODO: set timing
 //TODO: set and read filter cutoff
 //TODO: read self test
 
-
+//TODO: functional mpu test
 int main() {
     stdio_init_all();
 #if !defined(i2c_default) || !defined(PICO_DEFAULT_I2C_SDA_PIN) || !defined(PICO_DEFAULT_I2C_SCL_PIN)
@@ -148,7 +162,8 @@ int main() {
     //setting CLKSEL = 1 gets better results than 0 if gyro is running and no sleep mode
     mpu6050_power(1, false, false, false);
     MPU6050_Scale testscale = MPU_FS_0;
-    mpu6050_setscale(testscale, testscale);
+    mpu6050_setscale_accel(testscale);
+    mpu6050_setscale_gyro(testscale);
 
     int16_t accel_raw[3], gyro_raw[3], temp_raw;
     float acceleration[3], gyro[3];
@@ -157,14 +172,12 @@ int main() {
         mpu6050_read_raw(accel_raw, gyro_raw, &temp_raw);
         //TODO time read function
 
-        // These are the raw numbers from the chip, so will need tweaking to be really useful.
-        // See the datasheet for more information
+        // These are the raw numbers from the chip
         printf("Raw Acc. X = %d, Y = %d, Z = %d\n", accel_raw[0], accel_raw[1], accel_raw[2]);
         printf("Raw Gyro. X = %d, Y = %d, Z = %d\n", gyro_raw[0], gyro_raw[1], gyro_raw[2]);
         // Temperature is simple so use the datasheet calculation to get deg C.
         // Note this is chip temperature.
-        printf("Temp [C] = %f \t\t Scale = %d\n", (temp_raw / 340.0) + 36.53, testscale);
-        //include the scaling
+        printf("Temp [C] = %f \t\t Scale = %d\n", mpu6050_scale_temp(temp_raw), testscale);
         mpu6050_scale_accel(acceleration, accel_raw, testscale);
         mpu6050_scale_gyro_rad(gyro, gyro_raw, testscale);
         printf("Acc [m/s^2] X = %f, Y = %f, Z = %f\n", acceleration[0], acceleration[1], acceleration[2]);

i2c/mpu6050_i2c/mpu6050_i2c.h

@@ -0,0 +1,31 @@
+/* header for accessing mpu6050 functions in pico-examples
+
+Niraj Patel March 2022 */
+
+#include "pico/stdlib.h"
+#include "pico/binary_info.h"
+
+typedef enum MPU6050_Scale {MPU_FS_0, MPU_FS_1, MPU_FS_2, MPU_FS_3} MPU6050_Scale;
+
+// lower level functions for i2c
+void mpu6050_writereg(uint8_t reg, uint8_t value); //write one byte to a register
+void mpu6050_readreg(uint8_t reg, uint8_t *out, size_t length);
+void mpu6050_readreg16(uint8_t reg, uint16_t *out, size_t length);
+void mpu6050_setbusaddr(uint8_t addr); //set the i2c bus address for communication. MPU6050 must already have this value.
+
+// higher level mpu6050 functions
+void mpu6050_power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle); /* Set the power and clock settings.
+    CLKSEL is clock source, see docs. Recommended CLKSEL = 1 if gyro is enabled.
+    temp_disable disables temperature, sleep enables sleep mode, cycle wakes up only when converting. */
+void mpu6050_reset(); // Reset power management and signal path registers. MPU6050 returns to default settings.
+//set and use scaling
+void mpu6050_setscale_accel(MPU6050_Scale accel_scale);
+void mpu6050_setscale_gyro(MPU6050_Scale gyro_scale);
+void mpu6050_scale_accel(float accel[3], int16_t accel_raw[3], MPU6050_Scale scale); //sets accel[3] in m/s^2 from accel_raw[3]
+void mpu6050_scale_gyro_deg(float gyro[3], int16_t gyro_raw[3], MPU6050_Scale scale); //sets gyro[3] in degrees from gyro_raw[3]
+void mpu6050_scale_gyro_rad(float gyro[3], int16_t gyro_raw[3], MPU6050_Scale scale); //sets gyro[3] in radians from gyro_raw[3]
+float mpu6050_scale_temp(float temp_raw);
+
+void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp); //read raw data values. Could read different timesteps.
+void mpu6050_read(float accel[3], float gyro[3], float *temp,
+                  MPU6050_Scale accel_scale, MPU6050_Scale gyro_scale); //reads all at same timestep, converts. temp can be NULL.