refactor. IMU and BME280 at once, mutex for IMU data
This commit is contained in:
franchioping
parent
404389af0b
commit
5cf3d7d8fc
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 84e7a2b2ba0d6016c6f3d6504491c39505f45319
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 177f256480f6237aee45d2a92057b69c7fcc72b3
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 2fd3d60b827e98af9f56334aa4a02a16a6f67e42
|
||||
|
|
@ -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"
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
#pragma once
|
||||
|
||||
namespace env_sens {
|
||||
|
||||
void setup();
|
||||
|
||||
float get_temperature();
|
||||
|
||||
float get_pressure();
|
||||
|
||||
void dbg_sens();
|
||||
|
||||
} // namespace env_sens
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue