lsm6ds3 + kalman

labathome_firmware/components/lsm6ds3/CMakeLists.txt

@@ -0,0 +1,4 @@
+idf_component_register(SRCS
+    INCLUDE_DIRS "include"
+    REQUIRES "i2c" "esp_timer" "i2c_sensor"
+    )

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   :  [email protected]
+ */
+
+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; };
+    };
+}

labathome_firmware/components/lsm6ds3/include/kalman2lsm6ds3.hh

@@ -0,0 +1,85 @@
+#pragma once
+
+#include <cstring>
+#include <math.h>
+
+#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);
+        }
+    }
+}

labathome_firmware/components/lsm6ds3/include/lsm6ds3.hh

@@ -0,0 +1,114 @@
+#pragma once
+
+#include <cstdio>
+#include <cstdint>
+#include <i2c_sensor.hh>
+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;
+        }
+    }
+}