mirrors/mch2022-template-app
1
0
Fork 1
mirror of https://github.com/badgeteam/mch2022-template-app.git synced 2024-11-22 08:41:00 +00:00
Code Issues Projects Releases Wiki Activity

Add initial version of BNO055 component, add BNO055 test code to factory_test firmware

Browse source
This commit is contained in:
Renze Nicolai 2022-01-01 03:06:55 +01:00
parent 9d059dcf9c
commit 88bbf0609e
5 changed files with 198 additions and 66 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
.gitmodules vendored
View file

@ -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

1
factory_test/components/i2c-bno055 Submodule

@ -0,0 +1 @@
Subproject commit 0812d28db91e16f30ff8840bf2cb68a69b1bc2c6

75
factory_test/main/hardware.c
View file

@ -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;
}

9
factory_test/main/hardware.h
View file

@ -4,14 +4,16 @@
#include <esp_err.h>
#include <driver/spi_master.h>
#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

173
factory_test/main/main.c
View file

@ -5,9 +5,84 @@
#include <esp_system.h>
#include <esp_spi_flash.h>
#include <esp_err.h>
#include <esp_log.h>
#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, &currentMode);
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);
}
}