|
| 1 | +// ========================================================================= |
| 2 | +// Released under the MIT License |
| 3 | +// Copyright 2017-2018 Natanael Josue Rabello. All rights reserved. |
| 4 | +// For the license information refer to LICENSE file in root directory. |
| 5 | +// ========================================================================= |
| 6 | + |
| 7 | +/** |
| 8 | + * @file mpu_real.cpp |
| 9 | + * A more 'elaborated' example. Shows how to: |
| 10 | + * - Use either SPI or I2C in the same code |
| 11 | + * - Use the MPU with interrupt signal |
| 12 | + * - Read sensor data from FIFO |
| 13 | + * - Perform Self-Test check |
| 14 | + * - Calibrate sensor data output using offset registers |
| 15 | + * - Calculate Tilt Angles |
| 16 | + * |
| 17 | + * @note |
| 18 | + * To try this example: \n |
| 19 | + * Set the I2C/SPI pins in 'Bus configuration' and the interrupt pin in 'MPU configuration'. |
| 20 | + * |
| 21 | + * @todo Document the steps |
| 22 | + */ |
| 23 | + |
| 24 | +#include <math.h> |
| 25 | +#include <stdint.h> |
| 26 | +#include <stdio.h> |
| 27 | +#include <stdlib.h> |
| 28 | + |
| 29 | +#include "driver/gpio.h" |
| 30 | +#include "driver/i2c.h" |
| 31 | +#include "driver/spi_master.h" |
| 32 | +#include "esp_err.h" |
| 33 | +#include "esp_log.h" |
| 34 | +#include "freertos/FreeRTOS.h" |
| 35 | +#include "freertos/portmacro.h" |
| 36 | +#include "freertos/task.h" |
| 37 | +#include "sdkconfig.h" |
| 38 | + |
| 39 | +#include "MPU.hpp" |
| 40 | +#include "mpu/math.hpp" |
| 41 | +#include "mpu/types.hpp" |
| 42 | + |
| 43 | +/* Bus configuration */ |
| 44 | + |
| 45 | +// This MACROS are defined in "skdconfig.h" and set through 'menuconfig'. |
| 46 | +// Can use to check which protocol has been selected. |
| 47 | +#if defined CONFIG_MPU_I2C |
| 48 | +#include "I2Cbus.hpp" |
| 49 | +static I2C_t& i2c = i2c0; // i2c0 or i2c1 |
| 50 | +static constexpr gpio_num_t SDA = GPIO_NUM_14; |
| 51 | +static constexpr gpio_num_t SCL = GPIO_NUM_26; |
| 52 | +static constexpr uint32_t CLOCK_SPEED = 400000; // 400 KHz |
| 53 | +#elif defined CONFIG_MPU_SPI |
| 54 | +#include "SPIbus.hpp" |
| 55 | +static SPI_t& spi = hspi; // hspi or vspi |
| 56 | +static constexpr int MOSI = 22; |
| 57 | +static constexpr int MISO = 21; |
| 58 | +static constexpr int SCLK = 23; |
| 59 | +static constexpr int CS = 16; |
| 60 | +static constexpr uint32_t CLOCK_SPEED = 1000000; // 1MHz |
| 61 | +#endif |
| 62 | + |
| 63 | +/* MPU configuration */ |
| 64 | + |
| 65 | +static constexpr int kInterruptPin = 17; // GPIO_NUM |
| 66 | +static constexpr uint16_t kSampleRate = 250; // Hz |
| 67 | +static constexpr mpud::accel_fs_t kAccelFS = mpud::ACCEL_FS_4G; |
| 68 | +static constexpr mpud::gyro_fs_t kGyroFS = mpud::GYRO_FS_500DPS; |
| 69 | +static constexpr mpud::dlpf_t kDLPF = mpud::DLPF_98HZ; |
| 70 | +static constexpr mpud::int_config_t kInterruptConfig{ |
| 71 | + .level = mpud::INT_LVL_ACTIVE_HIGH, |
| 72 | + .drive = mpud::INT_DRV_PUSHPULL, |
| 73 | + .mode = mpud::INT_MODE_PULSE50US, |
| 74 | + .clear = mpud::INT_CLEAR_STATUS_REG // |
| 75 | +}; |
| 76 | + |
| 77 | +/*-*/ |
| 78 | + |
| 79 | +static const char* TAG = "example"; |
| 80 | + |
| 81 | +static void mpuISR(void*); |
| 82 | +static void mpuTask(void*); |
| 83 | +static void printTask(void*); |
| 84 | + |
| 85 | +// Main |
| 86 | +extern "C" void app_main() |
| 87 | +{ |
| 88 | + printf("$ MPU Driver Example: Advanced\n"); |
| 89 | + fflush(stdout); |
| 90 | + // Initialize bus through either the Library API or esp-idf API |
| 91 | +#if defined CONFIG_MPU_I2C |
| 92 | + i2c.begin(SDA, SCL, CLOCK_SPEED); |
| 93 | +#elif defined CONFIG_MPU_SPI |
| 94 | + spi.begin(MOSI, MISO, SCLK); |
| 95 | +#endif |
| 96 | + // Create a task to setup mpu and read sensor data |
| 97 | + xTaskCreate(mpuTask, "mpuTask", 4 * 1024, nullptr, 6, nullptr); |
| 98 | + // Create a task to print angles |
| 99 | + xTaskCreate(printTask, "printTask", 2 * 1024, nullptr, 5, nullptr); |
| 100 | +} |
| 101 | + |
| 102 | +/* Tasks */ |
| 103 | + |
| 104 | +static MPU_t MPU; |
| 105 | +float roll{0}, pitch{0}, yaw{0}; |
| 106 | + |
| 107 | +static void mpuTask(void*) |
| 108 | +{ |
| 109 | +// Let MPU know which bus and address to use |
| 110 | +#if defined CONFIG_MPU_I2C |
| 111 | + MPU.setBus(i2c); |
| 112 | + MPU.setAddr(mpud::MPU_I2CADDRESS_AD0_LOW); |
| 113 | +#elif defined CONFIG_MPU_SPI |
| 114 | + MPU.setBus(spi); |
| 115 | + spi_device_handle_t mpu_spi_handle; |
| 116 | + spi.addDevice(0, CLOCK_SPEED, CS, &mpu_spi_handle); |
| 117 | + MPU.setAddr(mpu_spi_handle); |
| 118 | +#endif |
| 119 | + |
| 120 | + // Verify connection |
| 121 | + while (esp_err_t err = MPU.testConnection()) { |
| 122 | + ESP_LOGE(TAG, "Failed to connect to the MPU, error=%#X", err); |
| 123 | + vTaskDelay(1000 / portTICK_PERIOD_MS); |
| 124 | + } |
| 125 | + ESP_LOGI(TAG, "MPU connection successful!"); |
| 126 | + |
| 127 | + // Initialize |
| 128 | + ESP_ERROR_CHECK(MPU.initialize()); |
| 129 | + |
| 130 | + // Self-Test |
| 131 | + mpud::selftest_t retSelfTest; |
| 132 | + while (esp_err_t err = MPU.selfTest(&retSelfTest)) { |
| 133 | + ESP_LOGE(TAG, "Failed to perform MPU Self-Test, error=%#X", err); |
| 134 | + vTaskDelay(1000 / portTICK_PERIOD_MS); |
| 135 | + } |
| 136 | + ESP_LOGI(TAG, "MPU Self-Test result: Gyro=%s Accel=%s", // |
| 137 | + (retSelfTest & mpud::SELF_TEST_GYRO_FAIL ? "FAIL" : "OK"), |
| 138 | + (retSelfTest & mpud::SELF_TEST_ACCEL_FAIL ? "FAIL" : "OK")); |
| 139 | + |
| 140 | + // Calibrate |
| 141 | + mpud::raw_axes_t accelBias, gyroBias; |
| 142 | + ESP_ERROR_CHECK(MPU.computeOffsets(&accelBias, &gyroBias)); |
| 143 | + ESP_ERROR_CHECK(MPU.setAccelOffset(accelBias)); |
| 144 | + ESP_ERROR_CHECK(MPU.setGyroOffset(gyroBias)); |
| 145 | + |
| 146 | + // Configure |
| 147 | + ESP_ERROR_CHECK(MPU.setAccelFullScale(kAccelFS)); |
| 148 | + ESP_ERROR_CHECK(MPU.setGyroFullScale(kGyroFS)); |
| 149 | + ESP_ERROR_CHECK(MPU.setSampleRate(kSampleRate)); |
| 150 | + ESP_ERROR_CHECK(MPU.setDigitalLowPassFilter(kDLPF)); |
| 151 | + |
| 152 | + // Setup FIFO |
| 153 | + ESP_ERROR_CHECK(MPU.setFIFOConfig(mpud::FIFO_CFG_ACCEL | mpud::FIFO_CFG_GYRO)); |
| 154 | + ESP_ERROR_CHECK(MPU.setFIFOEnabled(true)); |
| 155 | + constexpr uint16_t kFIFOPacketSize = 12; |
| 156 | + |
| 157 | + // Setup Interrupt |
| 158 | + constexpr gpio_config_t kGPIOConfig{ |
| 159 | + .pin_bit_mask = (uint64_t) 0x1 << kInterruptPin, |
| 160 | + .mode = GPIO_MODE_INPUT, |
| 161 | + .pull_up_en = GPIO_PULLUP_DISABLE, |
| 162 | + .pull_down_en = GPIO_PULLDOWN_ENABLE, |
| 163 | + .intr_type = GPIO_INTR_POSEDGE // |
| 164 | + }; |
| 165 | + gpio_config(&kGPIOConfig); |
| 166 | + gpio_install_isr_service(ESP_INTR_FLAG_IRAM); |
| 167 | + gpio_isr_handler_add((gpio_num_t) kInterruptPin, mpuISR, xTaskGetCurrentTaskHandle()); |
| 168 | + ESP_ERROR_CHECK(MPU.setInterruptConfig(kInterruptConfig)); |
| 169 | + ESP_ERROR_CHECK(MPU.setInterruptEnabled(mpud::INT_EN_RAWDATA_READY)); |
| 170 | + |
| 171 | + // Ready to start reading |
| 172 | + ESP_ERROR_CHECK(MPU.resetFIFO()); // start clean |
| 173 | + |
| 174 | + // Reading Loop |
| 175 | + while (true) { |
| 176 | + // Wait for notification from mpuISR |
| 177 | + uint32_t notificationValue = ulTaskNotifyTake(pdTRUE, portMAX_DELAY); |
| 178 | + if (notificationValue > 1) { |
| 179 | + ESP_LOGW(TAG, "Task Notification higher than 1, value: %d", notificationValue); |
| 180 | + MPU.resetFIFO(); |
| 181 | + continue; |
| 182 | + } |
| 183 | + // Check FIFO count |
| 184 | + uint16_t fifocount = MPU.getFIFOCount(); |
| 185 | + if (esp_err_t err = MPU.lastError()) { |
| 186 | + ESP_LOGE(TAG, "Error reading fifo count, %#X", err); |
| 187 | + MPU.resetFIFO(); |
| 188 | + continue; |
| 189 | + } |
| 190 | + if (fifocount > kFIFOPacketSize * 2) { |
| 191 | + if (!(fifocount % kFIFOPacketSize)) { |
| 192 | + ESP_LOGE(TAG, "Sample Rate too high!, not keeping up the pace!, count: %d", fifocount); |
| 193 | + } |
| 194 | + else { |
| 195 | + ESP_LOGE(TAG, "FIFO Count misaligned! Expected: %d, Actual: %d", kFIFOPacketSize, fifocount); |
| 196 | + } |
| 197 | + MPU.resetFIFO(); |
| 198 | + continue; |
| 199 | + } |
| 200 | + // Burst read data from FIFO |
| 201 | + uint8_t buffer[kFIFOPacketSize]; |
| 202 | + if (esp_err_t err = MPU.readFIFO(kFIFOPacketSize, buffer)) { |
| 203 | + ESP_LOGE(TAG, "Error reading sensor data, %#X", err); |
| 204 | + MPU.resetFIFO(); |
| 205 | + continue; |
| 206 | + } |
| 207 | + // Format |
| 208 | + mpud::raw_axes_t rawAccel, rawGyro; |
| 209 | + rawAccel.x = buffer[0] << 8 | buffer[1]; |
| 210 | + rawAccel.y = buffer[2] << 8 | buffer[3]; |
| 211 | + rawAccel.z = buffer[4] << 8 | buffer[5]; |
| 212 | + rawGyro.x = buffer[6] << 8 | buffer[7]; |
| 213 | + rawGyro.y = buffer[8] << 8 | buffer[9]; |
| 214 | + rawGyro.z = buffer[10] << 8 | buffer[11]; |
| 215 | + // Calculate tilt angle |
| 216 | + // range: (roll[-180,180] pitch[-90,90] yaw[-180,180]) |
| 217 | + constexpr double kRadToDeg = 57.2957795131; |
| 218 | + constexpr float kDeltaTime = 1.f / kSampleRate; |
| 219 | + float gyroRoll = roll + mpud::math::gyroDegPerSec(rawGyro.x, kGyroFS) * kDeltaTime; |
| 220 | + float gyroPitch = pitch + mpud::math::gyroDegPerSec(rawGyro.y, kGyroFS) * kDeltaTime; |
| 221 | + float gyroYaw = yaw + mpud::math::gyroDegPerSec(rawGyro.z, kGyroFS) * kDeltaTime; |
| 222 | + float accelRoll = atan2(-rawAccel.x, rawAccel.z) * kRadToDeg; |
| 223 | + float accelPitch = atan2(rawAccel.y, sqrt(rawAccel.x * rawAccel.x + rawAccel.z * rawAccel.z)) * kRadToDeg; |
| 224 | + // Fusion |
| 225 | + roll = gyroRoll * 0.95f + accelRoll * 0.05f; |
| 226 | + pitch = gyroPitch * 0.95f + accelPitch * 0.05f; |
| 227 | + yaw = gyroYaw; |
| 228 | + // correct yaw |
| 229 | + if (yaw > 180.f) |
| 230 | + yaw -= 360.f; |
| 231 | + else if (yaw < -180.f) |
| 232 | + yaw += 360.f; |
| 233 | + } |
| 234 | + vTaskDelete(nullptr); |
| 235 | +} |
| 236 | + |
| 237 | +static void printTask(void*) |
| 238 | +{ |
| 239 | + vTaskDelay(2000 / portTICK_PERIOD_MS); |
| 240 | + while (true) { |
| 241 | + printf("Pitch: %+6.1f \t Roll: %+6.1f \t Yaw: %+6.1f \n", pitch, roll, yaw); |
| 242 | + vTaskDelay(50 / portTICK_PERIOD_MS); |
| 243 | + } |
| 244 | +} |
| 245 | + |
| 246 | +static IRAM_ATTR void mpuISR(TaskHandle_t taskHandle) |
| 247 | +{ |
| 248 | + BaseType_t HPTaskWoken = pdFALSE; |
| 249 | + vTaskNotifyGiveFromISR(taskHandle, &HPTaskWoken); |
| 250 | + if (HPTaskWoken == pdTRUE) portYIELD_FROM_ISR(); |
| 251 | +} |
0 commit comments