From 3031195ed2c8f43a394c64726ec3213ed0e44c6d Mon Sep 17 00:00:00 2001 From: Klaus Liebler Date: Sun, 15 Sep 2024 11:38:39 +0200 Subject: [PATCH] lsm6ds3 + kalman --- .../components/lsm6ds3/CMakeLists.txt | 4 + .../components/lsm6ds3/include/kalman.hh | 104 ++++++++++++++++ .../lsm6ds3/include/kalman2lsm6ds3.hh | 85 +++++++++++++ .../components/lsm6ds3/include/lsm6ds3.hh | 114 ++++++++++++++++++ 4 files changed, 307 insertions(+) create mode 100644 labathome_firmware/components/lsm6ds3/CMakeLists.txt create mode 100644 labathome_firmware/components/lsm6ds3/include/kalman.hh create mode 100644 labathome_firmware/components/lsm6ds3/include/kalman2lsm6ds3.hh create mode 100644 labathome_firmware/components/lsm6ds3/include/lsm6ds3.hh diff --git a/labathome_firmware/components/lsm6ds3/CMakeLists.txt b/labathome_firmware/components/lsm6ds3/CMakeLists.txt new file mode 100644 index 0000000..7a9396c --- /dev/null +++ b/labathome_firmware/components/lsm6ds3/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register(SRCS + INCLUDE_DIRS "include" + REQUIRES "i2c" "esp_timer" "i2c_sensor" + ) \ No newline at end of file diff --git a/labathome_firmware/components/lsm6ds3/include/kalman.hh b/labathome_firmware/components/lsm6ds3/include/kalman.hh new file mode 100644 index 0000000..26483ed --- /dev/null +++ b/labathome_firmware/components/lsm6ds3/include/kalman.hh @@ -0,0 +1,104 @@ +#pragma once +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +namespace filter +{ + class Kalman + { + private: + float Q_angle{0.001}; // Process noise variance for the accelerometer + float Q_bias{0.003}; // Process noise variance for the gyro bias + float R_measure{0.03}; // Measurement noise variance - this is actually the variance of the measurement noise + + float angle{0.0}; // The angle calculated by the Kalman filter - part of the 2x1 state vector + float bias{0.0}; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector + float rate{0.0}; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate + + float P[2][2]; // Error covariance matrix - This is a 2x2 matrix + + public: + Kalman() + { + P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical + P[0][1] = 0.0f; + P[1][0] = 0.0f; + P[1][1] = 0.0f; + } + + // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds + float getAngle(float newAngle, float newRate, float dt) + { + // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 + // Modified by Kristian Lauszus + // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it + + // Discrete Kalman filter time update equations - Time Update ("Predict") + // Update xhat - Project the state ahead + /* Step 1 */ + rate = newRate - bias; + angle += dt * rate; + + // Update estimation error covariance - Project the error covariance ahead + /* Step 2 */ + P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle); + P[0][1] -= dt * P[1][1]; + P[1][0] -= dt * P[1][1]; + P[1][1] += Q_bias * dt; + + // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") + // Calculate Kalman gain - Compute the Kalman gain + /* Step 4 */ + float S = P[0][0] + R_measure; // Estimate error + /* Step 5 */ + float K[2]; // Kalman gain - This is a 2x1 vector + K[0] = P[0][0] / S; + K[1] = P[1][0] / S; + + // Calculate angle and bias - Update estimate with measurement zk (newAngle) + /* Step 3 */ + float y = newAngle - angle; // Angle difference + /* Step 6 */ + angle += K[0] * y; + bias += K[1] * y; + + // Calculate estimation error covariance - Update the error covariance + /* Step 7 */ + float P00_temp = P[0][0]; + float P01_temp = P[0][1]; + + P[0][0] -= K[0] * P00_temp; + P[0][1] -= K[0] * P01_temp; + P[1][0] -= K[1] * P00_temp; + P[1][1] -= K[1] * P01_temp; + + return angle; + } + + void setAngle(float angle) { this->angle = angle; } // Used to set angle, this should be set as the starting angle + float getRate() { return this->rate; } // Return the unbiased rate + + /* These are used to tune the Kalman filter */ + void setQangle(float Q_angle) { this->Q_angle = Q_angle; }; + void setQbias(float Q_bias) { this->Q_bias = Q_bias; }; + void setRmeasure(float R_measure) { this->R_measure = R_measure; }; + + float getQangle() { return this->Q_angle; }; + float getQbias() { return this->Q_bias; }; + float getRmeasure() { return this->R_measure; }; + }; +} \ No newline at end of file diff --git a/labathome_firmware/components/lsm6ds3/include/kalman2lsm6ds3.hh b/labathome_firmware/components/lsm6ds3/include/kalman2lsm6ds3.hh new file mode 100644 index 0000000..dec145d --- /dev/null +++ b/labathome_firmware/components/lsm6ds3/include/kalman2lsm6ds3.hh @@ -0,0 +1,85 @@ +#pragma once + +#include +#include + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" + +#include "esp_timer.h" +#include "esp_log.h" + +#include "lsm6ds3.hh" +#include "kalman.hh" + +#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead +#define RAD_TO_DEG (180.0 / M_PI) +#define DEG_TO_RAD 0.0174533 + +namespace imu_kalmanXY +{ + class M + { + private: + Kalman kalmanX; // Create the Kalman instances + Kalman kalmanY; + int64_t lastTime{0}; + + double roll, pitch; // Roll and pitch are calculated using the accelerometer + double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter + public: + void getRollPitch(const float *acc, double *roll, double *pitch) + { + // atan2 outputs the value of - to (radians) - see http://en.wikipedia.org/wiki/Atan2 + // It is then converted from radians to degrees +#ifdef RESTRICT_PITCH // Eq. 25 and 26 + *roll = atan2(acc[1], acc[2]) * RAD_TO_DEG; + *pitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RAD_TO_DEG; +#else // Eq. 28 and 29 + *roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; + *pitch = atan2(-accX, accZ) * RAD_TO_DEG; +#endif + } + + void processSetup(lsm6ds3::M *imu) + { + + const float *acc = imu->GetAccXYZ(); + const float *gyro = imu->GetGyroXYZ(); + getRollPitch(accXYZ, &roll, &pitch); + kalAngleX = roll; + kalAngleY = pitch; + kalmanX.setAngle(roll); // Set starting angle + kalmanY.setAngle(pitch); + lastTime = esp_timer_get_time(); + } + + void processLoop(lsm6ds3::M *imu) + { + const float *acc = imu->GetAccXYZ(); + const float *gyro = imu->GetGyroXYZ(); + getRollPitch(accXYZ, &roll, &pitch); + int64_t now = esp_timer_get_time(); + + double dt = (double)(now - lastTime) / 1000000; // Calculate delta time + lastTime = now; + + /* Roll and pitch estimation */ + double gyroXrate = gyro[0]; + double gyroYrate = gyro[1]; + + // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees + if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) + { + kalmanX.setAngle(roll); + kalAngleX = roll; + } + else + kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter + + if (abs(kalAngleX) > 90) + gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading + kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); + } + } +} \ No newline at end of file diff --git a/labathome_firmware/components/lsm6ds3/include/lsm6ds3.hh b/labathome_firmware/components/lsm6ds3/include/lsm6ds3.hh new file mode 100644 index 0000000..a7b3cc7 --- /dev/null +++ b/labathome_firmware/components/lsm6ds3/include/lsm6ds3.hh @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include +namespace lsm6ds3 +{ + + constexpr uint8_t ADDRESS{0x6A}; + + constexpr uint8_t WHO_AM_I_REG{0X0F}; + constexpr uint8_t CTRL1_XL{0X10}; + constexpr uint8_t CTRL2_G{0X11}; + + constexpr uint8_t STATUS_REG{0X1E}; + + constexpr uint8_t CTRL6_C{0X15}; + constexpr uint8_t CTRL7_G{0X16}; + constexpr uint8_t CTRL8_XL{0X17}; + + constexpr uint8_t OUTX_L_G{0X22}; + constexpr uint8_t OUTX_H_G{0X23}; + constexpr uint8_t OUTY_L_G{0X24}; + constexpr uint8_t OUTY_H_G{0X25}; + constexpr uint8_t OUTZ_L_G{0X26}; + constexpr uint8_t OUTZ_H_G{0X27}; + + constexpr uint8_t OUTX_L_XL{0X28}; + constexpr uint8_t OUTX_H_XL{0X29}; + constexpr uint8_t OUTY_L_XL{0X2A}; + constexpr uint8_t OUTY_H_XL{0X2B}; + constexpr uint8_t OUTZ_L_XL{0X2C}; + constexpr uint8_t OUTZ_H_XL{0X2D}; + + class M : public I2CSensor + { + private: + float acc_xyz[3]; + float gyro_xyz[3]; + public: + M(i2c_master_bus_handle_t bus_handle):I2CSensor(bus_handle, ADDRESS){} + ErrorCode Trigger(int64_t &waitTillReadout) override{ + waitTillReadout=20; + return ErrorCode::OK + } + ErrorCode Readout(int64_t &waitTillNExtTrigger) override{ + // Results are in g (earth gravity). + int16_t data[6]; + RETURN_ON_ERRORCODE(ReadRegs8(OUTX_L_G, (uint8_t *)data, sizeof(data))); + + // Results are in degrees/second. + gyro_xyz[0] = data[0] / 131.0; + gyro_xyz[1] = data[1] / 131.0; + gyro_xyz[2] = data[2] / 131.0; + acc_xyz[0] = data[3] / 8192.0; + acc_xyz[1] = data[4] / 8192.0; + acc_xyz[2] = data[5] / 8192.0; + waitTillNExtTrigger=0; + return ErrorCode::OK; + } + // Precondition: dev_handle exists! + ErrorCode Initialize(int64_t &waitTillFirstTrigger) override + { + waitTillFirstTrigger=0; + uint8_t buf[8]; + RETURN_ON_ERRORCODE(ReadRegs8(WHO_AM_I_REG, buf, 1)); + if (*buf != 0x6C && *buf != 0x69) + { + return ErrorCode::UNKNOWN_HARDWARE_ID; + } + // Set the Accelerometer control register to work at 104 Hz, 4 g,and in bypass mode and enable ODR/4 + // low pass filter (check figure9 of LSM6DS3's datasheet) + RETURN_ON_ERRORCODE(this->WriteReg8(CTRL1_XL, 0x4A)); + + // set the gyroscope control register to work at 104 Hz, 250 dps and in bypass mode + RETURN_ON_ERRORCODE(this->WriteReg8(CTRL2_G, 0x40)); + + // set gyroscope power mode to high performance and bandwidth to 16 MHz + RETURN_ON_ERRORCODE(this->WriteReg8(CTRL7_G, 0x00)); + + // Set the ODR config register to ODR/4 + return this->WriteReg8(CTRL8_XL, 0x09); + } + + float accelerationSampleRate() { return 104.0F; } + + bool accelerationAvailable() + { + uint8_t buf{0}; + ReadRegs8(STATUS_REG, &buf, 1); + return buf & 0x01 == 1; + } + + const float* GetAccXYZ(){ + return this->acc_xyz; + } + + const float* GetGyroXYZ(){ + return this->gyro_xyz; + } + + float gyroscopeSampleRate() + { + return 104.0F; + } + + bool gyroscopeAvailable() + { + uint8_t buf{0}; + ReadRegs8(STATUS_REG, &buf, 1); + return buf & 0x02 == 1; + } + } +} \ No newline at end of file