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

141
IMU/main/mpu6886.c
View File

@@ -2,8 +2,8 @@
#include <string.h>
// New: initialise WiFi in either STA or AP mode. Pass ap_mode=true to start an Access Point.
static void wifi_init(bool ap_mode)
#ifdef CONFIG_ENV_MQTT_ENABLED
static void wifi_init()
{
ESP_ERROR_CHECK(nvs_flash_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();
ESP_ERROR_CHECK(esp_wifi_init(&cfg));
if (ap_mode) {
esp_netif_create_default_wifi_ap();
#ifdef CONFIG_WIFI_AP_MODE
esp_netif_create_default_wifi_ap();
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;
}
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);
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 {
esp_netif_create_default_wifi_sta();
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);
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
esp_netif_create_default_wifi_sta();
static esp_err_t mpu6886_write_byte(mpu6886_t *dev, uint8_t reg, uint8_t data) {
uint8_t tx[2] = { reg, data };
return i2c_master_write_to_device(dev->i2c_port, dev->address, tx, sizeof(tx), pdMS_TO_TICKS(100));
}
wifi_config_t wifi_config = {
.sta = {
.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) {
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_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);
static int16_t bytes_to_int16(uint8_t high, uint8_t low) {
return (int16_t)((high << 8) | low);
#endif
}
static void mqtt_app_start(void)
@@ -75,9 +62,22 @@ static void mqtt_app_start(void)
}
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_update_sensitivity(mpu6886_t *dev)
static esp_err_t mpu6886_write_byte(mpu6886_t* dev, uint8_t reg, uint8_t data) {
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;
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);
}
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->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_GYRO_CONFIG, 0x00);
dev->gyro_offset = (vec3_t){0, 0, 0};
dev->accel_offset = (vec3_t){0, 0, 0};
dev->gyro_offset = (vec3_t) { 0, 0, 0 };
dev->accel_offset = (vec3_t) { 0, 0, 0 };
// detect actual sensitivities from device registers and set divisors
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;
}
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];
esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_ACCEL_XOUT_H, buf, 6);
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;
}
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];
esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_GYRO_XOUT_H, buf, 6);
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;
}
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];
esp_err_t ret = mpu6886_read_bytes(dev, MPU6886_TEMP_OUT_H, buf, 2);
if (ret != ESP_OK) return ret;
@@ -192,8 +192,8 @@ esp_err_t mpu6886_read_temp(mpu6886_t *dev, float *temp) {
return ESP_OK;
}
esp_err_t mpu6886_calibrate_gyro(mpu6886_t *dev, int samples, int delay_ms) {
vec3_t sum = {0, 0, 0}, g;
esp_err_t mpu6886_calibrate_gyro(mpu6886_t* dev, int samples, int delay_ms) {
vec3_t sum = { 0, 0, 0 }, g;
for (int i = 0; i < samples; i++) {
if (mpu6886_read_gyro(dev, &g) != ESP_OK) return ESP_FAIL;
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;
}
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.
// 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++) {
if (mpu6886_read_accel(dev, &a) != ESP_OK) {
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,
// otherwise the default is STA mode. You can also call wifi_init(true)
// or wifi_init(false) directly to toggle at runtime.
#ifdef CONFIG_WIFI_AP_MODE
wifi_init(true);
#else
wifi_init(false);
#ifdef CONFIG_ENV_MQTT_ENABLED
wifi_init();
// Start MQTT client (will retry until network is available)
mqtt_app_start();
#endif
mpu6886_t mpu;
mpu.i2c_port = I2C_NUM_0;
mpu.address = MPU6886_ADDR;
esp_err_t ret = mpu6886_init(&mpu, I2C_NUM_0);
if (ret != ESP_OK) {
// Handle initialization error
ESP_LOGE("MPU6886", "init failed");
return;
}
// Start MQTT client (will retry until network is available)
mqtt_app_start();
mpu6886_calibrate_gyro(&mpu, 100, 10);
mpu6886_calibrate_accel(&mpu, 100, 10);
@@ -276,17 +272,13 @@ void app_main(void)
mpu6886_read_accel(&mpu, &accel);
mpu6886_read_gyro(&mpu, &gyro);
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",
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z,
temp);
// Publish readings as JSON over MQTT if client available
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z,
temp);
#else
if (s_mqtt_client != NULL) {
char payload[256];
int n = snprintf(payload, sizeof(payload),
@@ -301,6 +293,7 @@ void app_main(void)
ESP_LOGW("MQTT", "payload truncated or encoding error");
}
}
#endif
vTaskDelay(pdMS_TO_TICKS(500));
}
}