diff --git a/.gitmodules b/.gitmodules index 9ab5d66..5ee78b5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -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 diff --git a/components/esp32_Adafruit_BME280_Library b/components/esp32_Adafruit_BME280_Library new file mode 160000 index 0000000..84e7a2b --- /dev/null +++ b/components/esp32_Adafruit_BME280_Library @@ -0,0 +1 @@ +Subproject commit 84e7a2b2ba0d6016c6f3d6504491c39505f45319 diff --git a/components/esp32_Adafruit_BusIO b/components/esp32_Adafruit_BusIO new file mode 160000 index 0000000..177f256 --- /dev/null +++ b/components/esp32_Adafruit_BusIO @@ -0,0 +1 @@ +Subproject commit 177f256480f6237aee45d2a92057b69c7fcc72b3 diff --git a/components/esp32_Adafruit_Sensor b/components/esp32_Adafruit_Sensor new file mode 160000 index 0000000..2fd3d60 --- /dev/null +++ b/components/esp32_Adafruit_Sensor @@ -0,0 +1 @@ +Subproject commit 2fd3d60b827e98af9f56334aa4a02a16a6f67e42 diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index b337a4a..55afbf2 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -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" +) diff --git a/main/env_sens.cpp b/main/env_sens.cpp new file mode 100644 index 0000000..75006d7 --- /dev/null +++ b/main/env_sens.cpp @@ -0,0 +1,61 @@ +#include "esp_log.h" +#include +#include +#include + +#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 diff --git a/main/env_sens.h b/main/env_sens.h new file mode 100644 index 0000000..3e875e8 --- /dev/null +++ b/main/env_sens.h @@ -0,0 +1,13 @@ +#pragma once + +namespace env_sens { + +void setup(); + +float get_temperature(); + +float get_pressure(); + +void dbg_sens(); + +} // namespace env_sens diff --git a/main/imu.cpp b/main/imu.cpp new file mode 100644 index 0000000..2f08be8 --- /dev/null +++ b/main/imu.cpp @@ -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; +} diff --git a/main/imu.h b/main/imu.h new file mode 100644 index 0000000..3955384 --- /dev/null +++ b/main/imu.h @@ -0,0 +1,20 @@ +#pragma once + +#include "BNO08x.hpp" +#include "BNO08xGlobalTypes.hpp" +#include "drone_controller.h" +#include +#include + +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; diff --git a/main/main.cpp b/main/main.cpp index 4dca69d..dc82f2f 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -1,73 +1,36 @@ -#include "BNO08x.hpp" -#include "BNO08xGlobalTypes.hpp" -#include "drone_controller.h" +#include "esp_log.h" #include "esp_timer.h" #include #include #include +#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(); } }