74 lines
2.1 KiB
C++
74 lines
2.1 KiB
C++
#include "BNO08x.hpp"
|
|
#include "BNO08xGlobalTypes.hpp"
|
|
#include "drone_controller.h"
|
|
#include "esp_timer.h"
|
|
#include <cmath>
|
|
#include <cstdint>
|
|
#include <stdio.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();
|
|
|
|
if (!imu->initialize()) {
|
|
ESP_LOGE(TAG, "Init failure");
|
|
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};
|
|
}
|
|
});
|
|
|
|
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);
|
|
}
|
|
}
|