C++ interrupts and interrupt example

Author: NirajPatelRobots

i2c/mpu6050_i2c/CMakeLists.txt

@@ -29,3 +29,13 @@ pico_add_extra_outputs(mpu6050_i2c_scale_test)
 # enable usb output, disable uart output
 pico_enable_stdio_usb(mpu6050_i2c_scale_test 1)
 pico_enable_stdio_uart(mpu6050_i2c_scale_test 0)
+
+
+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)
+# enable usb output, disable uart output
+pico_enable_stdio_usb(mpu6050_i2c_irq 1)
+pico_enable_stdio_uart(mpu6050_i2c_irq 0)

i2c/mpu6050_i2c/mpu6050.cpp

@@ -72,9 +72,18 @@ void MPU6050::set_timing(uint8_t lowpass_filter_cfg, uint8_t 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();
+}

i2c/mpu6050_i2c/mpu6050.hpp

@@ -36,6 +36,12 @@ class MPU6050 {
     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;

i2c/mpu6050_i2c/mpu6050_i2c_irq_main.cpp

@@ -0,0 +1,90 @@
+/*
+ * MPU6050 I2C example measuring the interrupt pin and lowpass filter.
+ */
+
+#include <stdio.h>
+#include <memory>
+#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<MPU6050> IMU;
+static float accel[3] = {0}, gyro[3] = {0}; // TODO we shouldn't need this
+
+void print_SensorTimingParams(const MPU6050SensorTimingParams &params) {
+    printf("Sample Rate: %.2f, Bandwidth: %i, Delay: %.1f\n", params.sample_rate, params.bandwidth, params.delay);
+}
+
+void print_TimingParams(const MPU6050TimingParams &params) {
+    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<MPU6050>(accel, gyro);
+    IMU->reset();
+    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");
+        }
+    }
+}