Cansat
/
ESP32-CAN
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

refactor. IMU and BME280 at once, mutex for IMU data

This commit is contained in:
franchioping 2026-03-18 21:43:28 +00:00
parent 404389af0b
commit 5cf3d7d8fc
10 changed files with 205 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
.gitmodules vendored
View File

@ -8,3 +8,12 @@
[submodule "components/esp32_Adafruit_GPS"]
path = components/esp32_Adafruit_GPS
url = ssh://git@forgejo.galard.uk:222/Cansat/esp32_Adafruit_GPS.git
[submodule "components/esp32_Adafruit_BME280_Library"]
path = components/esp32_Adafruit_BME280_Library
url = ssh://git@forgejo.galard.uk:222/Cansat/esp32_Adafruit_BME280_Library.git
[submodule "components/esp32_Adafruit_BusIO"]
path = components/esp32_Adafruit_BusIO
url = ssh://git@forgejo.galard.uk:222/Cansat/esp32_Adafruit_BusIO.git
[submodule "components/esp32_Adafruit_Sensor"]
path = components/esp32_Adafruit_Sensor
url = ssh://git@forgejo.galard.uk:222/Cansat/esp32_Adafruit_Sensor.git

1
components/esp32_Adafruit_BME280_Library Submodule

@ -0,0 +1 @@
Subproject commit 84e7a2b2ba0d6016c6f3d6504491c39505f45319

1
components/esp32_Adafruit_BusIO Submodule

@ -0,0 +1 @@
Subproject commit 177f256480f6237aee45d2a92057b69c7fcc72b3

1
components/esp32_Adafruit_Sensor Submodule

@ -0,0 +1 @@
Subproject commit 2fd3d60b827e98af9f56334aa4a02a16a6f67e42

10
main/CMakeLists.txt
View File

@ -1,4 +1,8 @@
idf_component_register(SRCS "main.cpp"
INCLUDE_DIRS "")
idf_component_register(SRCS "main.cpp" "imu.cpp" "env_sens.cpp"
INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x)
target_compile_options(${COMPONENT_LIB} PRIVATE
"-Wno-gnu-array-member-paren-init"
"-Wno-parentheses-equality"
"-Wno-template-in-declaration-name"
)

61
main/env_sens.cpp Normal file
View File

@ -0,0 +1,61 @@
#include "esp_log.h"
#include <Adafruit_BME280.h>
#include <SPI.h>
#include <Wire.h>
#define SEALEVELPRESSURE_HPA (1013.25)
Adafruit_BME280 bme; // use I2C interface
Adafruit_Sensor *bme_temp = bme.getTemperatureSensor();
Adafruit_Sensor *bme_pressure = bme.getPressureSensor();
Adafruit_Sensor *bme_humidity = bme.getHumiditySensor();
static const constexpr char *TAG = "IMU";
namespace env_sens {
void setup() {
if (!bme.begin()) {
ESP_LOGE(TAG, "Couldn't find a valid sensor");
return;
}
bme_temp->printSensorDetails();
bme_pressure->printSensorDetails();
bme_humidity->printSensorDetails();
}
float get_temperature() {
sensors_event_t temp_event;
bme_temp->getEvent(&temp_event);
return temp_event.temperature;
}
float get_pressure() {
sensors_event_t e;
bme_pressure->getEvent(&e);
return e.pressure;
}
float calculateAltitude(float pressure, float seaLevelPressure,
float tempCelsius) {
float altitude =
(((pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) *
(tempCelsius + 273.15)) /
0.0065;
return altitude;
}
float get_altitude() {
return calculateAltitude(get_pressure(), SEALEVELPRESSURE_HPA,
get_temperature());
}
void dbg_sens() {
ESP_LOGI(TAG, "T (ºC): %f, P (hPa): %f, Alt (m): %f", get_temperature(),
get_pressure(), get_altitude());
}
} // namespace env_sens

13
main/env_sens.h Normal file
View File

@ -0,0 +1,13 @@
#pragma once
namespace env_sens {
void setup();
float get_temperature();
float get_pressure();
void dbg_sens();
} // namespace env_sens

72
main/imu.cpp Normal file
View File

@ -0,0 +1,72 @@
#include "imu.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/idf_additions.h"
static const char *TAG = "IMU";
SemaphoreHandle_t imuStateMutex = NULL;
BNO08x *setup_imu(imu_state *state) {
BNO08x *imu = new BNO08x();
if (!imu->initialize()) {
ESP_LOGE(TAG, "BNO08x Init failure.");
return nullptr;
}
if (imuStateMutex == NULL) {
imuStateMutex = xSemaphoreCreateMutex();
}
imu->dynamic_calibration_autosave_enable();
imu->dynamic_calibration_enable(BNO08xCalSel::all);
imu->rpt.rv_game.enable(2500UL);
imu->rpt.linear_accelerometer.enable(2500UL);
// imu->rpt.rv_game.tare();
imu->register_cb([imu, state]() {
if (imu->rpt.rv_game.has_new_data()) {
auto sens_rot = imu->rpt.rv_game.get_quat();
auto euler = imu->rpt.rv_game.get_euler();
if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
state->euler_ang = {euler.x, euler.y, euler.z};
state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
xSemaphoreGive(imuStateMutex);
}
}
if (imu->rpt.linear_accelerometer.has_new_data()) {
if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
auto sens_accel = imu->rpt.linear_accelerometer.get();
state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
int64_t current_time = esp_timer_get_time();
if (state->last_time != -1) {
float dt = (current_time - state->last_time) / 1000000.0f;
Vec3C delta = apply_rot(&state->accel, &state->rot);
// Trapezoidal Integration for Velocity: v = v0 + 0.5 * (a + a0) * dt
delta.x += state->last_accel.x;
delta.y += state->last_accel.y;
delta.z += state->last_accel.z;
multiply_vec_by(&delta, dt * 0.5f);
add_to_vec(&state->vel, &delta);
state->last_accel = state->accel;
}
state->last_time = current_time;
xSemaphoreGive(imuStateMutex);
}
}
});
ESP_LOGI(TAG, "IMU Setup Success.");
return imu;
}

