inholland/ros2-assignments
3
1
Fork 0
generated from wessel/boilerplate
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(esp32-IMU): Add toggle functionality MQTT/Serial using boot button

Browse Source
This commit is contained in:
Vincent Kompjoeteraar Winter
2025-11-05 10:59:53 +01:00
parent 6fe67fe87f
commit 2784a4bb79
2 changed files with 127 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

184
IMU/main/mpu6886.c
View File

@@ -1,67 +1,115 @@
#include "mpu6886.h"
#ifdef CONFIG_ENV_MQTT_ENABLED
static void wifi_init()
{
ESP_ERROR_CHECK(nvs_flash_init());
ESP_ERROR_CHECK(esp_netif_init());
ESP_ERROR_CHECK(esp_event_loop_create_default());
static void IRAM_ATTR button_isr_handler(void* arg) {
uint32_t gpio_num = (uint32_t)arg;
// Keep it short — avoid delays, logging, malloc, etc.
mqtt_toggle = !mqtt_toggle;
toggle_completed = false;
esp_rom_printf("Button pressed! GPIO: %lu\n", gpio_num);
}
wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_wifi_init(&cfg));
static void init_button(void) {
gpio_config_t io_conf = {
.intr_type = GPIO_INTR_NEGEDGE, // Trigger on falling edge
.mode = GPIO_MODE_INPUT,
.pin_bit_mask = (1ULL << BOOT_BUTTON_GPIO),
.pull_up_en = GPIO_PULLUP_ENABLE, // Use pull-up since button pulls low
.pull_down_en = GPIO_PULLDOWN_DISABLE
};
gpio_config(&io_conf);
#ifdef CONFIG_WIFI_AP_MODE
esp_netif_create_default_wifi_ap();
// Install GPIO ISR service
gpio_install_isr_service(0); // Pass 0 for default interrupt flags
// Attach the interrupt handler
gpio_isr_handler_add(BOOT_BUTTON_GPIO, button_isr_handler, (void*)BOOT_BUTTON_GPIO);
}
wifi_config_t wifi_config = { 0 };
strncpy((char*)wifi_config.ap.ssid, CONFIG_WIFI_AP_SSID, sizeof(wifi_config.ap.ssid));
wifi_config.ap.ssid_len = strlen(CONFIG_WIFI_AP_SSID);
strncpy((char*)wifi_config.ap.password, CONFIG_WIFI_AP_PASSWORD, sizeof(wifi_config.ap.password));
wifi_config.ap.max_connection = 4;
if (strlen(CONFIG_WIFI_AP_PASSWORD) == 0) {
wifi_config.ap.authmode = WIFI_AUTH_OPEN;
} else {
wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK;
}
static void wifi_init()
{
ESP_ERROR_CHECK(nvs_flash_init());
ESP_ERROR_CHECK(esp_netif_init());
ESP_ERROR_CHECK(esp_event_loop_create_default());
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config));
ESP_ERROR_CHECK(esp_wifi_start());
ESP_LOGI("WIFI", "AP started SSID:%s", CONFIG_WIFI_AP_SSID);
#else
esp_netif_create_default_wifi_sta();
wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_wifi_init(&cfg));
wifi_config_t wifi_config = {
.sta = {
.ssid = CONFIG_WIFI_SSID,
.password = CONFIG_WIFI_PASSWORD,
},
};
#ifdef CONFIG_WIFI_AP_MODE
wifi_netif = esp_netif_create_default_wifi_ap();
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
ESP_ERROR_CHECK(esp_wifi_start());
ESP_ERROR_CHECK(esp_wifi_connect());
ESP_LOGI("WIFI", "STA started, connecting to: %s", CONFIG_WIFI_SSID);
#endif
wifi_config_t wifi_config = { 0 };
strncpy((char*)wifi_config.ap.ssid, CONFIG_WIFI_AP_SSID, sizeof(wifi_config.ap.ssid));
wifi_config.ap.ssid_len = strlen(CONFIG_WIFI_AP_SSID);
strncpy((char*)wifi_config.ap.password, CONFIG_WIFI_AP_PASSWORD, sizeof(wifi_config.ap.password));
wifi_config.ap.max_connection = 4;
if (strlen(CONFIG_WIFI_AP_PASSWORD) == 0) {
wifi_config.ap.authmode = WIFI_AUTH_OPEN;
} else {
wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK;
}
static void mqtt_app_start(void)
{
esp_mqtt_client_config_t mqtt_cfg = { 0 };
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config));
ESP_ERROR_CHECK(esp_wifi_start());
ESP_LOGI("WIFI", "AP started SSID:%s", CONFIG_WIFI_AP_SSID);
#else
wifi_netif = esp_netif_create_default_wifi_sta();
mqtt_cfg.broker.address.uri = CONFIG_MQTT_BROKER_URI;
wifi_config_t wifi_config = {
.sta = {
.ssid = CONFIG_WIFI_SSID,
.password = CONFIG_WIFI_PASSWORD,
},
};
s_mqtt_client = esp_mqtt_client_init(&mqtt_cfg);
if (s_mqtt_client == NULL) {
ESP_LOGW("MQTT", "failed to init mqtt client");
return;
}
esp_mqtt_client_start(s_mqtt_client);
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
ESP_ERROR_CHECK(esp_wifi_start());
ESP_ERROR_CHECK(esp_wifi_connect());
ESP_LOGI("WIFI", "STA started, connecting to: %s", CONFIG_WIFI_SSID);
#endif
wifi_initialized = true;
}
static void wifi_deinit(void)
{
ESP_ERROR_CHECK(esp_wifi_stop());
ESP_ERROR_CHECK(esp_wifi_deinit());
if (wifi_netif != NULL) {
esp_netif_destroy_default_wifi(wifi_netif);
wifi_netif = NULL;
}
#endif // CONFIG_ENV_MQTT_ENABLED
ESP_ERROR_CHECK(esp_event_loop_delete_default());
// ESP_ERROR_CHECK(esp_netif_deinit());
ESP_ERROR_CHECK(nvs_flash_deinit());
ESP_LOGI("WIFI", "Wi-Fi stopped and deinitialized");
wifi_initialized = false;
}
static void mqtt_app_start(void)
{
esp_mqtt_client_config_t mqtt_cfg = { 0 };
mqtt_cfg.broker.address.uri = CONFIG_MQTT_BROKER_URI;
s_mqtt_client = esp_mqtt_client_init(&mqtt_cfg);
if (s_mqtt_client == NULL) {
ESP_LOGW("MQTT", "failed to init mqtt client");
return;
}
esp_mqtt_client_start(s_mqtt_client);
}
static void mqtt_app_stop(void)
{
if (s_mqtt_client != NULL) {
esp_mqtt_client_stop(s_mqtt_client);
esp_mqtt_client_destroy(s_mqtt_client);
s_mqtt_client = NULL;
}
}
static esp_err_t mpu6886_write_byte(mpu6886_t* dev, uint8_t reg, uint8_t data) {
uint8_t tx[2] = { reg, data };
@@ -141,7 +189,7 @@ esp_err_t mpu6886_init(mpu6886_t* dev, i2c_port_t i2c_port) {
// Auto select clock
if (mpu6886_write_byte(dev, MPU6886_PWR_MGMT_1, 0x01) != ESP_OK) return ESP_FAIL;
// Default config: set to ±2G accel, ±250DPS gyro (device may have different settings; we'll detect them)
// Default config: set to ±2G accel, ±250DPS gyro
mpu6886_write_byte(dev, MPU6886_ACCEL_CONFIG, 0x00);
mpu6886_write_byte(dev, MPU6886_GYRO_CONFIG, 0x00);
@@ -242,14 +290,7 @@ void app_main(void)
return;
}
#ifdef CONFIG_ENV_MQTT_ENABLED
wifi_init();
// Start MQTT client (will retry until network is available)
mqtt_app_start();
#endif
init_button();
mpu6886_t mpu;
mpu.i2c_port = I2C_NUM_0;
mpu.address = MPU6886_ADDR;
@@ -260,25 +301,28 @@ void app_main(void)
return;
}
mpu6886_calibrate_gyro(&mpu, 100, 10);
mpu6886_calibrate_accel(&mpu, 100, 10);
vec3_t accel, gyro;
float temp;
while (1) {
if (mqtt_toggle && !toggle_completed && !wifi_initialized) {
wifi_init();
mqtt_app_start();
ESP_LOGI("BOOT", "Boot button pressed: starting in MQTT mode");
toggle_completed = true;
} else if (!mqtt_toggle && !toggle_completed && wifi_initialized) {
mqtt_app_stop();
wifi_deinit();
ESP_LOGI("BOOT", "Boot button not pressed: starting in serial-only mode");
toggle_completed = true;
}
mpu6886_read_accel(&mpu, &accel);
mpu6886_read_gyro(&mpu, &gyro);
mpu6886_read_temp(&mpu, &temp);
#ifndef CONFIG_ENV_MQTT_ENABLED
printf("{\"accel\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"gyro\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"Temp\":%8.2f}\n",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z,
temp);
#else
if (s_mqtt_client != NULL) {
if (mqtt_toggle && s_mqtt_client != NULL) {
char payload[256];
int n = snprintf(payload, sizeof(payload),
"{\"accel\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"gyro\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"Temp\":%8.2f}",
@@ -291,8 +335,12 @@ void app_main(void)
} else {
ESP_LOGW("MQTT", "payload truncated or encoding error");
}
} else {
printf("{\"accel\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"gyro\":{\"x\":%8.3f,\"y\":%8.3f,\"z\":%8.3f},\"Temp\":%8.2f}\n",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z,
temp);
}
#endif
vTaskDelay(pdMS_TO_TICKS(500));
}
}

11
IMU/main/mpu6886.h
View File

@@ -13,6 +13,9 @@
#include "mqtt_client.h"
#include <string.h>
#include <stdio.h>
#include "driver/gpio.h"
#include "esp_rom_sys.h"
#define I2C_MASTER_SCL_IO CONFIG_I2C_MASTER_SCL /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO CONFIG_I2C_MASTER_SDA /*!< GPIO number used for I2C master data */
@@ -22,6 +25,8 @@
#define I2C_MASTER_RX_BUF_DISABLE 0 /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS 1000
#define BOOT_BUTTON_GPIO 0 // GPIO number for boot mode selection button
#define MPU6886_ADDR 0x68
// MPU6886 Registers
@@ -66,6 +71,12 @@ typedef struct {
vec3_t accel_offset;
} mpu6886_t;
bool mqtt_toggle = false;
bool toggle_completed = false;
bool wifi_initialized = false;
esp_netif_t* wifi_netif = NULL;
static esp_mqtt_client_handle_t s_mqtt_client = NULL;
// Function declarations