Skip to content

Instantly share code, notes, and snippets.

@natanaeljr
Created April 1, 2018 13:07
Show Gist options
  • Save natanaeljr/39da8d673ad3fad945be525d32f599f6 to your computer and use it in GitHub Desktop.
Save natanaeljr/39da8d673ad3fad945be525d32f599f6 to your computer and use it in GitHub Desktop.
#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();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment