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 MQTT/Serial toggle

Browse Source
This commit is contained in:
Vincent Kompjoeteraar Winter
2025-10-31 21:07:52 +01:00
committed by Wessel T
parent 57a1dc4b7f
commit d26e428f99
4 changed files with 138 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
IMU/main/Kconfig.projbuild
View File

@@ -1,7 +1,8 @@
menu "Example Configuration" menu "ESP32 IMU Project Configuration"
orsource "$IDF_PATH/examples/common_components/env_caps/$IDF_TARGET/Kconfig.env_caps" orsource "$IDF_PATH/examples/common_components/env_caps/$IDF_TARGET/Kconfig.env_caps"
menu "I2C Master Configuration"
config I2C_MASTER_SCL config I2C_MASTER_SCL
int "SCL GPIO Num" int "SCL GPIO Num"
range ENV_GPIO_RANGE_MIN ENV_GPIO_OUT_RANGE_MAX range ENV_GPIO_RANGE_MIN ENV_GPIO_OUT_RANGE_MAX
@@ -21,22 +22,20 @@ menu "Example Configuration"
default 400000 default 400000
help help
I2C Speed of Master device. I2C Speed of Master device.
endmenu
config WIFI_SSID config ENV_MQTT_ENABLED
string "WiFi SSID" bool "Enable MQTT Communication"
default "YourNetworkName" default y
help help
SSID of WiFi network to connect to. Enable this option to use MQTT for communication.
If disabled, communication will use usb uart connection.
config WIFI_PASSWORD
string "WiFi Password"
default "YourPassword"
help
Password of WiFi network to connect to.
menu "MQTT Configuration"
depends on ENV_MQTT_ENABLED
config MQTT_BROKER_URI config MQTT_BROKER_URI
string "MQTT Broker URI" string "MQTT Broker URI"
default "mqtt://192.168.178.26:1883" default "mqtt://192.168.4.2:1883"
help help
URI of the MQTT broker to connect to. URI of the MQTT broker to connect to.
@@ -45,23 +44,43 @@ menu "Example Configuration"
default "esp32/imu" default "esp32/imu"
help help
MQTT topic to publish IMU data to. MQTT topic to publish IMU data to.
endmenu
menu "WiFi Access Point Configuration"
depends on ENV_MQTT_ENABLED
config WIFI_AP_MODE config WIFI_AP_MODE
bool "Enable WiFi AP Mode" bool "Enable WiFi AP Mode"
default n default y
help help
Enable this option to start the device in Access Point mode. Enable this option to start the device in Access Point mode.
If disabled, the device will start in Station mode. If disabled, the device will start in Station mode.
config WIFI_SSID
string "WiFi SSID"
default "YourNetworkName"
depends on !WIFI_AP_MODE
help
SSID of WiFi network to connect to.
config WIFI_PASSWORD
string "WiFi Password"
default "YourPassword"
depends on !WIFI_AP_MODE
help
Password of WiFi network to connect to.
config WIFI_AP_SSID config WIFI_AP_SSID
string "WiFi AP SSID" string "WiFi AP SSID"
default "ESP32_IMU_AP" default "ESP32_IMU_AP"
depends on WIFI_AP_MODE
help help
SSID of the WiFi Access Point when in AP mode. SSID of the WiFi Access Point when in AP mode.
config WIFI_AP_PASSWORD config WIFI_AP_PASSWORD
string "WiFi AP Password" string "WiFi AP Password"
default "esp32imuap" default "esp32imuap"
depends on WIFI_AP_MODE
help help
Password of the WiFi Access Point when in AP mode. Password of the WiFi Access Point when in AP mode.
endmenu
endmenu endmenu

141
IMU/main/mpu6886.c
View File

