From f7a3659f7fa05746815e4dc33997b76b8360e0ed Mon Sep 17 00:00:00 2001 From: NirajPatelRobots Date: Thu, 31 Mar 2022 17:27:51 -0400 Subject: [PATCH] mpu6050 i2c example expanded into C/C++ library --- i2c/mpu6050_i2c/CMakeLists.txt | 37 +++- i2c/mpu6050_i2c/mpu6050.cpp | 99 ++++++++++ i2c/mpu6050_i2c/mpu6050.hpp | 51 +++++ i2c/mpu6050_i2c/mpu6050_i2c.c | 236 ++++++++++++++++------- i2c/mpu6050_i2c/mpu6050_i2c.h | 66 +++++++ i2c/mpu6050_i2c/mpu6050_i2c_irq_main.cpp | 95 +++++++++ i2c/mpu6050_i2c/mpu6050_i2c_main.c | 75 +++++++ i2c/mpu6050_i2c/mpu6050_scale_test.cpp | 40 ++++ 8 files changed, 623 insertions(+), 76 deletions(-) create mode 100644 i2c/mpu6050_i2c/mpu6050.cpp create mode 100644 i2c/mpu6050_i2c/mpu6050.hpp create mode 100644 i2c/mpu6050_i2c/mpu6050_i2c.h create mode 100644 i2c/mpu6050_i2c/mpu6050_i2c_irq_main.cpp create mode 100644 i2c/mpu6050_i2c/mpu6050_i2c_main.c create mode 100644 i2c/mpu6050_i2c/mpu6050_scale_test.cpp diff --git a/i2c/mpu6050_i2c/CMakeLists.txt b/i2c/mpu6050_i2c/CMakeLists.txt index 83be3d51e..5ca9adce8 100644 --- a/i2c/mpu6050_i2c/CMakeLists.txt +++ b/i2c/mpu6050_i2c/CMakeLists.txt @@ -1,12 +1,39 @@ -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 and uart output +pico_enable_stdio_usb(mpu6050_i2c 1) +pico_enable_stdio_uart(mpu6050_i2c 1) # add url via pico_set_program_url example_auto_set_url(mpu6050_i2c) + + +add_executable(mpu6050_i2c_scale_test mpu6050_scale_test.cpp) +target_link_libraries(mpu6050_i2c_scale_test MPU6050_i2c_pico_cpp_lib) + +# create map/bin/hex file etc. +pico_add_extra_outputs(mpu6050_i2c_scale_test) +pico_enable_stdio_usb(mpu6050_i2c_scale_test 1) +pico_enable_stdio_uart(mpu6050_i2c_scale_test 1) + + +add_executable(mpu6050_i2c_irq mpu6050_i2c_irq_main.cpp) +target_link_libraries(mpu6050_i2c_irq MPU6050_i2c_pico_cpp_lib) + +# create map/bin/hex file etc. +pico_add_extra_outputs(mpu6050_i2c_irq) +pico_enable_stdio_usb(mpu6050_i2c_irq 1) +pico_enable_stdio_uart(mpu6050_i2c_irq 1) diff --git a/i2c/mpu6050_i2c/mpu6050.cpp b/i2c/mpu6050_i2c/mpu6050.cpp new file mode 100644 index 000000000..f93ef22cf --- /dev/null +++ b/i2c/mpu6050_i2c/mpu6050.cpp @@ -0,0 +1,99 @@ +#include "mpu6050.hpp" +#include "mpu6050_i2c.h" + + +MPU6050SensorTimingParams::MPU6050SensorTimingParams(int _bandwidth, float _delay, float _sample_rate) + : bandwidth(_bandwidth), delay(_delay), sample_rate(_sample_rate) {} + +inline const MPU6050SensorTimingParams convert(mpu6050_timing_params_t *c_struct) { // unfortunate awkwardness of wrapping a C lib + return MPU6050SensorTimingParams(c_struct->bandwidth, c_struct->delay, c_struct->sample_rate); +} + +MPU6050TimingParams::MPU6050TimingParams(uint8_t lowpass_filter_cfg, uint8_t sample_rate_div) { + mpu6050_timing_params_t accel_timing_c, gyro_timing_c; + mpu6050_calc_timing(lowpass_filter_cfg, sample_rate_div, &accel_timing_c, &gyro_timing_c); + accel_timing = convert(&accel_timing_c); + gyro_timing = convert(&gyro_timing_c); +} + +MPU6050TimingParams::MPU6050TimingParams(const MPU6050SensorTimingParams &_accel_timing, + const MPU6050SensorTimingParams &_gyro_timing) + : accel_timing(_accel_timing), gyro_timing(_gyro_timing) {} + + +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(); +} + +void MPU6050::power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle) { + mpu6050_setbusaddr(bus_addr); + mpu6050_power(CLKSEL, temp_disable, sleep, cycle); +} + +void MPU6050::reset(void) { + mpu6050_setbusaddr(bus_addr); + mpu6050_reset(); +} + +void MPU6050::setscale_accel(MPU6050::Scale scale) { + mpu6050_setbusaddr(bus_addr); + mpu6050_setscale_accel((MPU6050_Scale)scale); + accel_scale = scale; +} + +void MPU6050::setscale_gyro(MPU6050::Scale scale) { + mpu6050_setbusaddr(bus_addr); + mpu6050_setscale_gyro((MPU6050_Scale)scale); + gyro_scale = scale; +} + +void MPU6050::read(void) { + mpu6050_setbusaddr(bus_addr); + 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(); +} + +void MPU6050::set_timing(uint8_t lowpass_filter_cfg, uint8_t sample_rate_div) { + mpu6050_setbusaddr(bus_addr); + mpu6050_set_timing(lowpass_filter_cfg, sample_rate_div); +} + +MPU6050TimingParams MPU6050::read_timing(void) { + mpu6050_setbusaddr(bus_addr); + mpu6050_timing_params_t accel_timing_c, gyro_timing_c; + mpu6050_read_timing(&accel_timing_c, &gyro_timing_c); + return MPU6050TimingParams(convert(&accel_timing_c), convert(&gyro_timing_c)); +} + +void MPU6050::configure_interrupt(bool active_low, bool open_drain, bool latch_pin, bool read_clear, bool enable) { + mpu6050_setbusaddr(bus_addr); + mpu6050_configure_interrupt(active_low, open_drain, latch_pin, read_clear, enable); +} + +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]; +} diff --git a/i2c/mpu6050_i2c/mpu6050.hpp b/i2c/mpu6050_i2c/mpu6050.hpp new file mode 100644 index 000000000..9a1709f35 --- /dev/null +++ b/i2c/mpu6050_i2c/mpu6050.hpp @@ -0,0 +1,51 @@ +/* class for using an MPU6050 IMU sensor on pi pico. +A C++ interface to the pico example C code. + */ +#include "stdint.h" + +struct MPU6050SensorTimingParams { + MPU6050SensorTimingParams() = default; + MPU6050SensorTimingParams(int _bandwidth, float _delay, float _sample_rate); + int bandwidth; // lowpass filter bandwidth [Hz] + float delay; // lowpass filter delay [ms] + float sample_rate; // rate of new data loading in the register [Hz] +}; +struct MPU6050TimingParams { + MPU6050TimingParams(uint8_t lowpass_filter_cfg, uint8_t sample_rate_div); + MPU6050TimingParams(const MPU6050SensorTimingParams &_accel_timing, const MPU6050SensorTimingParams &_gyro_timing); + MPU6050SensorTimingParams accel_timing, gyro_timing; +}; + +class MPU6050 { +public: + const float &chip_temp; // [C] + + 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); + void reset(void); + + enum Scale {Scale_0 = 0, Scale_1, Scale_2, Scale_3}; + // Warning: The first call to read() after setscale() might not have the updated scaling. + 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); + void configure_interrupt(bool active_low, // Whether the INT pin is active low or active high + bool open_drain, // Whether the INT pin is push-pull or open-drain + bool latch_pin, // Whether the INT pin latches or pulses for 50us + bool read_clear, // Whether interrupt status bits are cleared by reading interrupt status (default) or on any read + bool enable); // Turn interrupts on or off + uint8_t read_interrupt_status(); // 0 = no interrupts set, 1 = data ready +private: + float *accel, *gyro, temp; + 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); diff --git a/i2c/mpu6050_i2c/mpu6050_i2c.c b/i2c/mpu6050_i2c/mpu6050_i2c.c index 4dd0a9e32..585899d16 100644 --- a/i2c/mpu6050_i2c/mpu6050_i2c.c +++ b/i2c/mpu6050_i2c/mpu6050_i2c.c @@ -4,113 +4,207 @@ * SPDX-License-Identifier: BSD-3-Clause */ -#include -#include -#include "pico/stdlib.h" +#include +#include #include "pico/binary_info.h" +#include "hardware/gpio.h" #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. +// By default these devices are on bus address 0x68 +static uint8_t bus_addr = 0x68; +// Register addresses in the MPU6050 to read and write data to +const uint8_t REG_ACCEL_OUT = 0x3B, REG_GYRO_OUT = 0x43, REG_TEMP_OUT = 0x41, + REG_INT_PIN_CFG = 0x37, REG_INT_ENABLE = 0x38, REG_INT_STATUS = 0x3A, + REG_SMPRT_DIV = 0x19, REG_SIGNAL_PATH_RESET = 0x68, REG_PWR_MGMT_1 = 0x6B, REG_WHO_AM_I = 0x75, + REG_CONFIG = 0x1A, REG_GYRO_CONFIG = 0x1B, REG_ACCEL_CONFIG = 0x1C; - 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. +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_vals[] = {2 * 9.81 / 0x7fff, 4 * 9.81 / 0x7fff, 8 * 9.81 / 0x7fff, 16 * 9.81 / 0x7fff}; - You will need to use a level shifter on the I2C lines if you want to run the - board at 5v. +// 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}; +const float gyro_delay_lookup[] = {0.98, 1.9, 2.8, 4.8, 8.3, 13.4, 18.6}; - 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 int addr = 0x68; +////////////////////////////////////////////////////lower level functions for i2c #ifdef i2c_default -static void mpu6050_reset() { - // Two byte reset. First byte register, second byte data - // There are a load more options to set up the device in different ways that could be added here - uint8_t buf[] = {0x6B, 0x80}; - i2c_write_blocking(i2c_default, addr, buf, 2, false); +void mpu6050_writereg(uint8_t reg, uint8_t value) { + // write one byte to the register reg. Send register byte then data byte. + uint8_t buf[] = {reg, value}; + i2c_write_blocking(i2c_default, bus_addr, buf, 2, false); // False - finished with bus } -static void mpu6050_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp) { +void mpu6050_readreg(uint8_t reg, uint8_t *out, size_t length) { + i2c_write_blocking(i2c_default, bus_addr, ®, 1, true); // true to keep master control of bus + i2c_read_blocking(i2c_default, bus_addr, out, length, false); +} +#else +void mpu6050_writereg(uint8_t reg, uint8_t value) {}; +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) { // 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. + static uint8_t buffer[64]; + if (length > 32) length = 32; + mpu6050_readreg(reg, buffer, length * 2); + for (int i = 0; i < length; i++) { + out[i] = (buffer[i * 2] << 8 | buffer[(i * 2) + 1]); + } +} + +void mpu6050_setbusaddr(uint8_t addr) { + bus_addr = addr; +} - uint8_t buffer[6]; - // Start reading acceleration registers from register 0x3B for 6 bytes - uint8_t val = 0x3B; - i2c_write_blocking(i2c_default, addr, &val, 1, true); // true to keep master control of bus - i2c_read_blocking(i2c_default, addr, buffer, 6, false); +////////////////////////////////////////////////////higher level mpu6050 functions +void mpu6050_power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle) { + 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 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 + sleep_ms(100); +} +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); +} + +void mpu6050_read(float accel[3], float gyro_rad[3], float *temp, MPU6050_Scale accel_scale, MPU6050_Scale gyro_scale) { + // 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); +} +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) { + 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[3], int16_t accel_raw[3], MPU6050_Scale scale) { + for (int i = 0; i < 3; i++) { + 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) { for (int i = 0; i < 3; i++) { - accel[i] = (buffer[i * 2] << 8 | buffer[(i * 2) + 1]); + gyro[i] = gyro_raw[i] * gyro_scale_deg[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; +} - // Now gyro data from reg 0x43 for 6 bytes - // The register is auto incrementing on each read - val = 0x43; - i2c_write_blocking(i2c_default, addr, &val, 1, true); - i2c_read_blocking(i2c_default, addr, buffer, 6, false); // False - finished with bus +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); +} - for (int i = 0; i < 3; i++) { - gyro[i] = (buffer[i * 2] << 8 | buffer[(i * 2) + 1]);; +void mpu6050_set_timing(uint8_t lowpass_filter_cfg, uint8_t sample_rate_div) { + mpu6050_writereg(REG_SMPRT_DIV, sample_rate_div); + mpu6050_writereg(REG_CONFIG, lowpass_filter_cfg & 7); +} +void mpu6050_read_timing(mpu6050_timing_params_t *accel_timing, mpu6050_timing_params_t *gyro_timing) { + uint8_t reg_data[2]; + mpu6050_readreg(REG_SMPRT_DIV, reg_data, 2); + mpu6050_calc_timing(reg_data[1], reg_data[0], accel_timing, gyro_timing); +} +void mpu6050_calc_timing(uint8_t filter_cfg, uint8_t sample_rate_div, + mpu6050_timing_params_t *accel_timing, mpu6050_timing_params_t *gyro_timing) { + int i = filter_cfg > 6 ? 0 : filter_cfg; + float gyro_measure_rate = (i == 0) ? 8000 : 1000; + if (accel_timing) { + accel_timing->bandwidth = accel_bandwidth_lookup[i]; + accel_timing->delay = accel_delay_lookup[i]; + accel_timing->sample_rate = fmin(1000, gyro_measure_rate / (1 + sample_rate_div)); + } + if (gyro_timing) { + gyro_timing->bandwidth = gyro_bandwidth_lookup[i]; + gyro_timing->delay = gyro_delay_lookup[i]; + gyro_timing->sample_rate = gyro_measure_rate / (1 + sample_rate_div); } +} - // Now temperature from reg 0x41 for 2 bytes - // The register is auto incrementing on each read - val = 0x41; - i2c_write_blocking(i2c_default, addr, &val, 1, true); - i2c_read_blocking(i2c_default, addr, buffer, 2, false); // False - finished with bus +void mpu6050_configure_interrupt(bool active_low, bool open_drain, bool latch_pin, bool read_clear, bool enable) { + mpu6050_writereg(REG_INT_PIN_CFG, ((uint8_t)active_low << 7) + ((uint8_t)open_drain << 6) + + ((uint8_t)latch_pin << 5) + ((uint8_t)read_clear << 4)); + mpu6050_writereg(REG_INT_ENABLE, (uint8_t)enable); +} +uint8_t mpu6050_read_interrupt_status() { + uint8_t interrupt_status; + mpu6050_readreg(REG_INT_STATUS, &interrupt_status, 1); + return interrupt_status; +} - *temp = buffer[0] << 8 | buffer[1]; +bool mpu6050_is_connected(void) { + uint8_t who_are_you = 0; + mpu6050_readreg(REG_WHO_AM_I, &who_are_you, 1); + return who_are_you == 0x68; } -#endif -int main() { - 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); + 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); // 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)); - - mpu6050_reset(); - - int16_t acceleration[3], gyro[3], temp; - - while (1) { - mpu6050_read_raw(acceleration, gyro, &temp); - - // These are the raw numbers from the chip, so will need tweaking to be really useful. - // See the datasheet for more information - printf("Acc. X = %d, Y = %d, Z = %d\n", acceleration[0], acceleration[1], acceleration[2]); - printf("Gyro. X = %d, Y = %d, Z = %d\n", gyro[0], gyro[1], gyro[2]); - // Temperature is simple so use the datasheet calculation to get deg C. - // Note this is chip temperature. - printf("Temp. = %f\n", (temp / 340.0) + 36.53); - - sleep_ms(100); - } + return true; #endif } diff --git a/i2c/mpu6050_i2c/mpu6050_i2c.h b/i2c/mpu6050_i2c/mpu6050_i2c.h new file mode 100644 index 000000000..b9cdf4480 --- /dev/null +++ b/i2c/mpu6050_i2c/mpu6050_i2c.h @@ -0,0 +1,66 @@ +/* mpu6050 functions in pico-examples */ +#ifdef __cplusplus +extern "C" { +#endif + +#include "stdint.h" +#include "stddef.h" + +typedef enum MPU6050_Scale {MPU_FS_0 = 0, MPU_FS_1, MPU_FS_2, MPU_FS_3} MPU6050_Scale; + +typedef struct { + int bandwidth; // lowpass filter bandwidth [Hz] + float delay; // lowpass filter delay [ms] + float sample_rate; // rate of new data loading in the register [Hz] +} mpu6050_timing_params_t; + +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. + +/* 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_power(uint8_t CLKSEL, bool temp_disable, bool sleep, bool cycle); +// MPU6050 returns to default settings and enters sleep mode. Includes 200ms of wait. +void mpu6050_reset(); +// Check whether MPU6050 is connected using the read-only WHO_AM_I register, which is always 0x68 +bool mpu6050_is_connected(); +// 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(); + +// configure lowpass filter and sample rate. Higher filter_cfg -> slower change, higher sample_rate_div -> slower sampling +void mpu6050_set_timing(uint8_t lowpass_filter_cfg, uint8_t sample_rate_div); +void mpu6050_read_timing(mpu6050_timing_params_t *accel_timing, mpu6050_timing_params_t *gyro_timing); +void mpu6050_calc_timing(uint8_t filter_cfg, uint8_t sample_rate_div, + mpu6050_timing_params_t *accel_timing, mpu6050_timing_params_t *gyro_timing); + +void mpu6050_configure_interrupt(bool active_low, // Whether the INT pin is active low or active high + bool open_drain, // Whether the INT pin is push-pull or open-drain + bool latch_pin, // Whether the INT pin latches or pulses for 50us + bool read_clear, // Whether interrupt status bits are cleared by reading interrupt status (default) or on any read + bool enable); // Turn interrupts on or off +uint8_t mpu6050_read_interrupt_status(); // 0 = no interrupts set, 1 = data ready + +//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/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 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. +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 +} +#endif diff --git a/i2c/mpu6050_i2c/mpu6050_i2c_irq_main.cpp b/i2c/mpu6050_i2c/mpu6050_i2c_irq_main.cpp new file mode 100644 index 000000000..ce9ec9dbf --- /dev/null +++ b/i2c/mpu6050_i2c/mpu6050_i2c_irq_main.cpp @@ -0,0 +1,95 @@ +/* + * MPU6050 I2C example measuring the interrupt pin and lowpass filter. + */ + +#include +#include +#include "pico/stdlib.h" +#include "pico/float.h" +#include "pico/binary_info.h" +#include "hardware/gpio.h" +#include "mpu6050.hpp" + +const bool USE_READ_CLEAR = true; // true => fewer reads required, eliminates a failure mode (forgetting to clear irq) +const unsigned READ_TIME_MS = 8000; +const uint8_t IRQ_PIN = 6; +const uint8_t SAMPLE_RATE_DIVS[] = {99, 9}; +const uint8_t LOWPASS_FILTER_CFGS[] = {5, 1}; +volatile bool got_irq; + +static std::unique_ptr IMU; +static float accel[3] = {0}, gyro[3] = {0}; + +void print_SensorTimingParams(const MPU6050SensorTimingParams ¶ms) { + printf("Sample Rate: %.2f, Bandwidth: %i, Delay: %.1f\n", params.sample_rate, params.bandwidth, params.delay); +} + +void print_TimingParams(const MPU6050TimingParams ¶ms) { + printf("Accelerometer "); + print_SensorTimingParams(params.accel_timing); + printf("Gyroscope "); + print_SensorTimingParams(params.gyro_timing); +} + +void irq_callback(uint gpio, uint32_t events) { + static unsigned num_reads = 0, sample_rate_div_idx = 0, lowpass_filter_cfg_idx = 0; + static float last_accel[3] = {0}, last_gyro[3] = {0}; + static uint64_t last_time_us = 0; + static float avg_change_acc = 0, avg_change_gyro = 0; + got_irq = true; + if (!USE_READ_CLEAR) { + IMU->read_interrupt_status(); + } + IMU->read(); + float change_acc = 0, change_gyro = 0; + for (int i = 0; i < 3; i++) { + change_acc += (accel[i] - last_accel[i]) * (accel[i] - last_accel[i]); + change_gyro += (gyro[i] - last_gyro[i]) * (gyro[i] - last_gyro[i]); + last_accel[i] = accel[i]; + last_gyro[i] = gyro[i]; + } + avg_change_acc += sqrtf(change_acc); + avg_change_gyro += sqrtf(change_gyro); + if (++num_reads > READ_TIME_MS / (SAMPLE_RATE_DIVS[sample_rate_div_idx] + 1)) { + uint64_t time_us = to_us_since_boot(get_absolute_time()); + printf("Measured cycle rate: %f Hz\n", (1e6 * num_reads) / (time_us - last_time_us)); + last_time_us = time_us; + printf("Average change between readings:\n\t accel: %f mm/s2 gyro: %f mrad/s\n\n", + 1000 * avg_change_acc / num_reads, 1000 * avg_change_gyro / num_reads); + avg_change_acc = 0; + avg_change_gyro = 0; + num_reads = 0; + if (lowpass_filter_cfg_idx == 1) + sample_rate_div_idx = 1 - sample_rate_div_idx; + lowpass_filter_cfg_idx = 1 - lowpass_filter_cfg_idx; + IMU->set_timing(LOWPASS_FILTER_CFGS[lowpass_filter_cfg_idx], SAMPLE_RATE_DIVS[sample_rate_div_idx]); + print_TimingParams(IMU->read_timing()); + } +} + +int main() { + stdio_init_all(); + bi_decl(bi_1pin_with_name(IRQ_PIN, "IMU IRQ pin 1")); + IMU = std::make_unique(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); + IMU->set_timing(LOWPASS_FILTER_CFGS[0], SAMPLE_RATE_DIVS[0]); + print_TimingParams(IMU->read_timing()); + gpio_set_irq_enabled_with_callback(IRQ_PIN, GPIO_IRQ_LEVEL_HIGH, true, &irq_callback); + while (true) { + got_irq = false; + sleep_ms(3000); + if (!IMU->is_connected()) { + 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]); + } + } +} diff --git a/i2c/mpu6050_i2c/mpu6050_i2c_main.c b/i2c/mpu6050_i2c/mpu6050_i2c_main.c new file mode 100644 index 000000000..fe73676d1 --- /dev/null +++ b/i2c/mpu6050_i2c/mpu6050_i2c_main.c @@ -0,0 +1,75 @@ +/** + 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 +#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"); + } + 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; +} diff --git a/i2c/mpu6050_i2c/mpu6050_scale_test.cpp b/i2c/mpu6050_i2c/mpu6050_scale_test.cpp new file mode 100644 index 000000000..8dd8df088 --- /dev/null +++ b/i2c/mpu6050_i2c/mpu6050_scale_test.cpp @@ -0,0 +1,40 @@ +/** + * mpu6050 I2c C++ example demonstrating the mpu6050's scaling feature + * Scaled Accelerometer and Gyroscope readings + * Should be roughly equal before and after scale changes + */ + +#include +#include "mpu6050.hpp" +#include "pico/stdlib.h" + +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}; + 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()) { + printf("MPU6050 is not connected...\n"); + sleep_ms(1000); + } + printf("\nAccelerometer scale: %d, Gyroscope scale: %d\n", accel_scale, gyro_scale); + for (int i = 0; i < READS_PER_SCALE; i++) { + IMU->read(); + printf("%+6f %+6f %+6f | %+6f %+6f %+6f\r", + accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]); + if (i < PRINTS_PER_SCALE - 1) { printf("\n"); } + sleep_ms(READ_DELAY_MS); + } + accel_scale = (accel_scale + 1) % 4; + gyro_scale = (gyro_scale + 1) % 4; + IMU->setscale_accel((MPU6050::Scale)(accel_scale)); + IMU->setscale_gyro((MPU6050::Scale)(gyro_scale)); + } +}