From 88bbf0609e29712cb2b59bb9f53d9c80158d1cf0 Mon Sep 17 00:00:00 2001 From: Renze Nicolai Date: Sat, 1 Jan 2022 03:06:55 +0100 Subject: [PATCH] Add initial version of BNO055 component, add BNO055 test code to factory_test firmware --- .gitmodules | 6 + factory_test/components/i2c-bno055 | 1 + factory_test/main/hardware.c | 75 ++----------- factory_test/main/hardware.h | 9 +- factory_test/main/main.c | 173 +++++++++++++++++++++++++++++ 5 files changed, 198 insertions(+), 66 deletions(-) create mode 160000 factory_test/components/i2c-bno055 diff --git a/.gitmodules b/.gitmodules index add00f7..88d3cd1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -5,6 +5,12 @@ [submodule "factory_test/components/bus-i2c"] path = factory_test/components/bus-i2c url = git@github.com:Nicolai-Electronics/esp32-component-bus-i2c.git + branch = master [submodule "factory_test/components/i2c-pca9555"] path = factory_test/components/i2c-pca9555 url = git@github.com:Nicolai-Electronics/esp32-component-i2c-pca9555.git + branch = master +[submodule "factory_test/components/i2c-bno055"] + path = factory_test/components/i2c-bno055 + url = git@github.com:Nicolai-Electronics/esp32-component-i2c-bno055.git + branch = master diff --git a/factory_test/components/i2c-bno055 b/factory_test/components/i2c-bno055 new file mode 160000 index 0000000..0812d28 --- /dev/null +++ b/factory_test/components/i2c-bno055 @@ -0,0 +1 @@ +Subproject commit 0812d28db91e16f30ff8840bf2cb68a69b1bc2c6 diff --git a/factory_test/main/hardware.c b/factory_test/main/hardware.c index 4a2c340..6c96e3b 100644 --- a/factory_test/main/hardware.c +++ b/factory_test/main/hardware.c @@ -7,46 +7,7 @@ static const char *TAG = "hardware"; PCA9555 pca9555; - -void button_handler(uint8_t pin, bool value) { - switch(pin) { - case PCA9555_PIN_BTN_START: - printf("Start button %s\n", value ? "pressed" : "released"); - break; - case PCA9555_PIN_BTN_SELECT: - printf("Select button %s\n", value ? "pressed" : "released"); - break; - case PCA9555_PIN_BTN_MENU: - printf("Menu button %s\n", value ? "pressed" : "released"); - break; - case PCA9555_PIN_BTN_HOME: - printf("Home button %s\n", value ? "pressed" : "released"); - break; - case PCA9555_PIN_BTN_JOY_LEFT: - printf("Joystick horizontal %s\n", value ? "left" : "center"); - break; - case PCA9555_PIN_BTN_JOY_PRESS: - printf("Joystick %s\n", value ? "pressed" : "released"); - break; - case PCA9555_PIN_BTN_JOY_DOWN: - printf("Joystick vertical %s\n", value ? "down" : "center"); - break; - case PCA9555_PIN_BTN_JOY_UP: - printf("Joy vertical %s\n", value ? "up" : "center"); - break; - case PCA9555_PIN_BTN_JOY_RIGHT: - printf("Joy horizontal %s\n", value ? "right" : "center"); - break; - case PCA9555_PIN_BTN_BACK: - printf("Back button %s\n", value ? "pressed" : "released"); - break; - case PCA9555_PIN_BTN_ACCEPT: - printf("Accept button %s\n", value ? "pressed" : "released"); - break; - default: - printf("Unknown button %d %s\n", pin, value ? "pressed" : "released"); - } -} +BNO055 bno055; esp_err_t hardware_init() { esp_err_t res; @@ -86,31 +47,13 @@ esp_err_t hardware_init() { return res; } - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_START, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_SELECT, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_MENU, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_HOME, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_JOY_LEFT, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_JOY_PRESS, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_JOY_DOWN, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_JOY_UP, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_JOY_RIGHT, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_BACK, true); - pca9555_set_gpio_polarity(&pca9555, PCA9555_PIN_BTN_ACCEPT, true); + // BNO055 sensor on system I2C bus - pca9555.pin_state = 0; // Reset all pin states so that the interrupt function doesn't trigger all the handlers because we inverted the polarity :D - - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_START, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_SELECT, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_MENU, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_HOME, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_JOY_LEFT, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_JOY_PRESS, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_JOY_DOWN, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_JOY_UP, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_JOY_RIGHT, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_BACK, button_handler); - pca9555_set_interrupt_handler(&pca9555, PCA9555_PIN_BTN_ACCEPT, button_handler); + res = bno055_init(&bno055, I2C_BUS_SYS, BNO055_ADDR, GPIO_INT_BNO055, true); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Initializing BNO055 failed"); + return res; + } // User I2C bus res = i2c_init(I2C_BUS_EXT, GPIO_I2C_EXT_SDA, GPIO_I2C_EXT_SCL, I2C_SPEED_EXT, false, false); @@ -125,3 +68,7 @@ esp_err_t hardware_init() { PCA9555* get_pca9555() { return &pca9555; } + +BNO055* get_bno055() { + return &bno055; +} diff --git a/factory_test/main/hardware.h b/factory_test/main/hardware.h index 4c23af4..d160bb4 100644 --- a/factory_test/main/hardware.h +++ b/factory_test/main/hardware.h @@ -4,14 +4,16 @@ #include #include #include "pca9555.h" +#include "bno055.h" esp_err_t hardware_init(); PCA9555* get_pca9555(); +BNO055* get_bno055(); // Interrupts #define GPIO_INT_STM32 0 #define GPIO_INT_PCA9555 34 -#define GPIO_INT_ACCEL 36 +#define GPIO_INT_BNO055 36 #define GPIO_INT_FPGA 39 // SD card @@ -29,7 +31,7 @@ PCA9555* get_pca9555(); #define GPIO_I2C_SYS_SCL 21 #define GPIO_I2C_SYS_SDA 22 #define I2C_BUS_SYS 0 -#define I2C_SPEED_SYS 40000 +#define I2C_SPEED_SYS 20000 // PCA9555 IO expander #define PCA9555_ADDR 0x26 @@ -49,6 +51,9 @@ PCA9555* get_pca9555(); #define PCA9555_PIN_BTN_BACK 14 #define PCA9555_PIN_BTN_ACCEPT 15 +// BNO055 sensor +#define BNO055_ADDR 0x28 + // User I2C bus #define GPIO_I2C_EXT_SCL 25 #define GPIO_I2C_EXT_SDA 26 diff --git a/factory_test/main/main.c b/factory_test/main/main.c index 1f0b79d..55c714f 100644 --- a/factory_test/main/main.c +++ b/factory_test/main/main.c @@ -5,9 +5,84 @@ #include #include #include +#include #include "hardware.h" #include "pca9555.h" +static const char *TAG = "main"; + +bool calibrate = true; + +void button_handler(uint8_t pin, bool value) { + switch(pin) { + case PCA9555_PIN_BTN_START: + printf("Start button %s\n", value ? "pressed" : "released"); + break; + case PCA9555_PIN_BTN_SELECT: + printf("Select button %s\n", value ? "pressed" : "released"); + break; + case PCA9555_PIN_BTN_MENU: + printf("Menu button %s\n", value ? "pressed" : "released"); + break; + case PCA9555_PIN_BTN_HOME: + printf("Home button %s\n", value ? "pressed" : "released"); + break; + case PCA9555_PIN_BTN_JOY_LEFT: + printf("Joystick horizontal %s\n", value ? "left" : "center"); + break; + case PCA9555_PIN_BTN_JOY_PRESS: + printf("Joystick %s\n", value ? "pressed" : "released"); + break; + case PCA9555_PIN_BTN_JOY_DOWN: + printf("Joystick vertical %s\n", value ? "down" : "center"); + break; + case PCA9555_PIN_BTN_JOY_UP: + printf("Joy vertical %s\n", value ? "up" : "center"); + break; + case PCA9555_PIN_BTN_JOY_RIGHT: + printf("Joy horizontal %s\n", value ? "right" : "center"); + break; + case PCA9555_PIN_BTN_BACK: + printf("Back button %s\n", value ? "pressed" : "released"); + break; + case PCA9555_PIN_BTN_ACCEPT: + //printf("Accept button %s\n", value ? "pressed" : "released"); + if (value) calibrate = true; + break; + default: + printf("Unknown button %d %s\n", pin, value ? "pressed" : "released"); + } +} + +void button_init() { + PCA9555* pca9555 = get_pca9555(); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_START, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_SELECT, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_MENU, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_HOME, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_JOY_LEFT, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_JOY_PRESS, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_JOY_DOWN, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_JOY_UP, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_JOY_RIGHT, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_BACK, true); + pca9555_set_gpio_polarity(pca9555, PCA9555_PIN_BTN_ACCEPT, true); + + pca9555->pin_state = 0; // Reset all pin states so that the interrupt function doesn't trigger all the handlers because we inverted the polarity :D + + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_START, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_SELECT, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_MENU, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_HOME, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_JOY_LEFT, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_JOY_PRESS, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_JOY_DOWN, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_JOY_UP, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_JOY_RIGHT, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_BACK, button_handler); + pca9555_set_interrupt_handler(pca9555, PCA9555_PIN_BTN_ACCEPT, button_handler); +} + void restart() { for (int i = 3; i >= 0; i--) { printf("Restarting in %d seconds...\n", i); @@ -18,6 +93,22 @@ void restart() { esp_restart(); } +esp_err_t bno055_workaround(BNO055* device) { + bno055_opmode_t currentMode = 0; + esp_err_t res; + res = bno055_get_mode(device, ¤tMode); + if (res != ESP_OK) return res; + if (currentMode != BNO055_OPERATION_MODE_NDOF) { + printf("!!! Reconfigure BNO055 !!! (%u != %u)\n", currentMode, BNO055_OPERATION_MODE_NDOF); + res = bno055_set_power_mode(device, BNO055_POWER_MODE_NORMAL); + if (res != ESP_OK) return res; + + res = bno055_set_mode(device, BNO055_OPERATION_MODE_NDOF); + if (res != ESP_OK) return res; + } + return res; +} + void app_main(void) { esp_err_t res; @@ -42,4 +133,86 @@ void app_main(void) { (chip_info.features & CHIP_FEATURE_EMB_FLASH) ? "embedded" : "external"); printf("Minimum free heap size: %d bytes\n", esp_get_minimum_free_heap_size()); + + button_init(); + + BNO055* bno055 = get_bno055(); + + bno055_vector_t acceleration, magnetism, orientation, rotation, linear_acceleration, gravity; + bno055_vector_t rotation_offset; + rotation_offset.x = 0; + rotation_offset.y = 0; + rotation_offset.z = 0; + + while (1) { + vTaskDelay(10 / portTICK_PERIOD_MS); + /*res = bno055_test(bno055); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Testing BNO055 failed"); + continue; + }*/ + + res = bno055_workaround(bno055); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Workaround failed %d\n", res); + continue; + } + + /*res = bno055_get_vector(bno055, BNO055_VECTOR_ACCELEROMETER, &acceleration); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Acceleration failed to read %d\n", res); + continue; + }*/ + + res = bno055_get_vector(bno055, BNO055_VECTOR_MAGNETOMETER, &magnetism); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Magnetic field to read %d\n", res); + continue; + } + + /*res = bno055_get_vector(bno055, BNO055_VECTOR_GYROSCOPE, &orientation); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Orientation failed to read %d\n", res); + continue; + }*/ + + res = bno055_get_vector(bno055, BNO055_VECTOR_EULER, &rotation); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Rotation failed to read %d\n", res); + continue; + } + + /*res = bno055_get_vector(bno055, BNO055_VECTOR_LINEARACCEL, &linear_acceleration); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Linear acceleration failed to read %d\n", res); + continue; + } + + res = bno055_get_vector(bno055, BNO055_VECTOR_GRAVITY, &gravity); + if (res != ESP_OK) { + ESP_LOGE(TAG, "Gravity failed to read %d\n", res); + continue; + }*/ + + if (calibrate) { + rotation_offset.x = rotation.x; + rotation_offset.y = rotation.y; + rotation_offset.z = rotation.z; + calibrate = false; + } + + rotation.x -= rotation_offset.x; + rotation.y -= rotation_offset.y; + rotation.z -= rotation_offset.z; + + /*printf("\n\n"); + printf("Acceleration (m/s²) x = %5.8f y = %5.8f z = %5.8f\n", acceleration.x, acceleration.y, acceleration.z); + printf("Magnetic field (uT) x = %5.8f y = %5.8f z = %5.8f\n", magnetism.x, magnetism.y, magnetism.z); + printf("Orientation (dps) x = %5.8f y = %5.8f z = %5.8f\n", orientation.x, orientation.y, orientation.z); + printf("Rotation (degrees) x = %5.8f y = %5.8f z = %5.8f\n", rotation.x, rotation.y, rotation.z); + printf("Linear acceleration (m/s²) x = %5.8f y = %5.8f z = %5.8f\n", linear_acceleration.x, linear_acceleration.y, linear_acceleration.z); + printf("Gravity (m/s²) x = %5.8f y = %5.8f z = %5.8f\n", gravity.x, gravity.y, gravity.z);*/ + + printf("Magnetic (uT) x: %5.4f y: %5.4f z: %5.4f Rotation (deg): x: %5.4f y: %5.4f z: %5.4f \n", magnetism.x, magnetism.y, magnetism.z, rotation.x, rotation.y, rotation.z); + } }