@@ -2,8 +2,8 @@
#include <string.h> #include <string.h>
// New: initialise WiFi in either STA or AP mode. Pass ap_mode=true to start an Access Point. #ifdef CONFIG_ENV_MQTT_ENABLED
static void wifi_init(bool ap_mode) static void wifi_init()
{ {
ESP_ERROR_CHECK(nvs_flash_init()); ESP_ERROR_CHECK(nvs_flash_init());
ESP_ERROR_CHECK(esp_netif_init()); ESP_ERROR_CHECK(esp_netif_init());
@@ -12,54 +12,41 @@ static void wifi_init(bool ap_mode)
wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_wifi_init(&cfg)); ESP_ERROR_CHECK(esp_wifi_init(&cfg));
if (ap_mode) { #ifdef CONFIG_WIFI_AP_MODE
esp_netif_create_default_wifi_ap(); esp_netif_create_default_wifi_ap();
wifi_config_t wifi_config = { 0 }; wifi_config_t wifi_config = { 0 };
strncpy((char *)wifi_config.ap.ssid, CONFIG_WIFI_AP_SSID, sizeof(wifi_config.ap.ssid)); 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); 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)); strncpy((char*)wifi_config.ap.password, CONFIG_WIFI_AP_PASSWORD, sizeof(wifi_config.ap.password));
wifi_config.ap.max_connection = 4; wifi_config.ap.max_connection = 4;
if (strlen(CONFIG_WIFI_AP_PASSWORD) == 0) { if (strlen(CONFIG_WIFI_AP_PASSWORD) == 0) {
wifi_config.ap.authmode = WIFI_AUTH_OPEN; wifi_config.ap.authmode = WIFI_AUTH_OPEN;
} else {
wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK;
}
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 { } else {
esp_netif_create_default_wifi_sta(); wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK;
wifi_config_t wifi_config = {
.sta = {
.ssid = CONFIG_WIFI_SSID,
.password = CONFIG_WIFI_PASSWORD,
},
};
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);
} }
}
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();
static esp_err_t mpu6886_write_byte(mpu6886_t *dev, uint8_t reg, uint8_t data) { wifi_config_t wifi_config = {
uint8_t tx[2] = { reg, data }; .sta = {
return i2c_master_write_to_device(dev->i2c_port, dev->address, tx, sizeof(tx), pdMS_TO_TICKS(100)); .ssid = CONFIG_WIFI_SSID,
} .password = CONFIG_WIFI_PASSWORD,
},
};
static esp_err_t mpu6886_read_bytes(mpu6886_t *dev, uint8_t reg, uint8_t *data, size_t len) { ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
return i2c_master_write_read_device(dev->i2c_port, dev->address, &reg, 1, data, len, pdMS_TO_TICKS(100)); 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);
static int16_t bytes_to_int16(uint8_t high, uint8_t low) { #endif
return (int16_t)((high << 8) | low);
} }
static void mqtt_app_start(void) static void mqtt_app_start(void)
@@ -75,9 +62,22 @@ static void mqtt_app_start(void)
} }
esp_mqtt_client_start(s_mqtt_client); esp_mqtt_client_start(s_mqtt_client);
} }
#endif // CONFIG_ENV_MQTT_ENABLED
// new: detect accel/gyro full-scale selections and set divisors accordingly static esp_err_t mpu6886_write_byte(mpu6886_t* dev, uint8_t reg, uint8_t data) {
static esp_err_t mpu6886_update_sensitivity(mpu6886_t *dev) uint8_t tx[2] = { reg, data };
return i2c_master_write_to_device(dev->i2c_port, dev->address, tx, sizeof(tx), pdMS_TO_TICKS(100));
}
static esp_err_t mpu6886_read_bytes(mpu6886_t* dev, uint8_t reg, uint8_t* data, size_t len) {
return i2c_master_write_read_device(dev->i2c_port, dev->address, &reg, 1, data, len, pdMS_TO_TICKS(100));
}
static int16_t bytes_to_int16(uint8_t high, uint8_t low) {
return (int16_t)((high << 8) | low);
}
static esp_err_t mpu6886_update_sensitivity(mpu6886_t* dev)
{ {
uint8_t aconf = 0, gconf = 0; uint8_t aconf = 0, gconf = 0;
esp_err_t err; esp_err_t err;
@@ -123,7 +123,7 @@ static esp_err_t i2c_master_init(i2c_port_t i2c_num, gpio_num_t sda_io, gpio_num
return i2c_driver_install(i2c_num, I2C_MODE_MASTER, 0, 0, 0); return i2c_driver_install(i2c_num, I2C_MODE_MASTER, 0, 0, 0);
} }
esp_err_t mpu6886_init(mpu6886_t *dev, i2c_port_t i2c_port) { esp_err_t mpu6886_init(mpu6886_t* dev, i2c_port_t i2c_port) {
dev->i2c_port = i2c_port; dev->i2c_port = i2c_port;
dev->address = MPU6886_ADDR; dev->address = MPU6886_ADDR;
@@ -146,8 +146,8 @@ esp_err_t mpu6886_init(mpu6886_t *dev, i2c_port_t i2c_port) {
mpu6886_write_byte(dev, MPU6886_ACCEL_CONFIG, 0x00); mpu6886_write_byte(dev, MPU6886_ACCEL_CONFIG, 0x00);
mpu6886_write_byte(dev, MPU6886_GYRO_CONFIG, 0x00); mpu6886_write_byte(dev, MPU6886_GYRO_CONFIG, 0x00);
dev->gyro_offset = (vec3_t){0, 0, 0}; dev->gyro_offset = (vec3_t) { 0, 0, 0 };
dev->accel_offset = (vec3_t){0, 0, 0}; dev->accel_offset = (vec3_t) { 0, 0, 0 };
// detect actual sensitivities from device registers and set divisors // detect actual sensitivities from device registers and set divisors
if (mpu6886_update_sensitivity(dev) != ESP_OK) { if (mpu6886_update_sensitivity(dev) != ESP_OK) {
@@ -159,7 +159,7 @@ esp_err_t mpu6886_init(mpu6886_t *dev, i2c_port_t i2c_port) {
return ESP_OK; return ESP_OK;
} }
esp_err_t mpu6886_read_accel(mpu6886_t *dev, vec3_t *accel) { esp_err_t mpu6886_read_accel(mpu6886_t* dev, vec3_t* accel) {
uint8_t buf[6]; uint8_t buf[6];
esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_ACCEL_XOUT_H, buf, 6); esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_ACCEL_XOUT_H, buf, 6);
if (ret != ESP_OK) return ret; if (ret != ESP_OK) return ret;
@@ -171,7 +171,7 @@ esp_err_t mpu6886_read_accel(mpu6886_t *dev, vec3_t *accel) {
return ESP_OK; return ESP_OK;
} }
esp_err_t mpu6886_read_gyro(mpu6886_t *dev, vec3_t *gyro) { esp_err_t mpu6886_read_gyro(mpu6886_t* dev, vec3_t* gyro) {
uint8_t buf[6]; uint8_t buf[6];
esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_GYRO_XOUT_H, buf, 6); esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_GYRO_XOUT_H, buf, 6);
if (ret != ESP_OK) return ret; if (ret != ESP_OK) return ret;
@@ -182,7 +182,7 @@ esp_err_t mpu6886_read_gyro(mpu6886_t *dev, vec3_t *gyro) {
return ESP_OK; return ESP_OK;
} }
esp_err_t mpu6886_read_temp(mpu6886_t *dev, float *temp) { esp_err_t mpu6886_read_temp(mpu6886_t* dev, float* temp) {
static uint8_t buf[2]; static uint8_t buf[2];
esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_TEMP_OUT_H, buf, 2); esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_TEMP_OUT_H, buf, 2);
if (ret != ESP_OK) return ret; if (ret != ESP_OK) return ret;
@@ -192,8 +192,8 @@ esp_err_t mpu6886_read_temp(mpu6886_t *dev, float *temp) {
return ESP_OK; return ESP_OK;
} }
esp_err_t mpu6886_calibrate_gyro(mpu6886_t *dev, int samples, int delay_ms) { esp_err_t mpu6886_calibrate_gyro(mpu6886_t* dev, int samples, int delay_ms) {
vec3_t sum = {0, 0, 0}, g; vec3_t sum = { 0, 0, 0 }, g;
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
if (mpu6886_read_gyro(dev, &g) != ESP_OK) return ESP_FAIL; if (mpu6886_read_gyro(dev, &g) != ESP_OK) return ESP_FAIL;
sum.x += g.x; sum.x += g.x;
@@ -207,11 +207,11 @@ esp_err_t mpu6886_calibrate_gyro(mpu6886_t *dev, int samples, int delay_ms) {
return ESP_OK; return ESP_OK;
} }
esp_err_t mpu6886_calibrate_accel(mpu6886_t *dev, int samples, int delay_ms) esp_err_t mpu6886_calibrate_accel(mpu6886_t* dev, int samples, int delay_ms)
{ {
// Calibrate accelerometer offsets while the device is stationary. // Calibrate accelerometer offsets while the device is stationary.
// The function will detect the device full-scale and compute offsets in m/s^2. // The function will detect the device full-scale and compute offsets in m/s^2.
vec3_t sum = {0, 0, 0}, a; vec3_t sum = { 0, 0, 0 }, a;
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
if (mpu6886_read_accel(dev, &a) != ESP_OK) { if (mpu6886_read_accel(dev, &a) != ESP_OK) {
return ESP_FAIL; return ESP_FAIL;
@@ -244,28 +244,24 @@ void app_main(void)
} }
// Start WiFi: set CONFIG_WIFI_AP_MODE in sdkconfig to build in AP mode, #ifdef CONFIG_ENV_MQTT_ENABLED
// otherwise the default is STA mode. You can also call wifi_init(true) wifi_init();
// or wifi_init(false) directly to toggle at runtime. // Start MQTT client (will retry until network is available)
#ifdef CONFIG_WIFI_AP_MODE mqtt_app_start();
wifi_init(true);
#else
wifi_init(false);
#endif #endif
mpu6886_t mpu; mpu6886_t mpu;
mpu.i2c_port = I2C_NUM_0; mpu.i2c_port = I2C_NUM_0;
mpu.address = MPU6886_ADDR; mpu.address = MPU6886_ADDR;
esp_err_t ret = mpu6886_init(&mpu, I2C_NUM_0); esp_err_t ret = mpu6886_init(&mpu, I2C_NUM_0);
if (ret != ESP_OK) { if (ret != ESP_OK) {
// Handle initialization error
ESP_LOGE("MPU6886", "init failed"); ESP_LOGE("MPU6886", "init failed");
return; return;
} }
// Start MQTT client (will retry until network is available)
mqtt_app_start();
mpu6886_calibrate_gyro(&mpu, 100, 10); mpu6886_calibrate_gyro(&mpu, 100, 10);
mpu6886_calibrate_accel(&mpu, 100, 10); mpu6886_calibrate_accel(&mpu, 100, 10);
@@ -276,17 +272,13 @@ void app_main(void)
mpu6886_read_accel(&mpu, &accel); mpu6886_read_accel(&mpu, &accel);
mpu6886_read_gyro(&mpu, &gyro); mpu6886_read_gyro(&mpu, &gyro);
mpu6886_read_temp(&mpu, &temp); mpu6886_read_temp(&mpu, &temp);
// ESP_LOGI("MPU6886", "Accel: X=%8.2f Y=%8.2f Z=%8.2f m/s² | Gyro: X=%8.2f Y=%8.2f Z=%8.2f rad/s | Temp: %8.2f °C",
// accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z, temp);
// printf("aX=%8.2f aY=%8.2f aZ=%8.2f m/s² | gX=%8.2f gY=%8.2f gZ=%8.2f rad/s | Temp: %8.2f °C\n",
// accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z, 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", 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, accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z, gyro.x, gyro.y, gyro.z,
temp); temp);
#else
// Publish readings as JSON over MQTT if client available
if (s_mqtt_client != NULL) { if (s_mqtt_client != NULL) {
char payload[256]; char payload[256];
int n = snprintf(payload, sizeof(payload), int n = snprintf(payload, sizeof(payload),
@@ -301,6 +293,7 @@ void app_main(void)
ESP_LOGW("MQTT", "payload truncated or encoding error"); ESP_LOGW("MQTT", "payload truncated or encoding error");
} }
} }
#endif
vTaskDelay(pdMS_TO_TICKS(500)); vTaskDelay(pdMS_TO_TICKS(500));
} }
} }

