mpu_dmp_example_1.cpp

#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include "driver/gpio.h"
#include "driver/i2c.h"
#include "driver/spi_master.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "sdkconfig.h"

#include "MPU.hpp"
#include "MPUdmp.hpp"
#include "dmp/types.hpp"
#include "mpu/math.hpp"
#include "mpu/types.hpp"

/* Bus configuration */
#if defined CONFIG_MPU_I2C
#include "I2Cbus.hpp"
static I2C_t& i2c                     = i2c0;
static constexpr gpio_num_t SDA       = GPIO_NUM_14;
static constexpr gpio_num_t SCL       = GPIO_NUM_26;
static constexpr uint32_t CLOCK_SPEED = 400000;  // 400 KHz
#elif defined CONFIG_MPU_SPI
#include "SPIbus.hpp"
static SPI_t& spi                     = hspi;
static constexpr int MOSI             = 22;
static constexpr int MISO             = 21;
static constexpr int SCLK             = 23;
static constexpr int CS               = 16;
static constexpr uint32_t CLOCK_SPEED = 1000000;  // 1MHz
#endif

/* MPU configuration */
static constexpr int kInterruptPin      = 17;   // GPIO for Interrupt pin
static constexpr uint8_t kDMPOutputRate = 100;  // Data Reading rate in Hz, max: 200
static constexpr int kDataPrintRate     = 20;   // Data Print rate in Hz

//====

static const char* TAG = "APP";
static void IRAM_ATTR mpuISR(TaskHandle_t);
static void mpuTask(void*);
static void printTask(void*);

// Main
extern "C" void app_main()
{
    printf(">> DMP test\n");
    fflush(stdout);

#if defined CONFIG_MPU_I2C
    i2c.begin(SDA, SCL, CLOCK_SPEED);
#elif defined CONFIG_MPU_SPI
    spi.begin(MOSI, MISO, SCLK);
#endif
    // Create a task to setup mpu and read sensor data
    xTaskCreate(mpuTask, "mpuTask", 4 * 1024, nullptr, 6, nullptr);
    // Create a task to print data
    xTaskCreate(printTask, "printTask", 2 * 1024, nullptr, 5, nullptr);
}

/* MPU data */

MPUdmp_t mpu;
mpud::quat_t quat;
mpud::float_axes_t euler;
mpud::float_axes_t accelG, gyroDPS;

/* Tasks */

