generated from sirlilpanda/kicad-project-template-actionless
200 lines
8.6 KiB
C++
200 lines
8.6 KiB
C++
#include <chrono>
|
|
#include <cmath>
|
|
#include <vector>
|
|
|
|
#include "i2c.hpp"
|
|
#include "kalman_filter.hpp"
|
|
#include "logger.hpp"
|
|
#include "lsm6dso.hpp"
|
|
#include "madgwick_filter.hpp"
|
|
|
|
using namespace std::chrono_literals;
|
|
|
|
extern "C" void app_main(void) {
|
|
espp::Logger logger({.tag = "LSM6DSO Example", .level = espp::Logger::Verbosity::INFO});
|
|
logger.info("Starting LSM6DSO example!");
|
|
|
|
//! [lsm6dso example]
|
|
using Imu = espp::Lsm6dso<espp::lsm6dso::Interface::I2C>;
|
|
|
|
// I2C config (customize as needed)
|
|
static constexpr auto i2c_port = I2C_NUM_0;
|
|
static constexpr auto i2c_clock_speed = CONFIG_EXAMPLE_I2C_CLOCK_SPEED_HZ; // Set in sdkconfig
|
|
static constexpr gpio_num_t i2c_sda = (gpio_num_t)CONFIG_EXAMPLE_I2C_SDA_GPIO; // Set in sdkconfig
|
|
static constexpr gpio_num_t i2c_scl = (gpio_num_t)CONFIG_EXAMPLE_I2C_SCL_GPIO; // Set in sdkconfig
|
|
espp::I2c i2c({.port = i2c_port,
|
|
.sda_io_num = i2c_sda,
|
|
.scl_io_num = i2c_scl,
|
|
.sda_pullup_en = GPIO_PULLUP_ENABLE,
|
|
.scl_pullup_en = GPIO_PULLUP_ENABLE,
|
|
.clk_speed = i2c_clock_speed});
|
|
|
|
// make the orientation filter to compute orientation from accel + gyro
|
|
static constexpr float angle_noise = 0.001f;
|
|
static constexpr float rate_noise = 0.1f;
|
|
static espp::KalmanFilter<2> kf;
|
|
kf.set_process_noise(rate_noise);
|
|
kf.set_measurement_noise(angle_noise);
|
|
|
|
auto kalman_filter_fn = [](float dt, const Imu::Value &accel,
|
|
const Imu::Value &gyro) -> Imu::Value {
|
|
// Apply Kalman filter
|
|
float accelRoll = atan2(accel.y, accel.z);
|
|
float accelPitch = atan2(-accel.x, sqrt(accel.y * accel.y + accel.z * accel.z));
|
|
kf.predict({espp::deg_to_rad(gyro.x), espp::deg_to_rad(gyro.y)}, dt);
|
|
kf.update({accelRoll, accelPitch});
|
|
float roll, pitch;
|
|
std::tie(roll, pitch) = kf.get_state();
|
|
// return the computed orientation
|
|
Imu::Value orientation{};
|
|
orientation.roll = roll;
|
|
orientation.pitch = pitch;
|
|
orientation.yaw = 0.0f;
|
|
return orientation;
|
|
};
|
|
|
|
// Madgwick filter for orientation
|
|
static constexpr float beta = 0.1f;
|
|
static espp::MadgwickFilter madgwick(beta);
|
|
auto madgwick_filter_fn = [](float dt, const Imu::Value &accel,
|
|
const Imu::Value &gyro) -> Imu::Value {
|
|
madgwick.update(dt, accel.x, accel.y, accel.z, espp::deg_to_rad(gyro.x),
|
|
espp::deg_to_rad(gyro.y), espp::deg_to_rad(gyro.z));
|
|
float roll, pitch, yaw;
|
|
madgwick.get_euler(roll, pitch, yaw);
|
|
Imu::Value orientation{};
|
|
orientation.roll = espp::deg_to_rad(roll);
|
|
orientation.pitch = espp::deg_to_rad(pitch);
|
|
orientation.yaw = espp::deg_to_rad(yaw);
|
|
return orientation;
|
|
};
|
|
|
|
// IMU config
|
|
Imu::Config config{
|
|
.device_address = Imu::DEFAULT_I2C_ADDRESS,
|
|
.write = std::bind(&espp::I2c::write, &i2c, std::placeholders::_1, std::placeholders::_2,
|
|
std::placeholders::_3),
|
|
.read = std::bind(&espp::I2c::read, &i2c, std::placeholders::_1, std::placeholders::_2,
|
|
std::placeholders::_3),
|
|
.imu_config =
|
|
{
|
|
.accel_range = Imu::AccelRange::RANGE_2G,
|
|
.accel_odr = Imu::AccelODR::ODR_416_HZ,
|
|
.gyro_range = Imu::GyroRange::DPS_2000,
|
|
.gyro_odr = Imu::GyroODR::ODR_416_HZ,
|
|
},
|
|
.orientation_filter = kalman_filter_fn,
|
|
.auto_init = true,
|
|
.log_level = espp::Logger::Verbosity::INFO,
|
|
};
|
|
|
|
logger.info("Creating LSM6DSO IMU");
|
|
Imu imu(config);
|
|
|
|
std::error_code ec;
|
|
|
|
// set the accel / gyro on-chip filters
|
|
static constexpr uint8_t accel_filter_bandwidth = 0b001; // ODR / 10
|
|
static constexpr uint8_t gyro_lpf_bandwidth = 0b001; // ODR / 3
|
|
static constexpr bool gyro_hpf_enabled = false; // disable high-pass filter on gyro
|
|
static constexpr auto gyro_hpf_bandwidth = Imu::GyroHPF::HPF_0_26_HZ; // 0.26Hz
|
|
if (!imu.set_accelerometer_filter(accel_filter_bandwidth, Imu::AccelFilter::LOWPASS, ec)) {
|
|
logger.error("Failed to set accelerometer filter: {}", ec.message());
|
|
}
|
|
// set the gyroscope filter to have lowpass
|
|
if (!imu.set_gyroscope_filter(gyro_lpf_bandwidth, gyro_hpf_enabled, gyro_hpf_bandwidth, ec)) {
|
|
logger.error("Failed to set gyroscope filter: {}", ec.message());
|
|
}
|
|
|
|
// make a task to read out the IMU data and print it to console
|
|
espp::Task imu_task({.callback = [&](std::mutex &m, std::condition_variable &cv) -> bool {
|
|
static auto start = std::chrono::steady_clock::now();
|
|
|
|
auto now = esp_timer_get_time(); // time in microseconds
|
|
static auto t0 = now;
|
|
auto t1 = now;
|
|
float dt = (t1 - t0) / 1'000'000.0f; // convert us to s
|
|
t0 = t1;
|
|
|
|
std::error_code ec;
|
|
// update the imu data
|
|
if (!imu.update(dt, ec)) {
|
|
return false;
|
|
}
|
|
|
|
// get accel
|
|
auto accel = imu.get_accelerometer();
|
|
auto gyro = imu.get_gyroscope();
|
|
auto temp = imu.get_temperature();
|
|
auto orientation = imu.get_orientation();
|
|
auto gravity_vector = imu.get_gravity_vector();
|
|
|
|
[[maybe_unused]] auto t2 = esp_timer_get_time(); // time in microseconds
|
|
|
|
auto madgwick_orientation = madgwick_filter_fn(dt, accel, gyro);
|
|
float roll = madgwick_orientation.roll;
|
|
float pitch = madgwick_orientation.pitch;
|
|
float yaw = madgwick_orientation.yaw;
|
|
float vx = sin(pitch);
|
|
float vy = -cos(pitch) * sin(roll);
|
|
float vz = -cos(pitch) * cos(roll);
|
|
|
|
// print time and raw IMU data
|
|
std::string text = "";
|
|
text += fmt::format("{:.3f},", now / 1'000'000.0f);
|
|
text += fmt::format("{:02.3f},{:02.3f},{:02.3f},", (float)accel.x,
|
|
(float)accel.y, (float)accel.z);
|
|
text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)gyro.x,
|
|
(float)gyro.y, (float)gyro.z);
|
|
text += fmt::format("{:02.1f},", temp);
|
|
// print kalman filter outputs
|
|
text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)orientation.x,
|
|
(float)orientation.y, (float)orientation.z);
|
|
text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)gravity_vector.x,
|
|
(float)gravity_vector.y, (float)gravity_vector.z);
|
|
// print madgwick filter outputs
|
|
text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", roll, pitch, yaw);
|
|
text += fmt::format("{:03.3f},{:03.3f},{:03.3f}", vx, vy, vz);
|
|
|
|
fmt::print("{}\n", text);
|
|
|
|
// fmt::print("IMU update took {:.3f} ms\n", (t2 - t0) / 1000.0f);
|
|
|
|
// sleep first in case we don't get IMU data and need to exit early
|
|
{
|
|
std::unique_lock<std::mutex> lock(m);
|
|
cv.wait_until(lock, start + 10ms);
|
|
}
|
|
|
|
return false;
|
|
},
|
|
.task_config = {
|
|
.name = "IMU",
|
|
.stack_size_bytes = 6 * 1024,
|
|
.priority = 10,
|
|
.core_id = 0,
|
|
}});
|
|
|
|
// print the header for the IMU data (for plotting)
|
|
fmt::print("% Time (s), "
|
|
// raw IMU data (accel, gyro, temp)
|
|
"Accel X (m/s^2), Accel Y (m/s^2), Accel Z (m/s^2), "
|
|
"Gyro X (rad/s), Gyro Y (rad/s), Gyro Z (rad/s), "
|
|
"Temp (C), "
|
|
// kalman filter outputs
|
|
"Kalman Roll (rad), Kalman Pitch (rad), Kalman Yaw (rad), "
|
|
"Kalman Gravity X, Kalman Gravity Y, Kalman Gravity Z, "
|
|
// madgwick filter outputs
|
|
"Madgwick Roll (rad), Madgwick Pitch (rad), Madgwick Yaw (rad), "
|
|
"Madgwick Gravity X, Madgwick Gravity Y, Madgwick Gravity Z\n");
|
|
|
|
logger.info("Starting IMU task");
|
|
imu_task.start();
|
|
|
|
// loop forever
|
|
while (true) {
|
|
std::this_thread::sleep_for(1s);
|
|
}
|
|
//! [lsm6dso example]
|
|
}
|