20
main/imu.h Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include "BNO08x.hpp"
#include "BNO08xGlobalTypes.hpp"
#include "drone_controller.h"
#include <cmath>
#include <cstdint>
struct imu_state {
Vec3C accel = {0, 0, 0};
Vec3C last_accel = {0, 0, 0};
QuatC rot = {0, 0, 0, 1};
Vec3C vel = {0, 0, 0};
int64_t last_time = -1;
Vec3C euler_ang = {0, 0, 0};
};
BNO08x *setup_imu(imu_state *state);
extern SemaphoreHandle_t imuStateMutex;

77
main/main.cpp
View File

@ -1,73 +1,36 @@
#include "BNO08x.hpp"
#include "BNO08xGlobalTypes.hpp"
#include "drone_controller.h"
#include "esp_log.h"
#include "esp_timer.h"
#include <cmath>
#include <cstdint>
#include <stdio.h>
#include "env_sens.h"
#include "freertos/idf_additions.h"
#include "imu.h"
static const constexpr char *TAG = "Main";
static struct Vec3C accel = {0, 0, 0};
static struct Vec3C last_accel = {0, 0, 0};
static struct QuatC rot = {0, 0, 0, 0};
static struct Vec3C pos = {0, 0, 0};
static struct Vec3C vel = {0, 0, 0};
static int64_t last_time = -1;
static struct Vec3C euler_ang = {0, 0, 0};
extern "C" void app_main(void) {
BNO08x *imu = new BNO08x();
env_sens::setup();
if (!imu->initialize()) {
ESP_LOGE(TAG, "Init failure");
imu_state imu_state;
BNO08x *imu = setup_imu(&imu_state);
if (imu == nullptr) {
ESP_LOGE(TAG, "IMU setup failed.");
return;
}
imu->dynamic_calibration_autosave_enable();
imu->dynamic_calibration_enable(BNO08xCalSel::all);
// Linear accel at 100Hz (10000UL us)
imu->rpt.rv_game.enable(2500UL);
imu->rpt.linear_accelerometer.enable(2500UL);
imu->rpt.rv_game.tare();
imu->register_cb([imu]() {
if (imu->rpt.linear_accelerometer.has_new_data()) {
auto sens_accel = imu->rpt.linear_accelerometer.get();
accel = {sens_accel.x, sens_accel.y, sens_accel.z};
int64_t time = esp_timer_get_time();
if (last_time != -1) {
float dt = (time - last_time) / 1000000.0f;
Vec3C delta = apply_rot(&accel, &rot);
delta.x += last_accel.x;
delta.y += last_accel.y;
delta.z += last_accel.z;
multiply_vec_by(&delta, dt * 0.5f);
add_to_vec(&vel, &delta);
last_accel = accel;
}
last_time = time;
}
if (imu->rpt.rv_game.has_new_data()) {
auto sens_rot = imu->rpt.rv_game.get_quat();
auto euler = imu->rpt.rv_game.get_euler();
euler_ang = {euler.x, euler.y, euler.z};
rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
}
});
ESP_LOGE(TAG, "IMU setup sucess.");
while (1) {
vTaskDelay(pdMS_TO_TICKS(100));
// Print filtered data to see the difference
printf("accel_filt - %f, %f, %f", accel.x, accel.y, accel.z);
printf("; vel - %f, %f, %f", vel.x, vel.y, vel.z);
printf("; euler - %f, %f, %f\n", euler_ang.x, euler_ang.y, euler_ang.z);
// multiply_vec_by(&vel, 0.99);
vTaskDelay(pdMS_TO_TICKS(300));
if (xSemaphoreTake(imuStateMutex, pdMS_TO_TICKS(5)) == pdTRUE) {
ESP_LOGI(TAG, "accel - %f, %f, %f", imu_state.accel.x, imu_state.accel.y,
imu_state.accel.z);
ESP_LOGI(TAG, "; euler - %f, %f, %f\n", imu_state.euler_ang.x,
imu_state.euler_ang.y, imu_state.euler_ang.z);
xSemaphoreGive(imuStateMutex);
}
env_sens::dbg_sens();
}
}