static void mpuTask(void*)
{
// Setup Bus
#if defined CONFIG_MPU_I2C
    mpu.setBus(i2c);
    mpu.setAddr(mpud::MPU_I2CADDRESS_AD0_LOW);
#elif defined CONFIG_MPU_SPI
    mpu.setBus(spi);
    spi_device_handle_t mpu_spi_handle;
    spi.addDevice(0, CLOCK_SPEED, CS, &mpu_spi_handle);
    mpu.setAddr(mpu_spi_handle);
#endif

    // Verify Connection
    while (esp_err_t err = mpu.testConnection()) {
        ESP_LOGE(TAG, "Failed to connect to the MPU, error=%#X", err);
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
    ESP_LOGI(TAG, "MPU connection successful!");

    // Initialize DMP
    const mpud::dmp_feature_t kDMPFeatures =
        (mpud::DMP_FEATURE_LP_6X_QUAT | mpud::DMP_FEATURE_GYRO_CAL | mpud::DMP_FEATURE_TAP |
         mpud::DMP_FEATURE_SEND_CAL_GYRO | mpud::DMP_FEATURE_SEND_RAW_ACCEL);
    ESP_ERROR_CHECK(mpu.initialize());
    ESP_ERROR_CHECK(mpu.loadDMPFirmware());
    ESP_ERROR_CHECK(mpu.setDMPFeatures(kDMPFeatures));
    ESP_ERROR_CHECK(mpu.setDMPOutputRate(kDMPOutputRate));
    ESP_ERROR_CHECK(mpu.setDMPInterruptMode(mpud::DMP_INT_MODE_CONTINUOUS));

    // Setup GPIO for Interrupt pin
    const gpio_config_t kGPIOConf{
        .pin_bit_mask = (uint64_t) 1 << kInterruptPin,
        .mode         = GPIO_MODE_INPUT,
        .pull_up_en   = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type    = GPIO_INTR_POSEDGE  //
    };
    ESP_ERROR_CHECK(gpio_config(&kGPIOConf));
    ESP_ERROR_CHECK(gpio_install_isr_service(ESP_INTR_FLAG_IRAM));
    ESP_ERROR_CHECK(gpio_isr_handler_add((gpio_num_t) kInterruptPin, mpuISR, xTaskGetCurrentTaskHandle()));

    // Activate DMP and Interrupt
    ESP_ERROR_CHECK(mpu.enableDMP());
    ESP_ERROR_CHECK(mpu.setInterruptEnabled(mpud::INT_EN_DMP_READY));

    // Start clean
    mpu.getInterruptStatus();
    ESP_ERROR_CHECK(mpu.resetFIFO());
    const uint8_t kDMPPacketLength = mpu.getDMPPacketLength();
    ESP_LOGI(TAG, "kDMPPacketLength = %d", kDMPPacketLength);

    // Reading Loop
    while (true) {

        // Wait for notification from mpuISR
        uint32_t notifyCount = ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
        if (notifyCount > 1) {
            ESP_LOGW(TAG, "Sample Rate too high! Task Notification Count: %d", notifyCount);
            mpu.resetFIFO();
            continue;
        }

        // Check FIFO count
        uint16_t fifocount = mpu.getFIFOCount();
        if (esp_err_t err = mpu.lastError()) {
            ESP_LOGE(TAG, "Error reading fifo count, %#X", err);
            mpu.resetFIFO();
            continue;
        }
        if (fifocount > kDMPPacketLength) {
            if ((fifocount % kDMPPacketLength) == 0) {
                ESP_LOGE(TAG, "Sample Rate too high!, FIFO count: %d", fifocount);
            }
            else {
                ESP_LOGE(TAG, "FIFO Count misaligned! Expected: %d, Actual: %d", kDMPPacketLength, fifocount);
            }
            mpu.resetFIFO();
            continue;
        }
        else if (fifocount < kDMPPacketLength) {
            if (fifocount > 0) {
                ESP_LOGE(TAG, "FIFO Count misaligned! Expected: %d, Actual: %d", kDMPPacketLength, fifocount);
                mpu.resetFIFO();
                continue;
            }
        }

        // Read DMP Packet
        mpud::quat_q30_t quatRaw;
        mpud::raw_axes_t accelRaw, gyroRaw;
        if (esp_err_t err = mpu.readDMPPacket(&quatRaw, &gyroRaw, &accelRaw)) {
            ESP_LOGE(TAG, "Error reading sensor data, %#X", err);
            mpu.resetFIFO();
            continue;
        }

        // Convert Data
        mpud::axes_q16_t eulerRaw = mpud::quaternionToEuler(quatRaw);
        quat                      = mpud::q30_to_float(quatRaw);
        euler                     = mpud::q16_to_float(eulerRaw);
        accelG                    = mpud::accelGravity(accelRaw, mpud::ACCEL_FS_2G);
        gyroDPS                   = mpud::gyroDegPerSec(gyroRaw, mpud::GYRO_FS_2000DPS);
    }
}

static void printTask(void*)
{
    vTaskDelay(1000 / portTICK_PERIOD_MS);
    while (true) {
        printf("euler: [%+7.2f %+7.2f %+7.2f ](º/s)    ", euler.x, euler.y, euler.z);
        printf("quat: [%+.3f %+.3f %+.3f %+.3f ]    ", quat.w, quat.x, quat.y, quat.z);
        // printf("accel: [%+6.2f %+6.2f %+6.2f ](G)    ", accelG.x, accelG.y, accelG.z);
        // printf("gyro: [%+7.2f %+7.2f %+7.2f ](º/s)    ", gyroDPS.x, gyroDPS.y, gyroDPS.z);
        printf("\n");
        vTaskDelay(1000 / kDataPrintRate / portTICK_PERIOD_MS);
    }
}

static void IRAM_ATTR mpuISR(TaskHandle_t taskHandle)
{
    BaseType_t HPTaskWoken = pdFALSE;
    vTaskNotifyGiveFromISR(taskHandle, &HPTaskWoken);
    if (HPTaskWoken == pdTRUE) portYIELD_FROM_ISR();
}