split mpu6050 lib (c and c++) from demo main, refactor mostly comments

the code that's been running with simplewalker. Also add self-test even though idk what it does.

Author: NirajPatelRobots

i2c/mpu6050_i2c/CMakeLists.txt

@@ -1,12 +1,21 @@
-add_executable(mpu6050_i2c
-        mpu6050_i2c.c
-        )
-
+add_library(MPU6050_i2c_pico_lib mpu6050_i2c.c)
+target_include_directories(MPU6050_i2c_pico_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 # pull in common dependencies and additional i2c hardware support
-target_link_libraries(mpu6050_i2c pico_stdlib hardware_i2c)
+target_link_libraries(MPU6050_i2c_pico_lib pico_stdlib hardware_i2c)
+
+add_library(MPU6050_i2c_pico_cpp_lib mpu6050.cpp mpu6050_i2c.c)
+target_include_directories(MPU6050_i2c_pico_cpp_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(MPU6050_i2c_pico_cpp_lib pico_stdlib hardware_i2c)
+
+
+add_executable(mpu6050_i2c mpu6050_i2c_main.c)
+target_link_libraries(mpu6050_i2c MPU6050_i2c_pico_lib)
 
 # create map/bin/hex file etc.
 pico_add_extra_outputs(mpu6050_i2c)
+# enable usb output, disable uart output
+pico_enable_stdio_usb(mpu6050_i2c 1)
+pico_enable_stdio_uart(mpu6050_i2c 0)
 
 # add url via pico_set_program_url
 example_auto_set_url(mpu6050_i2c)

i2c/mpu6050_i2c/mpu6050.cpp

@@ -3,7 +3,7 @@
 #include "hardware/i2c.h"
 #include "mpu6050_i2c.h"
 
-/*MPU6050::MPU6050() :  //TODO: is there a good way to do this
+/*MPU6050::MPU6050() :  //TODO: smart pointers
 chip_temp(*temp) {
     accel = {0., 0., 0.};
     gyro = {0., 0., 0.};
@@ -15,12 +15,7 @@ MPU6050::MPU6050(float *accel_out, float *gyro_out) :
  accel(accel_out),  gyro(gyro_out), chip_temp(temp) {
     accel_scale = 0;
     gyro_scale = 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);
+    setup_MPU6050_i2c();
 }
 
 void MPU6050::power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle) {

i2c/mpu6050_i2c/mpu6050.hpp

@@ -1,17 +1,24 @@
 /* class for using an MPU6050 IMU sensor on pi pico.
-An abstraction of pico example C code. */
+An abstraction of pico example C code. 
+TODO:
+    smart ptrs for results
+    C++ scale enums
+    mpu6050_t struct in C code for storing scales and bus addr
+ */
 #include "pico/stdlib.h"
 
 class MPU6050 {
     float *accel, *gyro, temp;
     uint8_t accel_scale, gyro_scale;
 public:
-    //const float *accel, *gyro, &chip_temp; // [m/s^2], [rad/s], [C]
-    const float &chip_temp;
-    MPU6050(void);
-    MPU6050(float *accel_out, float *gyro_out);
+    const float &chip_temp; // [C]
+
+    MPU6050(); //TODO
+    MPU6050(float *accel_out, float *gyro_out); // [m/s^2], [rad/s]
+
     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

@@ -19,10 +19,8 @@
    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 therefore 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.
+   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.
 
@@ -41,7 +39,7 @@ const uint8_t REG_PWR_MGMT_1 = 0x6B, REG_ACCEL_OUT = 0x3B, REG_GYRO_OUT = 0x43,
 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};
-const float accel_scale[] = {2 * 9.81 / 0x7fff, 4 * 9.81 / 0x7fff, 8 * 9.81 / 0x7fff, 16 * 9.81 / 0x7fff};
+const float accel_scale_vals[] = {2 * 9.81 / 0x7fff, 4 * 9.81 / 0x7fff, 8 * 9.81 / 0x7fff, 16 * 9.81 / 0x7fff};
 
 
 ////////////////////////////////////////////////////lower level functions for i2c
@@ -53,7 +51,6 @@ void mpu6050_writereg(uint8_t reg, uint8_t value) {
 }
 
 void mpu6050_readreg(uint8_t reg, uint8_t *out, size_t length) {
-    //read length 8-bit integers from the register at reg, store them in out.
     i2c_write_blocking(i2c_default, bus_addr, &reg, 1, true); // true to keep master control of bus
     i2c_read_blocking(i2c_default, bus_addr, out, length, false);
 }
@@ -63,7 +60,6 @@ void mpu6050_readreg(uint8_t reg, uint8_t *out, size_t length) {};
 #endif
 
 void mpu6050_readreg16(uint8_t reg, int16_t *out, size_t length) {
-    //read length 16-bit from the register at reg, store them in out. Max length 32 (= 64 bytes)
     // For this particular device, we send the device the register we want to read
     // first, then subsequently read from the device. The register is auto incrementing
     // so we don't need to keep sending the register we want, just the first.
@@ -82,16 +78,13 @@ void mpu6050_setbusaddr(uint8_t addr) {
 
 ////////////////////////////////////////////////////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. */
     uint8_t pwrval = (CLKSEL & 7) + ((uint8_t)temp_disable << 3)
                     + ((uint8_t)sleep << 6) + ((uint8_t)cycle << 5);
     mpu6050_writereg(REG_PWR_MGMT_1, pwrval);
 }
 
 void mpu6050_reset() {
-    // Reset power management and signal path registers. MPU6050 returns to default settings.
-    // Reset instructions come from register map doc. Includes 200ms of wait.
+    // Reset instructions come from register map doc
     mpu6050_writereg(REG_PWR_MGMT_1, 1 << 7); //DEVICE_RESET = 1
     sleep_ms(100);
     mpu6050_writereg(REG_SIGNAL_PATH_RESET, 7); //GYRO_RESET = ACCEL_RESET = TEMP_RESET = 1
@@ -105,14 +98,15 @@ void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp) {
 }
 
 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
+    // reads acceleration [m/s], gyroscope [rad], and temperature [C] in one i2c interaction 
+    int16_t buffer[7]; // 3 accel + 3 gyro + scalar temp
+    mpu6050_readreg16(REG_ACCEL_OUT, buffer, 7);
     mpu6050_scale_accel(accel, buffer, accel_scale);
     if (temp) *temp = mpu6050_scale_temp(buffer[3]);
     mpu6050_scale_gyro_rad(gyro_rad, buffer + 4, gyro_scale);
 }
 
-// set and use sensitivity
+// functions to set and calculate with sensitivity
 void mpu6050_setscale_gyro(MPU6050_Scale gyro_scale) {
     mpu6050_writereg(REG_GYRO_CONFIG, (uint8_t)gyro_scale << 3);
 }
@@ -121,7 +115,7 @@ void mpu6050_setscale_accel(MPU6050_Scale accel_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];
+        accel[i] = accel_raw[i] * accel_scale_vals[scale];
     }
 }
 void mpu6050_scale_gyro_deg(float gyro[3], int16_t gyro_raw[3], MPU6050_Scale scale) {
@@ -137,21 +131,25 @@ void mpu6050_scale_gyro_rad(float gyro[3], int16_t gyro_raw[3], MPU6050_Scale sc
 float mpu6050_scale_temp(float temp_raw) {
     return (temp_raw / 340.0) + 36.53;
 }
+void mpu6050_gyro_selftest_on(void) {
+    uint8_t regdata = 0b11100000 & ((uint8_t)MPU_FS_0 << 3);
+    mpu6050_writereg(REG_GYRO_CONFIG, regdata);
+}
+void mpu6050_accel_selftest_on(void) {
+    uint8_t regdata = 0b11100000 & ((uint8_t)MPU_FS_2 << 3);
+    mpu6050_writereg(REG_ACCEL_CONFIG, regdata);
+}
 
 //TODO: set timing
 //TODO: set and read filter cutoff
-//TODO: read self test
+//TODO: interrupts
 
-//TODO: functional mpu test
-int test() {
-    stdio_init_all();
+bool setup_MPU6050_i2c() {
 #if !defined(i2c_default) || !defined(PICO_DEFAULT_I2C_SDA_PIN) || !defined(PICO_DEFAULT_I2C_SCL_PIN)
     #warning i2c/mpu6050_i2c example requires a board with I2C pins
     puts("Default I2C pins were not defined");
-    return 0;
+    return false;
 #else
-    printf("Hello, MPU6050! Reading raw data from registers...\n");
-
     // This example will use I2C0 on the default SDA and SCL pins (4, 5 on a Pico)
     i2c_init(i2c_default, 400 * 1000); // Max bus speed 400 kHz
     gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
@@ -160,33 +158,39 @@ int test() {
     gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
     // Make the I2C pins available to picotool
     bi_decl(bi_2pins_with_func(PICO_DEFAULT_I2C_SDA_PIN, PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C));
+    return true;
+#endif
+}
 
-    mpu6050_reset();
-    //setting CLKSEL = 1 gets better results than 0 if gyro is running and no sleep mode
-    mpu6050_power(1, false, false, false);
+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);
+
     while (1) {
+        uint64_t time_us = to_us_since_boot(get_absolute_time());
         mpu6050_read_raw(accel_raw, gyro_raw, &temp_raw);
-        //TODO time read function
+        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]);
-        // 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", mpu6050_scale_temp(temp_raw), testscale);
+        // This is chip temperature.
+        printf("Temp [C] = %f\n", mpu6050_scale_temp(temp_raw));
+        printf("Read time: %d us; \t\t Accel and Gyro Scale = %d\n", time_us, 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);
     }
-#endif
+    return 0;
 }