21
IMU/sdkconfig
View File

@@ -433,21 +433,38 @@ CONFIG_PARTITION_TABLE_MD5=y
# end of Partition Table # end of Partition Table
# #
# Example Configuration # ESP32 IMU Project Configuration
# #
CONFIG_ENV_GPIO_RANGE_MIN=0 CONFIG_ENV_GPIO_RANGE_MIN=0
CONFIG_ENV_GPIO_RANGE_MAX=39 CONFIG_ENV_GPIO_RANGE_MAX=39
CONFIG_ENV_GPIO_IN_RANGE_MAX=39 CONFIG_ENV_GPIO_IN_RANGE_MAX=39
CONFIG_ENV_GPIO_OUT_RANGE_MAX=33 CONFIG_ENV_GPIO_OUT_RANGE_MAX=33
#
# I2C Master Configuration
#
CONFIG_I2C_MASTER_SCL=22 CONFIG_I2C_MASTER_SCL=22
CONFIG_I2C_MASTER_SDA=21 CONFIG_I2C_MASTER_SDA=21
CONFIG_I2C_MASTER_FREQUENCY=100000 CONFIG_I2C_MASTER_FREQUENCY=100000
# end of I2C Master Configuration
CONFIG_ENV_MQTT_ENABLED=y
#
# MQTT Configuration
#
CONFIG_MQTT_BROKER_URI="mqtt://192.168.4.2:1883" CONFIG_MQTT_BROKER_URI="mqtt://192.168.4.2:1883"
CONFIG_MQTT_TOPIC="esp32/imu" CONFIG_MQTT_TOPIC="esp32/imu"
# end of MQTT Configuration
#
# WiFi Access Point Configuration
#
CONFIG_WIFI_AP_MODE=y CONFIG_WIFI_AP_MODE=y
CONFIG_WIFI_AP_SSID="ESP32_IMU_AP" CONFIG_WIFI_AP_SSID="ESP32_IMU_AP"
CONFIG_WIFI_AP_PASSWORD="esp32imuap" CONFIG_WIFI_AP_PASSWORD="esp32imuap"
# end of Example Configuration # end of WiFi Access Point Configuration
# end of ESP32 IMU Project Configuration
# #
# Compiler options # Compiler options

