tidy & clarify, move demo code to i2c_main.c

Author: NirajPatelRobots

i2c/mpu6050_i2c/mpu6050.cpp

@@ -21,14 +21,6 @@ MPU6050TimingParams::MPU6050TimingParams(const MPU6050SensorTimingParams &_accel
     : accel_timing(_accel_timing), gyro_timing(_gyro_timing) {}
 
 
-/*MPU6050::MPU6050() :  //TODO: smart pointers
-chip_temp(*temp) {
-    accel = {0., 0., 0.};
-    gyro = {0., 0., 0.};
-    temp = {};
-    MPU6050(accel, gyro, temp);
-} */
-
 MPU6050::MPU6050(float *accel_out, float *gyro_out, uint8_t addr) :
  chip_temp(temp), accel(accel_out),  gyro(gyro_out), accel_scale(Scale_0), gyro_scale(Scale_0), bus_addr(addr) {
     setup_MPU6050_i2c();
@@ -61,6 +53,16 @@ void MPU6050::read(void) {
     mpu6050_read(accel, gyro, &temp, (MPU6050_Scale)accel_scale, (MPU6050_Scale)gyro_scale);
 }
 
+void MPU6050::read_accel(void) {
+    mpu6050_setbusaddr(bus_addr);
+    mpu6050_read_accel(accel, (MPU6050_Scale)accel_scale);
+}
+
+void MPU6050::read_gyro(void) {
+    mpu6050_setbusaddr(bus_addr);
+    mpu6050_read_gyro_rad(gyro, (MPU6050_Scale)gyro_scale);
+}
+
 bool MPU6050::is_connected() {
     mpu6050_setbusaddr(bus_addr);
     return mpu6050_is_connected();
@@ -87,3 +89,11 @@ uint8_t MPU6050::read_interrupt_status() {
     mpu6050_setbusaddr(bus_addr);
     return mpu6050_read_interrupt_status();
 }
+
+
+float MPU6050_max_accel(MPU6050::Scale scale) {
+    return accel_scale_vals[(int)scale];
+}
+float MPU6050_max_gyro_rad(MPU6050::Scale scale) {
+    return gyro_scale_rad[(int)scale];
+}

i2c/mpu6050_i2c/mpu6050.hpp

@@ -1,7 +1,5 @@
 /* class for using an MPU6050 IMU sensor on pi pico.
-An abstraction of pico example C code. 
-TODO:
-    smart ptrs for results
+A C++ interface to the pico example C code.
  */
 #include "stdint.h"
 
@@ -22,7 +20,6 @@ class MPU6050 {
 public:
     const float &chip_temp; // [C]
 
-    MPU6050(); //TODO
     MPU6050(float *accel_out, float *gyro_out, uint8_t i2c_addr=0x68); // [m/s^2], [rad/s]
 
     void power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle);
@@ -33,6 +30,8 @@ class MPU6050 {
     void setscale_accel(Scale scale); //scale 0-3 is 2g, 4g, 8g, or 16g
     void setscale_gyro(Scale scale); // scale 0-3 is 250, 500, 1000, or 2000 deg/s
     void read(void);
+    void read_accel(void);
+    void read_gyro(void);
     bool is_connected(void);
     void set_timing(uint8_t lowpass_filter_cfg, uint8_t sample_rate_div);
     MPU6050TimingParams read_timing(void);
@@ -47,3 +46,6 @@ class MPU6050 {
     enum Scale accel_scale, gyro_scale;
     uint8_t bus_addr;
 };
+
+float MPU6050_max_accel(MPU6050::Scale scale);
+float MPU6050_max_gyro_rad(MPU6050::Scale scale);

i2c/mpu6050_i2c/mpu6050_i2c.c

@@ -12,23 +12,6 @@
 #include "hardware/i2c.h"
 #include "mpu6050_i2c.h"
 
-/* Example code to talk to a MPU6050 MEMS accelerometer and gyroscope
-
-   This is taking to simple approach of simply reading registers. It's perfectly
-   possible to link up an interrupt line and set things up to read from the
-   inbuilt FIFO to make it more useful.
-
-   NOTE: Ensure the device is capable of being driven at 3.3v NOT 5v. The Pico
-   GPIO (and therefor I2C) cannot be used at 5v. You will need to use a level
-   shifter on the I2C lines if you want to run the board at 5v.
-
-   Connections on Raspberry Pi Pico board, other boards may vary.
-
-   GPIO PICO_DEFAULT_I2C_SDA_PIN (On Pico this is GP4 (pin 6)) -> SDA on MPU6050 board
-   GPIO PICO_DEFAULT_I2C_SCL_PIN (On Pico this is GP5 (pin 7)) -> SCL on MPU6050 board
-   3.3v (pin 36) -> VCC on MPU6050 board
-   GND (pin 38)  -> GND on MPU6050 board
-*/
 
 // By default these devices are on bus address 0x68
 static uint8_t bus_addr = 0x68;
@@ -43,6 +26,7 @@ const float gyro_scale_rad[] = {M_PI / 180. * 250. / 0x7fff, M_PI / 180. * 500.
                           M_PI / 180. *  1000. / 0x7fff, M_PI / 180. *  2000. / 0x7fff};
 const float accel_scale_vals[] = {2 * 9.81 / 0x7fff, 4 * 9.81 / 0x7fff, 8 * 9.81 / 0x7fff, 16 * 9.81 / 0x7fff};
 
+// timing data, indexed with lowpass_filter_cfg
 const int accel_bandwidth_lookup[] = {260, 184, 94, 44, 21, 10, 5};
 const float accel_delay_lookup[] = {0, 2.0, 3.0, 4.9, 8.5, 13.8, 19.0};
 const int gyro_bandwidth_lookup[] = {256, 188, 98, 42, 20, 10, 5};
@@ -98,10 +82,12 @@ void mpu6050_reset() {
     sleep_ms(100);
 }
 
-void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp) {
+void mpu6050_read_accel_raw(int16_t accel[3]) {
     mpu6050_readreg16(REG_ACCEL_OUT, accel, 3);
+}
+
+void mpu6050_read_gyro_raw(int16_t gyro[3]) {
     mpu6050_readreg16(REG_GYRO_OUT, gyro, 3);
-    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) {
@@ -112,6 +98,21 @@ void mpu6050_read(float accel[3], float gyro_rad[3], float *temp, MPU6050_Scale
     if (temp) *temp = mpu6050_scale_temp(buffer[3]);
     mpu6050_scale_gyro_rad(gyro_rad, buffer + 4, gyro_scale);
 }
+void mpu6050_read_accel(float accel[3], MPU6050_Scale accel_scale) {
+    int16_t buffer[3];
+    mpu6050_readreg16(REG_ACCEL_OUT, buffer, 3);
+    mpu6050_scale_accel(accel, buffer, accel_scale);
+}
+void mpu6050_read_gyro_rad(float gyro[3], MPU6050_Scale gyro_scale) {
+    int16_t buffer[3];
+    mpu6050_readreg16(REG_GYRO_OUT, buffer, 3);
+    mpu6050_scale_gyro_rad(gyro, buffer, gyro_scale);
+}
+void mpu6050_read_gyro_deg(float gyro[3], MPU6050_Scale gyro_scale) {
+    int16_t buffer[3];
+    mpu6050_readreg16(REG_GYRO_OUT, buffer, 3);
+    mpu6050_scale_gyro_deg(gyro, buffer, gyro_scale);
+}
 
 // functions to set and calculate with sensitivity
 void mpu6050_setscale_gyro(MPU6050_Scale gyro_scale) {
@@ -208,42 +209,3 @@ bool setup_MPU6050_i2c() {
     return true;
 #endif
 }
-
-int run_MPU6050_demo() {
-    MPU6050_Scale testscale = MPU_FS_0;
-    mpu6050_setscale_accel(testscale);
-    mpu6050_setscale_gyro(testscale);
-
-    int16_t accel_raw[3], gyro_raw[3], temp_raw;
-    float acceleration[3], gyro[3];
-
-    mpu6050_reset();
-    //setting CLKSEL = 1 gets better results than 0 if gyro is running and no sleep mode
-    mpu6050_power(1, false, false, false);
-
-    mpu6050_set_timing(5, 99); // 10 Hz
-
-    while (1) {
-        while (!mpu6050_is_connected()) {
-            printf("MPU6050 is not connected...\n");
-            sleep_ms(1000);
-        }
-        uint64_t time_us = to_us_since_boot(get_absolute_time());
-        mpu6050_read_raw(accel_raw, gyro_raw, &temp_raw);
-        time_us = to_us_since_boot(get_absolute_time()) - time_us;
-
-        // 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]);
-        // This is chip temperature.
-        printf("Temp [C] = %f\n", mpu6050_scale_temp(temp_raw));
-        printf("Read time: %llu us; \t\t Accel and Gyro Scale = %d\n", time_us, (int)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]);
-        printf("Gyro [rad/s] X = %f, Y = %f, Z = %f\n", gyro[0], gyro[1], gyro[2]);
-
-        sleep_ms(100);
-    }
-    return 0;
-}

i2c/mpu6050_i2c/mpu6050_i2c.h

@@ -16,15 +16,9 @@ typedef struct {
     float sample_rate; // rate of new data loading in the register [Hz]
 } mpu6050_timing_params_t;
 
-// lower level functions for i2c
-void mpu6050_writereg(uint8_t reg, uint8_t value); //write one byte to a register
-//read length 8-bit integers from the register at reg, store them in *out.
-void mpu6050_readreg(uint8_t reg, uint8_t *out, size_t length);
-//read length 16-bit integers from the register at reg, store them in *out. Max length 32 (= 64 bytes)
-void mpu6050_readreg16(uint8_t reg, int16_t *out, size_t length);
+bool setup_MPU6050_i2c(); //call this before using any other functions
 void mpu6050_setbusaddr(uint8_t addr); //set the i2c bus address for communication. MPU6050 must already have this value.
 
-// higher level mpu6050 functions
 /* 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. */
@@ -54,18 +48,20 @@ uint8_t mpu6050_read_interrupt_status(); // 0 =  no interrupts set, 1 = data rea
 //set and use scaling. The first read() after setscale() might not have the updated scaling.
 void mpu6050_setscale_accel(MPU6050_Scale accel_scale);
 void mpu6050_setscale_gyro(MPU6050_Scale gyro_scale);
+extern const float gyro_scale_deg[4], gyro_scale_rad[4], accel_scale_vals[4]; // scale constants, index with MPU6050_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]
+void mpu6050_scale_gyro_deg(float gyro[3], int16_t gyro_raw[3], MPU6050_Scale scale); //sets gyro[3] in degrees/s 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/s from gyro_raw[3]
 float mpu6050_scale_temp(float temp_raw);
 
-// core IMU reading functions
-void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp); //read raw data values. Could read different samples.
-void mpu6050_read(float accel[3], float gyro[3], float *temp,
+// core sensor data reading functions
+void mpu6050_read_accel_raw(int16_t accel[3]);
+void mpu6050_read_gyro_raw(int16_t gyro[3]);
+void mpu6050_read(float accel[3], float gyro_rad[3], float *temp,
                   MPU6050_Scale accel_scale, MPU6050_Scale gyro_scale); //reads all at same sample time, converts. temp can be NULL.
-
-bool setup_MPU6050_i2c(); //call this before using any other functions
-int run_MPU6050_demo();
+void mpu6050_read_accel(float accel[3], MPU6050_Scale accel_scale);
+void mpu6050_read_gyro_rad(float gyro[3], MPU6050_Scale gyro_scale);
+void mpu6050_read_gyro_deg(float gyro[3], MPU6050_Scale gyro_scale);
 
 #ifdef __cplusplus
 }

i2c/mpu6050_i2c/mpu6050_i2c_irq_main.cpp

@@ -18,7 +18,7 @@ const uint8_t LOWPASS_FILTER_CFGS[] = {5, 1};
 volatile bool got_irq;
 
 static std::unique_ptr<MPU6050> IMU;
-static float accel[3] = {0}, gyro[3] = {0}; // TODO we shouldn't need this
+static float accel[3] = {0}, gyro[3] = {0};
 
 void print_SensorTimingParams(const MPU6050SensorTimingParams &params) {
     printf("Sample Rate: %.2f, Bandwidth: %i, Delay: %.1f\n", params.sample_rate, params.bandwidth, params.delay);
@@ -72,6 +72,7 @@ int main() {
     bi_decl(bi_1pin_with_name(IRQ_PIN, "IMU IRQ pin 1"));
     IMU = std::make_unique<MPU6050>(accel, gyro);
     IMU->reset();
+    //setting CLKSEL = 1 gets better results than 0 if gyro is running and no sleep mode
     IMU->power(1, false, false, false);
     // push-pull is faster, latch allows debugging with the INT pin
     IMU->configure_interrupt(false, false, true, USE_READ_CLEAR, true);
@@ -85,6 +86,10 @@ int main() {
             printf("MPU6050 is not connected...\n");
         } else if (!got_irq){
             printf("MPU6050 is connected, but didn't trigger an interrupt\n");
+            IMU->read();
+            printf("Read accel and gyro without IRQ:\n"
+                   "%+6f %+6f %+6f | %+6f %+6f %+6f\n",
+                   accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
         }
     }
 }

i2c/mpu6050_i2c/mpu6050_i2c_main.c

@@ -1,5 +1,16 @@
 /**
- * Boilerplate main for mpu6050 I2c example
+   Example code to talk to a MPU6050 MEMS accelerometer and gyroscope
+
+   NOTE: Ensure the device is capable of being driven at 3.3v NOT 5v. The Pico
+   GPIO (and therefor I2C) cannot be used at 5v. You will need to use a level
+   shifter on the I2C lines if you want to run the board at 5v.
+
+   Connections on Raspberry Pi Pico board, other boards may vary.
+
+   GPIO PICO_DEFAULT_I2C_SDA_PIN (On Pico this is GP4 (pin 6)) -> SDA on MPU6050 board
+   GPIO PICO_DEFAULT_I2C_SCL_PIN (On Pico this is GP5 (pin 7)) -> SCL on MPU6050 board
+   3.3v (pin 36) -> VCC on MPU6050 board
+   GND (pin 38)  -> GND on MPU6050 board
  */
 
 #include <stdio.h>
@@ -12,5 +23,53 @@ int main() {
     if (setup_MPU6050_i2c()) {
         printf("Hello, MPU6050! Reading raw data from registers...\n");
     }
-    return run_MPU6050_demo();
+    MPU6050_Scale testscale = MPU_FS_0;
+
+    float acceleration[3], gyro[3], temperature;
+    int16_t raw_accel[3], raw_gyro[3];
+
+    mpu6050_reset();
+    //setting CLKSEL = 1 gets better results than 0 if gyro is running and no sleep mode
+    mpu6050_power(1, false, false, false);
+    mpu6050_setscale_accel(testscale);
+    mpu6050_setscale_gyro(testscale);
+    mpu6050_set_timing(6, 99); // lowpass frequency = 5 Hz, sample rate = 10 Hz
+
+    while (1) {
+        while (!mpu6050_is_connected()) {
+            printf("MPU6050 is not connected...\n");
+            sleep_ms(1000);
+        }
+        uint64_t time_us = to_us_since_boot(get_absolute_time());
+        mpu6050_read_accel_raw(raw_accel);
+        mpu6050_read_gyro_raw(raw_gyro);
+        time_us = to_us_since_boot(get_absolute_time()) - time_us;
+        printf("Takes %llu us to read raw accel and gyro readings separately\n"
+               "Acc X = %d, Y = %d, Z = %d\nGyro X = %d, Y = %d, Z = %d\n",
+               time_us, raw_accel[0], raw_accel[1], raw_accel[2],
+               raw_gyro[0], raw_gyro[1], raw_gyro[2]);
+
+        time_us = to_us_since_boot(get_absolute_time());
+        mpu6050_read_accel(acceleration, testscale);
+        mpu6050_read_gyro_rad(gyro, testscale);
+        time_us = to_us_since_boot(get_absolute_time()) - time_us;
+        printf("Takes %llu us to read and scale accel and gyro readings separately\n"
+               "Acc [m/s^2] X = %f, Y = %f, Z = %f\n"
+               "Gyro [rad/s] X = %f, Y = %f, Z = %f\n",
+               time_us, acceleration[0], acceleration[1], acceleration[2],
+               gyro[0], gyro[1], gyro[2]);
+
+        time_us = to_us_since_boot(get_absolute_time());
+        mpu6050_read(acceleration, gyro, &temperature, testscale, testscale);
+        time_us = to_us_since_boot(get_absolute_time()) - time_us;
+        printf("Takes %llu us to read and scale all together\n"
+               "Acc [m/s^2] X = %f, Y = %f, Z = %f\n"
+               "Gyro [rad/s] X = %f, Y = %f, Z = %f\n"
+               "Temp [C] = %f\n----------------------\n",
+               time_us, acceleration[0], acceleration[1], acceleration[2],
+               gyro[0], gyro[1], gyro[2], temperature);
+
+        sleep_ms(1000);
+    }
+    return 0;
 }

i2c/mpu6050_i2c/mpu6050_scale_test.cpp

@@ -8,14 +8,16 @@
 #include "mpu6050.hpp"
 #include "pico/stdlib.h"
 
-const int READ_DELAY_MS = 100, READS_PER_SCALE = 50, PRINTS_PER_SCALE = 2;
+const uint8_t READ_DELAY_MS = 100, READS_PER_SCALE = 50, PRINTS_PER_SCALE = 3;
 
 int main() {
     stdio_init_all();
-    float accel[3] = {0}, gyro[3] = {0}; // TODO we shouldn't need this
+    float accel[3] = {0}, gyro[3] = {0};
     MPU6050 *IMU = new MPU6050(accel, gyro);
     IMU->reset();
+    //setting CLKSEL = 1 gets better results than 0 if gyro is running and no sleep mode
     IMU->power(1, false, false, false);
+    IMU->set_timing(6, READ_DELAY_MS - 1); // DLPF = 5 Hz
     int accel_scale = 0, gyro_scale = 2;
     while (true) {
         while (!IMU->is_connected()) {