i2c/mpu6050_i2c/mpu6050_i2c.h

@@ -11,15 +11,23 @@ typedef enum MPU6050_Scale {MPU_FS_0, MPU_FS_1, MPU_FS_2, MPU_FS_3} MPU6050_Scal
 
 // 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);
 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.
+/* 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.
+void mpu6050_power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle);
+// Reset power management and signal path registers. MPU6050 returns to default settings. Includes 200ms of wait.
+void mpu6050_reset();
+// turn on the mpu6050's accelerometer self test. Turn it off after a few 100ms with mpu6050_setscale_accel
+void mpu6050_accel_selftest_on();  // TODO: find out what "self test" does
+// turn on the mpu6050's gyroscope self test. Turn it off after a few 100ms with mpu6050_setscale_gyro
+void mpu6050_gyro_selftest_on();
 //set and use scaling
 void mpu6050_setscale_accel(MPU6050_Scale accel_scale);
 void mpu6050_setscale_gyro(MPU6050_Scale gyro_scale);
@@ -28,9 +36,13 @@ void mpu6050_scale_gyro_deg(float gyro[3], int16_t gyro_raw[3], MPU6050_Scale sc
 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.
+// 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,
-                  MPU6050_Scale accel_scale, MPU6050_Scale gyro_scale); //reads all at same timestep, converts. temp can be NULL.
+                  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();
 
 #ifdef __cplusplus
 }

i2c/mpu6050_i2c/mpu6050_i2c_main.c

@@ -0,0 +1,16 @@
+/**
+ * Boilerplate main for mpu6050 I2c example
+ */
+
+#include <stdio.h>
+#include "pico/stdlib.h"
+#include "mpu6050_i2c.h"
+
+
+int main() {
+    stdio_init_all();
+    if (setup_MPU6050_i2c()) {
+        printf("Hello, MPU6050! Reading raw data from registers...\n");
+    }
+    return run_MPU6050_demo();
+}