23
IMU/sdkconfig.old
View File

@@ -433,21 +433,38 @@ CONFIG_PARTITION_TABLE_MD5=y
# end of Partition Table # end of Partition Table
# #
# Example Configuration # ESP32 IMU Project Configuration
# #
CONFIG_ENV_GPIO_RANGE_MIN=0 CONFIG_ENV_GPIO_RANGE_MIN=0
CONFIG_ENV_GPIO_RANGE_MAX=39 CONFIG_ENV_GPIO_RANGE_MAX=39
CONFIG_ENV_GPIO_IN_RANGE_MAX=39 CONFIG_ENV_GPIO_IN_RANGE_MAX=39
CONFIG_ENV_GPIO_OUT_RANGE_MAX=33 CONFIG_ENV_GPIO_OUT_RANGE_MAX=33
#
# I2C Master Configuration
#
CONFIG_I2C_MASTER_SCL=22 CONFIG_I2C_MASTER_SCL=22
CONFIG_I2C_MASTER_SDA=21 CONFIG_I2C_MASTER_SDA=21
CONFIG_I2C_MASTER_FREQUENCY=100000 CONFIG_I2C_MASTER_FREQUENCY=100000
CONFIG_MQTT_BROKER_URI="mqtt://192.168.178.200:1883" # end of I2C Master Configuration
CONFIG_ENV_MQTT_ENABLED=y
#
# MQTT Configuration
#
CONFIG_MQTT_BROKER_URI="mqtt://192.168.4.2:1883"
CONFIG_MQTT_TOPIC="esp32/imu" CONFIG_MQTT_TOPIC="esp32/imu"
# end of MQTT Configuration
#
# WiFi Access Point Configuration
#
CONFIG_WIFI_AP_MODE=y CONFIG_WIFI_AP_MODE=y
CONFIG_WIFI_AP_SSID="ESP32_IMU_AP" CONFIG_WIFI_AP_SSID="ESP32_IMU_AP"
CONFIG_WIFI_AP_PASSWORD="esp32imuap" CONFIG_WIFI_AP_PASSWORD="esp32imuap"
# end of Example Configuration # end of WiFi Access Point Configuration
# end of ESP32 IMU Project Configuration
# #
# Compiler options # Compiler options