|
| 1 | +#include <chrono> |
| 2 | +#include <cmath> |
| 3 | +#include <vector> |
| 4 | + |
| 5 | +#include "i2c.hpp" |
| 6 | +#include "kalman_filter.hpp" |
| 7 | +#include "logger.hpp" |
| 8 | +#include "lsm6dso.hpp" |
| 9 | +#include "madgwick_filter.hpp" |
| 10 | + |
| 11 | +using namespace std::chrono_literals; |
| 12 | + |
| 13 | +extern "C" void app_main(void) { |
| 14 | + espp::Logger logger({.tag = "LSM6DSO Example", .level = espp::Logger::Verbosity::INFO}); |
| 15 | + logger.info("Starting LSM6DSO example!"); |
| 16 | + |
| 17 | + //! [lsm6dso example] |
| 18 | + using Imu = espp::Lsm6dso<espp::lsm6dso::Interface::I2C>; |
| 19 | + |
| 20 | + // I2C config (customize as needed) |
| 21 | + static constexpr auto i2c_port = I2C_NUM_0; |
| 22 | + static constexpr auto i2c_clock_speed = CONFIG_EXAMPLE_I2C_CLOCK_SPEED_HZ; // Set in sdkconfig |
| 23 | + static constexpr gpio_num_t i2c_sda = (gpio_num_t)CONFIG_EXAMPLE_I2C_SDA_GPIO; // Set in sdkconfig |
| 24 | + static constexpr gpio_num_t i2c_scl = (gpio_num_t)CONFIG_EXAMPLE_I2C_SCL_GPIO; // Set in sdkconfig |
| 25 | + espp::I2c i2c({.port = i2c_port, |
| 26 | + .sda_io_num = i2c_sda, |
| 27 | + .scl_io_num = i2c_scl, |
| 28 | + .sda_pullup_en = GPIO_PULLUP_ENABLE, |
| 29 | + .scl_pullup_en = GPIO_PULLUP_ENABLE, |
| 30 | + .clk_speed = i2c_clock_speed}); |
| 31 | + |
| 32 | + // make the orientation filter to compute orientation from accel + gyro |
| 33 | + static constexpr float angle_noise = 0.001f; |
| 34 | + static constexpr float rate_noise = 0.1f; |
| 35 | + static espp::KalmanFilter<2> kf; |
| 36 | + kf.set_process_noise(rate_noise); |
| 37 | + kf.set_measurement_noise(angle_noise); |
| 38 | + |
| 39 | + auto kalman_filter_fn = [](float dt, const Imu::Value &accel, |
| 40 | + const Imu::Value &gyro) -> Imu::Value { |
| 41 | + // Apply Kalman filter |
| 42 | + float accelRoll = atan2(accel.y, accel.z); |
| 43 | + float accelPitch = atan2(-accel.x, sqrt(accel.y * accel.y + accel.z * accel.z)); |
| 44 | + kf.predict({espp::deg_to_rad(gyro.x), espp::deg_to_rad(gyro.y)}, dt); |
| 45 | + kf.update({accelRoll, accelPitch}); |
| 46 | + float roll, pitch; |
| 47 | + std::tie(roll, pitch) = kf.get_state(); |
| 48 | + // return the computed orientation |
| 49 | + Imu::Value orientation{}; |
| 50 | + orientation.roll = roll; |
| 51 | + orientation.pitch = pitch; |
| 52 | + orientation.yaw = 0.0f; |
| 53 | + return orientation; |
| 54 | + }; |
| 55 | + |
| 56 | + // Madgwick filter for orientation |
| 57 | + static constexpr float beta = 0.1f; |
| 58 | + static espp::MadgwickFilter madgwick(beta); |
| 59 | + auto madgwick_filter_fn = [](float dt, const Imu::Value &accel, |
| 60 | + const Imu::Value &gyro) -> Imu::Value { |
| 61 | + madgwick.update(dt, accel.x, accel.y, accel.z, espp::deg_to_rad(gyro.x), |
| 62 | + espp::deg_to_rad(gyro.y), espp::deg_to_rad(gyro.z)); |
| 63 | + float roll, pitch, yaw; |
| 64 | + madgwick.get_euler(roll, pitch, yaw); |
| 65 | + Imu::Value orientation{}; |
| 66 | + orientation.roll = espp::deg_to_rad(roll); |
| 67 | + orientation.pitch = espp::deg_to_rad(pitch); |
| 68 | + orientation.yaw = espp::deg_to_rad(yaw); |
| 69 | + return orientation; |
| 70 | + }; |
| 71 | + |
| 72 | + // IMU config |
| 73 | + Imu::Config config{ |
| 74 | + .device_address = Imu::DEFAULT_I2C_ADDRESS, |
| 75 | + .write = std::bind(&espp::I2c::write, &i2c, std::placeholders::_1, std::placeholders::_2, |
| 76 | + std::placeholders::_3), |
| 77 | + .read = std::bind(&espp::I2c::read, &i2c, std::placeholders::_1, std::placeholders::_2, |
| 78 | + std::placeholders::_3), |
| 79 | + .imu_config = |
| 80 | + { |
| 81 | + .accel_range = Imu::AccelRange::RANGE_2G, |
| 82 | + .accel_odr = Imu::AccelODR::ODR_416_HZ, |
| 83 | + .gyro_range = Imu::GyroRange::DPS_2000, |
| 84 | + .gyro_odr = Imu::GyroODR::ODR_416_HZ, |
| 85 | + }, |
| 86 | + .orientation_filter = kalman_filter_fn, |
| 87 | + .auto_init = true, |
| 88 | + .log_level = espp::Logger::Verbosity::INFO, |
| 89 | + }; |
| 90 | + |
| 91 | + logger.info("Creating LSM6DSO IMU"); |
| 92 | + Imu imu(config); |
| 93 | + |
| 94 | + std::error_code ec; |
| 95 | + |
| 96 | + // set the accel / gyro on-chip filters |
| 97 | + static constexpr uint8_t accel_filter_bandwidth = 0b001; // ODR / 10 |
| 98 | + static constexpr uint8_t gyro_lpf_bandwidth = 0b001; // ODR / 3 |
| 99 | + static constexpr bool gyro_hpf_enabled = false; // disable high-pass filter on gyro |
| 100 | + static constexpr auto gyro_hpf_bandwidth = Imu::GyroHPF::HPF_0_26_HZ; // 0.26Hz |
| 101 | + if (!imu.set_accelerometer_filter(accel_filter_bandwidth, Imu::AccelFilter::LOWPASS, ec)) { |
| 102 | + logger.error("Failed to set accelerometer filter: {}", ec.message()); |
| 103 | + } |
| 104 | + // set the gyroscope filter to have lowpass |
| 105 | + if (!imu.set_gyroscope_filter(gyro_lpf_bandwidth, gyro_hpf_enabled, gyro_hpf_bandwidth, ec)) { |
| 106 | + logger.error("Failed to set gyroscope filter: {}", ec.message()); |
| 107 | + } |
| 108 | + |
| 109 | + // make a task to read out the IMU data and print it to console |
| 110 | + espp::Task imu_task({.callback = [&](std::mutex &m, std::condition_variable &cv) -> bool { |
| 111 | + static auto start = std::chrono::steady_clock::now(); |
| 112 | + |
| 113 | + auto now = esp_timer_get_time(); // time in microseconds |
| 114 | + static auto t0 = now; |
| 115 | + auto t1 = now; |
| 116 | + float dt = (t1 - t0) / 1'000'000.0f; // convert us to s |
| 117 | + t0 = t1; |
| 118 | + |
| 119 | + std::error_code ec; |
| 120 | + // update the imu data |
| 121 | + if (!imu.update(dt, ec)) { |
| 122 | + return false; |
| 123 | + } |
| 124 | + |
| 125 | + // get accel |
| 126 | + auto accel = imu.get_accelerometer(); |
| 127 | + auto gyro = imu.get_gyroscope(); |
| 128 | + auto temp = imu.get_temperature(); |
| 129 | + auto orientation = imu.get_orientation(); |
| 130 | + auto gravity_vector = imu.get_gravity_vector(); |
| 131 | + |
| 132 | + [[maybe_unused]] auto t2 = esp_timer_get_time(); // time in microseconds |
| 133 | + |
| 134 | + auto madgwick_orientation = madgwick_filter_fn(dt, accel, gyro); |
| 135 | + float roll = madgwick_orientation.roll; |
| 136 | + float pitch = madgwick_orientation.pitch; |
| 137 | + float yaw = madgwick_orientation.yaw; |
| 138 | + float vx = sin(pitch); |
| 139 | + float vy = -cos(pitch) * sin(roll); |
| 140 | + float vz = -cos(pitch) * cos(roll); |
| 141 | + |
| 142 | + // print time and raw IMU data |
| 143 | + std::string text = ""; |
| 144 | + text += fmt::format("{:.3f},", now / 1'000'000.0f); |
| 145 | + text += fmt::format("{:02.3f},{:02.3f},{:02.3f},", (float)accel.x, |
| 146 | + (float)accel.y, (float)accel.z); |
| 147 | + text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)gyro.x, |
| 148 | + (float)gyro.y, (float)gyro.z); |
| 149 | + text += fmt::format("{:02.1f},", temp); |
| 150 | + // print kalman filter outputs |
| 151 | + text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)orientation.x, |
| 152 | + (float)orientation.y, (float)orientation.z); |
| 153 | + text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)gravity_vector.x, |
| 154 | + (float)gravity_vector.y, (float)gravity_vector.z); |
| 155 | + // print madgwick filter outputs |
| 156 | + text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", roll, pitch, yaw); |
| 157 | + text += fmt::format("{:03.3f},{:03.3f},{:03.3f}", vx, vy, vz); |
| 158 | + |
| 159 | + fmt::print("{}\n", text); |
| 160 | + |
| 161 | + // fmt::print("IMU update took {:.3f} ms\n", (t2 - t0) / 1000.0f); |
| 162 | + |
| 163 | + // sleep first in case we don't get IMU data and need to exit early |
| 164 | + { |
| 165 | + std::unique_lock<std::mutex> lock(m); |
| 166 | + cv.wait_until(lock, start + 10ms); |
| 167 | + } |
| 168 | + |
| 169 | + return false; |
| 170 | + }, |
| 171 | + .task_config = { |
| 172 | + .name = "IMU", |
| 173 | + .stack_size_bytes = 6 * 1024, |
| 174 | + .priority = 10, |
| 175 | + .core_id = 0, |
| 176 | + }}); |
| 177 | + |
| 178 | + // print the header for the IMU data (for plotting) |
| 179 | + fmt::print("% Time (s), " |
| 180 | + // raw IMU data (accel, gyro, temp) |
| 181 | + "Accel X (m/s^2), Accel Y (m/s^2), Accel Z (m/s^2), " |
| 182 | + "Gyro X (rad/s), Gyro Y (rad/s), Gyro Z (rad/s), " |
| 183 | + "Temp (C), " |
| 184 | + // kalman filter outputs |
| 185 | + "Kalman Roll (rad), Kalman Pitch (rad), Kalman Yaw (rad), " |
| 186 | + "Kalman Gravity X, Kalman Gravity Y, Kalman Gravity Z, " |
| 187 | + // madgwick filter outputs |
| 188 | + "Madgwick Roll (rad), Madgwick Pitch (rad), Madgwick Yaw (rad), " |
| 189 | + "Madgwick Gravity X, Madgwick Gravity Y, Madgwick Gravity Z\n"); |
| 190 | + |
| 191 | + logger.info("Starting IMU task"); |
| 192 | + imu_task.start(); |
| 193 | + |
| 194 | + // loop forever |
| 195 | + while (true) { |
| 196 | + std::this_thread::sleep_for(1s); |
| 197 | + } |
| 198 | + //! [lsm6dso example] |
| 199 | +} |
0 commit comments