From 66c569ac397a777327247eff8744b723ae126527 Mon Sep 17 00:00:00 2001 From: Igor89 Date: Thu, 24 Sep 2020 00:52:26 +0300 Subject: [PATCH 01/20] fix files according to arduino library specification --- README.md | 23 +- library.properties | 9 + GOST4401_81.cpp => src/GOST4401_81.cpp | 0 GOST4401_81.h => src/GOST4401_81.h | 0 LPS331.cpp => src/LPS331.cpp | 0 LPS331.h => src/LPS331.h | 0 MadgwickAHRS.cpp => src/MadgwickAHRS.cpp | 468 +++++++++++------------ MadgwickAHRS.h => src/MadgwickAHRS.h | 68 ++-- TroykaIMU.h => src/TroykaIMU.h | 0 l3g4200d.cpp => src/l3g4200d.cpp | 0 l3g4200d.h => src/l3g4200d.h | 0 lis331dlh.cpp => src/lis331dlh.cpp | 0 lis331dlh.h => src/lis331dlh.h | 0 lis3mdl.cpp => src/lis3mdl.cpp | 0 lis3mdl.h => src/lis3mdl.h | 0 stmhw.cpp => src/stmhw.cpp | 0 stmhw.h => src/stmhw.h | 0 troyka-imu.h => src/troyka-imu.h | 4 +- 18 files changed, 281 insertions(+), 291 deletions(-) create mode 100644 library.properties rename GOST4401_81.cpp => src/GOST4401_81.cpp (100%) mode change 100755 => 100644 rename GOST4401_81.h => src/GOST4401_81.h (100%) mode change 100755 => 100644 rename LPS331.cpp => src/LPS331.cpp (100%) mode change 100755 => 100644 rename LPS331.h => src/LPS331.h (100%) mode change 100755 => 100644 rename MadgwickAHRS.cpp => src/MadgwickAHRS.cpp (97%) mode change 100755 => 100644 rename MadgwickAHRS.h => src/MadgwickAHRS.h (96%) mode change 100755 => 100644 rename TroykaIMU.h => src/TroykaIMU.h (100%) mode change 100755 => 100644 rename l3g4200d.cpp => src/l3g4200d.cpp (100%) mode change 100755 => 100644 rename l3g4200d.h => src/l3g4200d.h (100%) mode change 100755 => 100644 rename lis331dlh.cpp => src/lis331dlh.cpp (100%) mode change 100755 => 100644 rename lis331dlh.h => src/lis331dlh.h (100%) mode change 100755 => 100644 rename lis3mdl.cpp => src/lis3mdl.cpp (100%) mode change 100755 => 100644 rename lis3mdl.h => src/lis3mdl.h (100%) mode change 100755 => 100644 rename stmhw.cpp => src/stmhw.cpp (100%) mode change 100755 => 100644 rename stmhw.h => src/stmhw.h (100%) mode change 100755 => 100644 rename troyka-imu.h => src/troyka-imu.h (96%) mode change 100755 => 100644 diff --git a/README.md b/README.md index 445e196..54411fd 100755 --- a/README.md +++ b/README.md @@ -1,22 +1,3 @@ -TroykaIMU -========== +# TroykaIMU -Библиотека для Arduino, позволяющая управлять [IMU-сенсором на 10 степеней свободы (Troyka-модуль)](http://amperka.ru/product/troyka-imu-10-dof) -от [Амперки](http://amperka.ru/). - -Установка библиотеки -==================== - -В Arduino IDE выберите пункт меню «Скетч» → «Импортировать библиотеку» → -«Добавить библиотеку…». В появившемся окне выберите скачаный архив с -библиотекой. Установка завершена. - -Подробности и примеры работы для [IMU-сенсора на 10 степеней свободы (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-imu-10-dof). - -Подробности и примеры работы для [акселерометра (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-accelerometer). - -Подробности и примеры работы для [гироскопа (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-gyro). - -Подробности и примеры работы для [компаса (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-compass). - -Подробности и примеры работы для [барометра (Troyka-модуль)](http://wiki.amperka.ru/продукты:troyka-barometer). \ No newline at end of file +Arduino library to interface with the [Amperka IMU 10-DOF Sensor](https://amperka.ru/product/troyka-imu-10-dof). diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..99ce4c9 --- /dev/null +++ b/library.properties @@ -0,0 +1,9 @@ +name=Troyka-IMU +version=1.1.0 +author=Igor Dementiev +maintainer=Amperka +sentence=Arduino library for Amperka IMU-sensor. +paragraph=Allows you to read the accelerometer, magnetometer, gyroscope and barometer values from the Amperka IMU 10-DOF Sensor. +category=Sensors +url=https://github.com/amperka/Troyka-IMU +architectures=* diff --git a/GOST4401_81.cpp b/src/GOST4401_81.cpp old mode 100755 new mode 100644 similarity index 100% rename from GOST4401_81.cpp rename to src/GOST4401_81.cpp diff --git a/GOST4401_81.h b/src/GOST4401_81.h old mode 100755 new mode 100644 similarity index 100% rename from GOST4401_81.h rename to src/GOST4401_81.h diff --git a/LPS331.cpp b/src/LPS331.cpp old mode 100755 new mode 100644 similarity index 100% rename from LPS331.cpp rename to src/LPS331.cpp diff --git a/LPS331.h b/src/LPS331.h old mode 100755 new mode 100644 similarity index 100% rename from LPS331.h rename to src/LPS331.h diff --git a/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp old mode 100755 new mode 100644 similarity index 97% rename from MadgwickAHRS.cpp rename to src/MadgwickAHRS.cpp index 790aecc..3919e0b --- a/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -1,235 +1,235 @@ -#include "MadgwickAHRS.h" -#include -#include - -Madgwick::Madgwick() { - -} - -void Madgwick::setKoeff(float sampleFreq, float beta) { - _beta = beta; - _sampleFreq = sampleFreq; -} -void Madgwick::reset() { - _q0 = 1.0; - _q1 = 0; - _q2 = 0; - _q3 = 0; -} - -void Madgwick::readQuaternions(float *q0, float *q1, float *q2, float *q3) { - *q0 = _q0; - *q1 = _q1; - *q2 = _q2; - *q3 = _q3; -} - -//--------------------------------------------------------------------------------------------------- -// AHRS algorithm update - -void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float hx, hy; - float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - - // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - update(gx, gy, gz, ax, ay, az); - return; - } - - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); - qDot2 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); - qDot3 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); - qDot4 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Normalise magnetometer measurement - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - _2q0mx = 2.0f * _q0 * mx; - _2q0my = 2.0f * _q0 * my; - _2q0mz = 2.0f * _q0 * mz; - _2q1mx = 2.0f * _q1 * mx; - _2q0 = 2.0f * _q0; - _2q1 = 2.0f * _q1; - _2q2 = 2.0f * _q2; - _2q3 = 2.0f * _q3; - _2q0q2 = 2.0f * _q0 * _q2; - _2q2q3 = 2.0f * _q2 * _q3; - q0q0 = _q0 * _q0; - q0q1 = _q0 * _q1; - q0q2 = _q0 * _q2; - q0q3 = _q0 * _q3; - q1q1 = _q1 * _q1; - q1q2 = _q1 * _q2; - q1q3 = _q1 * _q3; - q2q2 = _q2 * _q2; - q2q3 = _q2 * _q3; - q3q3 = _q3 * _q3; - - // Reference direction of Earth's magnetic field - hx = mx * q0q0 - _2q0my * _q3 + _2q0mz * _q2 + mx * q1q1 + _2q1 * my * _q2 + _2q1 * mz * _q3 - mx * q2q2 - mx * q3q3; - hy = _2q0mx * _q3 + my * q0q0 - _2q0mz * _q1 + _2q1mx * _q2 - my * q1q1 + my * q2q2 + _2q2 * mz * _q3 - my * q3q3; - _2bx = sqrt(hx * hx + hy * hy); - _2bz = -_2q0mx * _q2 + _2q0my * _q1 + mz * q0q0 + _2q1mx * _q3 - mz * q1q1 + _2q2 * my * _q3 - mz * q2q2 + mz * q3q3; - _4bx = 2.0f * _2bx; - _4bz = 2.0f * _2bz; - - // Gradient decent algorithm corrective step - s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * _q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * _q3 + _2bz * _q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * _q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * _q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * _q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * _q2 + _2bz * _q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * _q3 - _4bz * _q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * _q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * _q2 - _2bz * _q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * _q1 + _2bz * _q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * _q0 - _4bz * _q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * _q3 + _2bz * _q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * _q0 + _2bz * _q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * _q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - // Apply feedback step - qDot1 -= _beta * s0; - qDot2 -= _beta * s1; - qDot3 -= _beta * s2; - qDot4 -= _beta * s3; - } - - // Integrate rate of change of quaternion to yield quaternion - _q0 += qDot1 * (1.0f / _sampleFreq); - _q1 += qDot2 * (1.0f / _sampleFreq); - _q2 += qDot3 * (1.0f / _sampleFreq); - _q3 += qDot4 * (1.0f / _sampleFreq); - - // Normalise quaternion - recipNorm = invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); - _q0 *= recipNorm; - _q1 *= recipNorm; - _q2 *= recipNorm; - _q3 *= recipNorm; -} - -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update - -void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az) { - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); - qDot2 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); - qDot3 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); - qDot4 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - _2q0 = 2.0f * _q0; - _2q1 = 2.0f * _q1; - _2q2 = 2.0f * _q2; - _2q3 = 2.0f * _q3; - _4q0 = 4.0f * _q0; - _4q1 = 4.0f * _q1; - _4q2 = 4.0f * _q2; - _8q1 = 8.0f * _q1; - _8q2 = 8.0f * _q2; - q0q0 = _q0 * _q0; - q1q1 = _q1 * _q1; - q2q2 = _q2 * _q2; - q3q3 = _q3 * _q3; - - - // Gradient decent algorithm corrective step - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * _q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * _q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * _q3 - _2q1 * ax + 4.0f * q2q2 * _q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - // Apply feedback step - qDot1 -= _beta * s0; - qDot2 -= _beta * s1; - qDot3 -= _beta * s2; - qDot4 -= _beta * s3; - } - - - // Integrate rate of change of quaternion to yield quaternion - _q0 += qDot1 * (1.0f / _sampleFreq); - _q1 += qDot2 * (1.0f / _sampleFreq); - _q2 += qDot3 * (1.0f / _sampleFreq); - _q3 += qDot4 * (1.0f / _sampleFreq); - - // Normalise quaternion - recipNorm = invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); - _q0 *= recipNorm; - _q1 *= recipNorm; - _q2 *= recipNorm; - _q3 *= recipNorm; -} - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root - -float Madgwick::invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i >> 1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - return y; -} - -float Madgwick::getYawRad() { - return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3, 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1); -} - -float Madgwick::getPitchRad() { - return atan2(2 * _q2 * _q3 - 2 * _q0 * _q1, 2 * _q0 * _q0 + 2 * _q3 * _q3 - 1); -} - -float Madgwick::getRollRad() { - return -1 * atan2(2.0f * (_q0 * _q2 - _q1 * _q3), 1.0f - 2.0f * (_q2 * _q2 + _q1 *_q1 )); -} - -float Madgwick::getYawDeg() { - return getYawRad() * RAD_TO_DEG; -} - -float Madgwick::getPitchDeg() { - return getPitchRad() * RAD_TO_DEG; -} - -float Madgwick::getRollDeg() { - return getRollRad() * RAD_TO_DEG; +#include "MadgwickAHRS.h" +#include +#include + +Madgwick::Madgwick() { + +} + +void Madgwick::setKoeff(float sampleFreq, float beta) { + _beta = beta; + _sampleFreq = sampleFreq; +} +void Madgwick::reset() { + _q0 = 1.0; + _q1 = 0; + _q2 = 0; + _q3 = 0; +} + +void Madgwick::readQuaternions(float *q0, float *q1, float *q2, float *q3) { + *q0 = _q0; + *q1 = _q1; + *q2 = _q2; + *q3 = _q3; +} + +//--------------------------------------------------------------------------------------------------- +// AHRS algorithm update + +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + + // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) + if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + update(gx, gy, gz, ax, ay, az); + return; + } + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); + qDot2 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); + qDot3 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); + qDot4 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Normalise magnetometer measurement + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0mx = 2.0f * _q0 * mx; + _2q0my = 2.0f * _q0 * my; + _2q0mz = 2.0f * _q0 * mz; + _2q1mx = 2.0f * _q1 * mx; + _2q0 = 2.0f * _q0; + _2q1 = 2.0f * _q1; + _2q2 = 2.0f * _q2; + _2q3 = 2.0f * _q3; + _2q0q2 = 2.0f * _q0 * _q2; + _2q2q3 = 2.0f * _q2 * _q3; + q0q0 = _q0 * _q0; + q0q1 = _q0 * _q1; + q0q2 = _q0 * _q2; + q0q3 = _q0 * _q3; + q1q1 = _q1 * _q1; + q1q2 = _q1 * _q2; + q1q3 = _q1 * _q3; + q2q2 = _q2 * _q2; + q2q3 = _q2 * _q3; + q3q3 = _q3 * _q3; + + // Reference direction of Earth's magnetic field + hx = mx * q0q0 - _2q0my * _q3 + _2q0mz * _q2 + mx * q1q1 + _2q1 * my * _q2 + _2q1 * mz * _q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * _q3 + my * q0q0 - _2q0mz * _q1 + _2q1mx * _q2 - my * q1q1 + my * q2q2 + _2q2 * mz * _q3 - my * q3q3; + _2bx = sqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * _q2 + _2q0my * _q1 + mz * q0q0 + _2q1mx * _q3 - mz * q1q1 + _2q2 * my * _q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * _q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * _q3 + _2bz * _q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * _q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * _q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * _q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * _q2 + _2bz * _q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * _q3 - _4bz * _q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * _q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * _q2 - _2bz * _q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * _q1 + _2bz * _q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * _q0 - _4bz * _q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * _q3 + _2bz * _q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * _q0 + _2bz * _q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * _q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= _beta * s0; + qDot2 -= _beta * s1; + qDot3 -= _beta * s2; + qDot4 -= _beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + _q0 += qDot1 * (1.0f / _sampleFreq); + _q1 += qDot2 * (1.0f / _sampleFreq); + _q2 += qDot3 * (1.0f / _sampleFreq); + _q3 += qDot4 * (1.0f / _sampleFreq); + + // Normalise quaternion + recipNorm = invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); + _q0 *= recipNorm; + _q1 *= recipNorm; + _q2 *= recipNorm; + _q3 *= recipNorm; +} + +//--------------------------------------------------------------------------------------------------- +// IMU algorithm update + +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); + qDot2 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); + qDot3 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); + qDot4 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0f * _q0; + _2q1 = 2.0f * _q1; + _2q2 = 2.0f * _q2; + _2q3 = 2.0f * _q3; + _4q0 = 4.0f * _q0; + _4q1 = 4.0f * _q1; + _4q2 = 4.0f * _q2; + _8q1 = 8.0f * _q1; + _8q2 = 8.0f * _q2; + q0q0 = _q0 * _q0; + q1q1 = _q1 * _q1; + q2q2 = _q2 * _q2; + q3q3 = _q3 * _q3; + + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * _q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * _q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * _q3 - _2q1 * ax + 4.0f * q2q2 * _q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= _beta * s0; + qDot2 -= _beta * s1; + qDot3 -= _beta * s2; + qDot4 -= _beta * s3; + } + + + // Integrate rate of change of quaternion to yield quaternion + _q0 += qDot1 * (1.0f / _sampleFreq); + _q1 += qDot2 * (1.0f / _sampleFreq); + _q2 += qDot3 * (1.0f / _sampleFreq); + _q3 += qDot4 * (1.0f / _sampleFreq); + + // Normalise quaternion + recipNorm = invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); + _q0 *= recipNorm; + _q1 *= recipNorm; + _q2 *= recipNorm; + _q3 *= recipNorm; +} + +//--------------------------------------------------------------------------------------------------- +// Fast inverse square-root +// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root + +float Madgwick::invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i >> 1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} + +float Madgwick::getYawRad() { + return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3, 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1); +} + +float Madgwick::getPitchRad() { + return atan2(2 * _q2 * _q3 - 2 * _q0 * _q1, 2 * _q0 * _q0 + 2 * _q3 * _q3 - 1); +} + +float Madgwick::getRollRad() { + return -1 * atan2(2.0f * (_q0 * _q2 - _q1 * _q3), 1.0f - 2.0f * (_q2 * _q2 + _q1 *_q1 )); +} + +float Madgwick::getYawDeg() { + return getYawRad() * RAD_TO_DEG; +} + +float Madgwick::getPitchDeg() { + return getPitchRad() * RAD_TO_DEG; +} + +float Madgwick::getRollDeg() { + return getRollRad() * RAD_TO_DEG; } \ No newline at end of file diff --git a/MadgwickAHRS.h b/src/MadgwickAHRS.h old mode 100755 new mode 100644 similarity index 96% rename from MadgwickAHRS.h rename to src/MadgwickAHRS.h index a3487fa..440af6f --- a/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -1,34 +1,34 @@ -#ifndef MADGWICK_AHRS_H_ -#define MADGWICK_AHRS_H_ - -#include - -#define SAMPLE_FREQ 1000.0f // sample frequency in Hz -#define BETA_DEF 0.5f // 2 * proportional gain - -class Madgwick { - -public: - Madgwick(); - void readQuaternions(float *q0, float *q1, float *q2, float *q3); - void reset(); - void setKoeff(float sampleFreq, float beta); - void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); - void update(float gx, float gy, float gz, float ax, float ay, float az); - float getPitchRad(); - float getRollRad(); - float getYawRad(); - float getPitchDeg(); - float getRollDeg(); - float getYawDeg(); - -private: - float invSqrt(float x); - volatile float _beta = BETA_DEF; // algorithm gain - volatile float _sampleFreq = SAMPLE_FREQ; - volatile float _q0 = 1.0f; - volatile float _q1 = 0.0f; - volatile float _q2 = 0.0f; - volatile float _q3 = 0.0f; -}; -#endif +#ifndef MADGWICK_AHRS_H_ +#define MADGWICK_AHRS_H_ + +#include + +#define SAMPLE_FREQ 1000.0f // sample frequency in Hz +#define BETA_DEF 0.5f // 2 * proportional gain + +class Madgwick { + +public: + Madgwick(); + void readQuaternions(float *q0, float *q1, float *q2, float *q3); + void reset(); + void setKoeff(float sampleFreq, float beta); + void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); + void update(float gx, float gy, float gz, float ax, float ay, float az); + float getPitchRad(); + float getRollRad(); + float getYawRad(); + float getPitchDeg(); + float getRollDeg(); + float getYawDeg(); + +private: + float invSqrt(float x); + volatile float _beta = BETA_DEF; // algorithm gain + volatile float _sampleFreq = SAMPLE_FREQ; + volatile float _q0 = 1.0f; + volatile float _q1 = 0.0f; + volatile float _q2 = 0.0f; + volatile float _q3 = 0.0f; +}; +#endif diff --git a/TroykaIMU.h b/src/TroykaIMU.h old mode 100755 new mode 100644 similarity index 100% rename from TroykaIMU.h rename to src/TroykaIMU.h diff --git a/l3g4200d.cpp b/src/l3g4200d.cpp old mode 100755 new mode 100644 similarity index 100% rename from l3g4200d.cpp rename to src/l3g4200d.cpp diff --git a/l3g4200d.h b/src/l3g4200d.h old mode 100755 new mode 100644 similarity index 100% rename from l3g4200d.h rename to src/l3g4200d.h diff --git a/lis331dlh.cpp b/src/lis331dlh.cpp old mode 100755 new mode 100644 similarity index 100% rename from lis331dlh.cpp rename to src/lis331dlh.cpp diff --git a/lis331dlh.h b/src/lis331dlh.h old mode 100755 new mode 100644 similarity index 100% rename from lis331dlh.h rename to src/lis331dlh.h diff --git a/lis3mdl.cpp b/src/lis3mdl.cpp old mode 100755 new mode 100644 similarity index 100% rename from lis3mdl.cpp rename to src/lis3mdl.cpp diff --git a/lis3mdl.h b/src/lis3mdl.h old mode 100755 new mode 100644 similarity index 100% rename from lis3mdl.h rename to src/lis3mdl.h diff --git a/stmhw.cpp b/src/stmhw.cpp old mode 100755 new mode 100644 similarity index 100% rename from stmhw.cpp rename to src/stmhw.cpp diff --git a/stmhw.h b/src/stmhw.h old mode 100755 new mode 100644 similarity index 100% rename from stmhw.h rename to src/stmhw.h diff --git a/troyka-imu.h b/src/troyka-imu.h old mode 100755 new mode 100644 similarity index 96% rename from troyka-imu.h rename to src/troyka-imu.h index a78e331..21423aa --- a/troyka-imu.h +++ b/src/troyka-imu.h @@ -1,3 +1,3 @@ -#include "TroykaIMU.h" - +#include "TroykaIMU.h" + #warning "Deprecated: use TroykaIMU.h" \ No newline at end of file From 335158287a66166e1fb066b7c46f51dd6bf74d7d Mon Sep 17 00:00:00 2001 From: Igor89 Date: Thu, 24 Sep 2020 03:01:32 +0300 Subject: [PATCH 02/20] rename src files and classes --- src/{lis331dlh.cpp => Accelerometer.cpp} | 27 ++-- src/{lis331dlh.h => Accelerometer.h} | 16 +-- src/{LPS331.cpp => Barometer.cpp} | 3 +- src/{LPS331.h => Barometer.h} | 9 +- src/{lis3mdl.cpp => Compass.cpp} | 38 +++--- src/{lis3mdl.h => Compass.h} | 12 +- src/GOST4401_81.cpp | 159 ++++++++++++----------- src/GOST4401_81.h | 52 ++++---- src/{l3g4200d.cpp => Gyroscope.cpp} | 28 ++-- src/{l3g4200d.h => Gyroscope.h} | 16 +-- src/{stmhw.cpp => IMU.cpp} | 27 ++-- src/{stmhw.h => IMU.h} | 15 ++- src/MadgwickAHRS.h | 7 +- src/TroykaIMU.h | 29 ++--- src/troyka-imu.h | 3 - 15 files changed, 215 insertions(+), 226 deletions(-) rename src/{lis331dlh.cpp => Accelerometer.cpp} (74%) rename src/{lis331dlh.h => Accelerometer.h} (60%) rename src/{LPS331.cpp => Barometer.cpp} (99%) rename src/{LPS331.h => Barometer.h} (95%) rename src/{lis3mdl.cpp => Compass.cpp} (74%) rename src/{lis3mdl.h => Compass.h} (84%) rename src/{l3g4200d.cpp => Gyroscope.cpp} (73%) rename src/{l3g4200d.h => Gyroscope.h} (66%) rename src/{stmhw.cpp => IMU.cpp} (80%) rename src/{stmhw.h => IMU.h} (85%) delete mode 100644 src/troyka-imu.h diff --git a/src/lis331dlh.cpp b/src/Accelerometer.cpp similarity index 74% rename from src/lis331dlh.cpp rename to src/Accelerometer.cpp index d3e5139..8bdf3c0 100644 --- a/src/lis331dlh.cpp +++ b/src/Accelerometer.cpp @@ -1,5 +1,4 @@ -#include -#include "lis331dlh.h" +#include "Accelerometer.h" #define ADR_FS_2 0x00 #define ADR_FS_4 0x10 @@ -7,10 +6,10 @@ #define G 9.8 -LIS331DLH_TWI::LIS331DLH_TWI(uint8_t addr) : AxisHw(addr) { +LIS331DLH::LIS331DLH(uint8_t addr) : IMU(addr) { } -void LIS331DLH_TWI::begin() { +void LIS331DLH::begin() { // подключаемся к шине I²C WIRE_IMU.begin(); // включаем координаты x, y, z @@ -24,7 +23,7 @@ void LIS331DLH_TWI::begin() { writeCtrlReg1(); } -void LIS331DLH_TWI::setRange(uint8_t range) { +void LIS331DLH::setRange(uint8_t range) { switch (range) { case RANGE_2G: { _ctrlReg4 = ADR_FS_2; @@ -49,7 +48,7 @@ void LIS331DLH_TWI::setRange(uint8_t range) { writeCtrlReg4(); } -void LIS331DLH_TWI::sleep(bool enable) { +void LIS331DLH::sleep(bool enable) { if (enable) _ctrlReg1 &= ~(1 << 5); else @@ -58,31 +57,31 @@ void LIS331DLH_TWI::sleep(bool enable) { writeCtrlReg1(); } -float LIS331DLH_TWI::readGX() { +float LIS331DLH::readGX() { return readX()*_mult; } -float LIS331DLH_TWI::readGY() { +float LIS331DLH::readGY() { return readY()*_mult; } -float LIS331DLH_TWI::readGZ() { +float LIS331DLH::readGZ() { return readZ()*_mult; } -float LIS331DLH_TWI::readAX() { +float LIS331DLH::readAX() { return readGX() * G; } -float LIS331DLH_TWI::readAY() { +float LIS331DLH::readAY() { return readGY() * G; } -float LIS331DLH_TWI::readAZ() { +float LIS331DLH::readAZ() { return readGZ() * G; } -void LIS331DLH_TWI::readGXYZ(float *gx, float *gy, float *gz) { +void LIS331DLH::readGXYZ(float *gx, float *gy, float *gz) { int16_t x, y, z; readXYZ(&x, &y, &z); *gx = x * _mult; @@ -90,7 +89,7 @@ void LIS331DLH_TWI::readGXYZ(float *gx, float *gy, float *gz) { *gz = z * _mult; } -void LIS331DLH_TWI::readAXYZ(float *ax, float *ay, float *az) { +void LIS331DLH::readAXYZ(float *ax, float *ay, float *az) { readGXYZ(ax, ay, az); (*ax) *= G; (*ay) *= G; diff --git a/src/lis331dlh.h b/src/Accelerometer.h similarity index 60% rename from src/lis331dlh.h rename to src/Accelerometer.h index 22ddbbb..dabd532 100644 --- a/src/lis331dlh.h +++ b/src/Accelerometer.h @@ -1,19 +1,19 @@ -#ifndef LIS331DLH_H -#define LIS331DLH_H +#ifndef __ACCELEROMETER_H__ +#define __ACCELEROMETER_H__ -#include "stmhw.h" +#include "IMU.h" -#define LIS331DLH_TWI_ADDRESS 0b0011000 -#define LIS331DLH_TWI_ADDRESS_V2 0b0011001 +#define LIS331DLH_ADDRESS 0b0011000 +#define LIS331DLH_ADDRESS_V2 0b0011001 #define RANGE_2G 2 #define RANGE_4G 4 #define RANGE_8G 8 -class LIS331DLH_TWI : public AxisHw +class LIS331DLH : public IMU { public: - LIS331DLH_TWI(uint8_t addr = LIS331DLH_TWI_ADDRESS); + LIS331DLH(uint8_t addr = LIS331DLH_ADDRESS); void begin(); void sleep(bool enable); void setRange(uint8_t range); @@ -27,4 +27,4 @@ class LIS331DLH_TWI : public AxisHw void readAXYZ(float *gx, float *gy, float *gz); }; -#endif +#endif // __ACCELEROMETER_H__ diff --git a/src/LPS331.cpp b/src/Barometer.cpp similarity index 99% rename from src/LPS331.cpp rename to src/Barometer.cpp index d311595..c5b0cb4 100644 --- a/src/LPS331.cpp +++ b/src/Barometer.cpp @@ -1,5 +1,4 @@ -#include -#include +#include #include // Constructors ////////////////////////////////////////////////////// diff --git a/src/LPS331.h b/src/Barometer.h similarity index 95% rename from src/LPS331.h rename to src/Barometer.h index 134ca27..3064504 100644 --- a/src/LPS331.h +++ b/src/Barometer.h @@ -1,8 +1,7 @@ -#ifndef LPS331_h -#define LPS331_h +#ifndef __BAROMETER_H__ +#define __BAROMETER_H__ -#include // for byte data type -#include "stmhw.h" +#include "IMU.h" // The Arduino two-WIRE_IMU interface uses a 7-bit number for the address, // and sets the last bit correctly based on reads and writes @@ -87,4 +86,4 @@ class LPS331 bool testWhoAmI(void); }; -#endif +#endif // __BAROMETER_H__ diff --git a/src/lis3mdl.cpp b/src/Compass.cpp similarity index 74% rename from src/lis3mdl.cpp rename to src/Compass.cpp index 6fdb1c9..5b03f74 100644 --- a/src/lis3mdl.cpp +++ b/src/Compass.cpp @@ -1,6 +1,4 @@ -#include -#include -#include "lis3mdl.h" +#include "Compass.h" #define ADR_FS_4 0x00 #define ADR_FS_8 0x20 @@ -12,17 +10,17 @@ #define SENS_FS_12 2281 #define SENS_FS_16 1711 -LIS3MDL_TWI::LIS3MDL_TWI(uint8_t addr) : AxisHw(addr) { +LIS3MDL::LIS3MDL(uint8_t addr) : IMU(addr) { } -void LIS3MDL_TWI::begin() { +void LIS3MDL::begin() { WIRE_IMU.begin(); // устанавливаем чувствительность setRange(RANGE_4_GAUSS); writeCtrlReg3(); } -void LIS3MDL_TWI::setRange(uint8_t range) { +void LIS3MDL::setRange(uint8_t range) { switch (range) { case RANGE_4_GAUSS: { _ctrlReg2 = ADR_FS_4; @@ -52,7 +50,7 @@ void LIS3MDL_TWI::setRange(uint8_t range) { writeCtrlReg2(); } -void LIS3MDL_TWI::sleep(bool enable) { +void LIS3MDL::sleep(bool enable) { if (enable) _ctrlReg3 |= (3 << 0); else @@ -61,46 +59,46 @@ void LIS3MDL_TWI::sleep(bool enable) { writeCtrlReg3(); } -float LIS3MDL_TWI::readGaussX() { +float LIS3MDL::readGaussX() { return readX() / _mult; } -float LIS3MDL_TWI::readGaussY() { +float LIS3MDL::readGaussY() { return readY() / _mult; } -float LIS3MDL_TWI::readGaussZ() { +float LIS3MDL::readGaussZ() { return readZ() / _mult; } -float LIS3MDL_TWI::readCalibrateX() { +float LIS3MDL::readCalibrateX() { calibrate(); return _xCalibrate; } -float LIS3MDL_TWI::readCalibrateY() { +float LIS3MDL::readCalibrateY() { calibrate(); return _yCalibrate; } -float LIS3MDL_TWI::readCalibrateZ() { +float LIS3MDL::readCalibrateZ() { calibrate(); return _zCalibrate; } -float LIS3MDL_TWI::readCalibrateGaussX() { +float LIS3MDL::readCalibrateGaussX() { return readCalibrateX()/_mult; } -float LIS3MDL_TWI::readCalibrateGaussY() { +float LIS3MDL::readCalibrateGaussY() { return readCalibrateY()/_mult; } -float LIS3MDL_TWI::readCalibrateGaussZ() { +float LIS3MDL::readCalibrateGaussZ() { return readCalibrateZ()/_mult; } -void LIS3MDL_TWI::calibrate() { +void LIS3MDL::calibrate() { float result[3] = {0, 0, 0}; float uncalibratedValues[3]; uncalibratedValues[0] = readX() - _bias[0]; @@ -118,19 +116,19 @@ void LIS3MDL_TWI::calibrate() { _zCalibrate = result[2]; } -void LIS3MDL_TWI::calibrateMatrix(const double calibrationMatrix[3][3], const double bias[3]) { +void LIS3MDL::calibrateMatrix(const double calibrationMatrix[3][3], const double bias[3]) { memcpy (_bias, bias, 3 * sizeof (double)); memcpy (_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof (double)); } -void LIS3MDL_TWI::readCalibrateGaussXYZ(float *x, float *y, float *z) { +void LIS3MDL::readCalibrateGaussXYZ(float *x, float *y, float *z) { calibrate(); *x = _xCalibrate / _mult; *y = _yCalibrate / _mult; *z = _zCalibrate / _mult; } -float LIS3MDL_TWI::readAzimut() { +float LIS3MDL::readAzimut() { calibrate(); float heading = atan2(_yCalibrate, _xCalibrate); diff --git a/src/lis3mdl.h b/src/Compass.h similarity index 84% rename from src/lis3mdl.h rename to src/Compass.h index 80abeb3..c3259b8 100644 --- a/src/lis3mdl.h +++ b/src/Compass.h @@ -1,7 +1,7 @@ -#ifndef LIS3MDL_H -#define LIS3MDL_H +#ifndef __COMPASS_H__ +#define __COMPASS_H__ -#include "stmhw.h" +#include "IMU.h" #define LIS3MDL_TWI_ADDRESS 0b0011100 #define LIS3MDL_TWI_ADDRESS_V2 0b0011110 @@ -11,10 +11,10 @@ #define RANGE_12_GAUSS 2 #define RANGE_16_GAUSS 3 -class LIS3MDL_TWI : public AxisHw +class LIS3MDL : public IMU { public: - LIS3MDL_TWI(uint8_t addr = LIS3MDL_TWI_ADDRESS); + LIS3MDL(uint8_t addr = LIS3MDL_TWI_ADDRESS); void begin(); void sleep(bool enable); void setRange(uint8_t range); @@ -40,4 +40,4 @@ class LIS3MDL_TWI : public AxisHw double _bias[3]; }; -#endif +#endif // __COMPASS_H__ diff --git a/src/GOST4401_81.cpp b/src/GOST4401_81.cpp index 58a0c99..6e63236 100644 --- a/src/GOST4401_81.cpp +++ b/src/GOST4401_81.cpp @@ -1,83 +1,89 @@ #include #include -static float _GOST4401_geopotential2geometric(float altitude){ - return altitude * GOST4401_E / (GOST4401_E - altitude); +static float _GOST4401_geopotential2geometric(float altitude) { + return altitude * GOST4401_E / (GOST4401_E - altitude); } -static float _GOST4401_geometric2geopotential(float altitude){ - return altitude * GOST4401_E / (GOST4401_E + altitude); +static float _GOST4401_geometric2geopotential(float altitude) { + return altitude * GOST4401_E / (GOST4401_E + altitude); } - -/** +/** * Returns geometric altitude value for the given pressure. * * @param float pressurePa - pressure in pascals * @retval float geometric altitude in meters */ -float GOST4401_getAltitude(float pressurePa){ +float GOST4401_getAltitude(float pressurePa) { - if ((pressurePa <= GOST4401_MIN_PRESSURE) || (pressurePa > GOST4401_MAX_PRESSURE)) - return NAN; - - int idx = 0; + if ((pressurePa <= GOST4401_MIN_PRESSURE) + || (pressurePa > GOST4401_MAX_PRESSURE)) + return NAN; - for (idx = 0; idx < GOST4401_LUT_RECORDS - 1; idx++){ - if ((pressurePa <= ag_table[idx].press) && (pressurePa > ag_table[idx + 1].press)) - break; - } + int idx = 0; - float Ps = ag_table[idx].press; - float Bm = ag_table[idx].t_grad; - float Tm = ag_table[idx].temp; - float Hb = ag_table[idx].alt; - float geopotH = 0; + for (idx = 0; idx < GOST4401_LUT_RECORDS - 1; idx++) { + if ((pressurePa <= ag_table[idx].press) + && (pressurePa > ag_table[idx + 1].press)) + break; + } + float Ps = ag_table[idx].press; + float Bm = ag_table[idx].t_grad; + float Tm = ag_table[idx].temp; + float Hb = ag_table[idx].alt; + float geopotH = 0; - if (Bm != 0.0) { - geopotH = ((Tm * pow(Ps / pressurePa, Bm * GOST4401_R / GOST4401_G) - Tm) / Bm); - } else { - geopotH = log10(Ps / pressurePa) * (GOST4401_R * Tm) / (GOST4401_G * 0.434294); - } - - return _GOST4401_geopotential2geometric(Hb + geopotH); + if (Bm != 0.0) { + geopotH + = ((Tm * pow(Ps / pressurePa, Bm * GOST4401_R / GOST4401_G) - Tm) + / Bm); + } else { + geopotH = log10(Ps / pressurePa) * (GOST4401_R * Tm) + / (GOST4401_G * 0.434294); + } + + return _GOST4401_geopotential2geometric(Hb + geopotH); }; -/** +/** * Returns pressure in pascals for the given geometric altitude * * @param float altitude - geometric altitude in meters * @retval float - pressure in pascals */ -float GOST4401_getPressure(float altitude){ - float geopotH = _GOST4401_geometric2geopotential(altitude); - - if ((geopotH < GOST4401_MIN_GPALT) || (geopotH >= GOST4401_MAX_GPALT)) - return NAN; - - int idx = 0; - - for (idx = 1; idx < GOST4401_LUT_RECORDS - 1; idx++) { - if ((geopotH >= ag_table[idx].alt) && (geopotH < ag_table[idx].alt)) - break; - } - - float Ps = ag_table[idx].press; - float Bm = ag_table[idx].t_grad; - float Tm = ag_table[idx].temp; - float Hb = ag_table[idx].alt; - float lP = 0; - - if (Bm != 0.0) { - lP = log10(Ps) - (GOST4401_G / (Bm * GOST4401_R)) * log10((Tm + Bm * (geopotH - Hb)) / Tm); +float GOST4401_getPressure(float altitude) { + float geopotH = _GOST4401_geometric2geopotential(altitude); + + if ((geopotH < GOST4401_MIN_GPALT) || (geopotH >= GOST4401_MAX_GPALT)) + return NAN; + + int idx = 0; + + for (idx = 1; idx < GOST4401_LUT_RECORDS - 1; idx++) { + if ((geopotH >= ag_table[idx].alt) && (geopotH < ag_table[idx].alt)) + break; + } + + float Ps = ag_table[idx].press; + float Bm = ag_table[idx].t_grad; + float Tm = ag_table[idx].temp; + float Hb = ag_table[idx].alt; + float lP = 0; + + if (Bm != 0.0) { + lP = log10(Ps) + - (GOST4401_G / (Bm * GOST4401_R)) + * log10((Tm + Bm * (geopotH - Hb)) / Tm); } else { - lP = log10(Ps) - 0.434294 *(GOST4401_G * (geopotH - Hb)) / (GOST4401_R * Tm); - } - - return pow(10, lP); + lP = log10(Ps) + - 0.434294 * (GOST4401_G * (geopotH - Hb)) / (GOST4401_R * Tm); + } + + return pow(10, lP); } /** @@ -87,28 +93,27 @@ float GOST4401_getPressure(float altitude){ * @retval float - temperature in degrees K */ -float GOST4401_getTemperature(float altitude){ - float geopotH = _GOST4401_geometric2geopotential(altitude); - - if ((geopotH < GOST4401_MIN_GPALT) || (geopotH >= GOST4401_MAX_GPALT)) - return NAN; - - int idx = 0; - - for (idx = 1; idx < GOST4401_LUT_RECORDS - 1; idx++) { - if ((geopotH >= ag_table[idx].alt) && (geopotH < ag_table[idx].alt)) - break; - } - - float Bm = ag_table[idx].t_grad; - float Tm = ag_table[idx].temp; - float Hb = ag_table[idx].alt; - float temp = Tm; - - if (Bm != 0.0){ - temp += Bm * (geopotH - Hb); - } - - return temp; -} +float GOST4401_getTemperature(float altitude) { + float geopotH = _GOST4401_geometric2geopotential(altitude); + + if ((geopotH < GOST4401_MIN_GPALT) || (geopotH >= GOST4401_MAX_GPALT)) + return NAN; + + int idx = 0; + for (idx = 1; idx < GOST4401_LUT_RECORDS - 1; idx++) { + if ((geopotH >= ag_table[idx].alt) && (geopotH < ag_table[idx].alt)) + break; + } + + float Bm = ag_table[idx].t_grad; + float Tm = ag_table[idx].temp; + float Hb = ag_table[idx].alt; + float temp = Tm; + + if (Bm != 0.0) { + temp += Bm * (geopotH - Hb); + } + + return temp; +} diff --git a/src/GOST4401_81.h b/src/GOST4401_81.h index ca6274a..15640d1 100644 --- a/src/GOST4401_81.h +++ b/src/GOST4401_81.h @@ -1,56 +1,50 @@ /** - * Partial implementation of standard atmospheric model as described in - * GOST 4401-81 useful for processing of data from meteorological balloon + * Partial implementation of standard atmospheric model as described in + * GOST 4401-81 useful for processing of data from meteorological balloon * sensors. * * Supported modelling of temperature and pressure over the altitude span from * 0 up to 51km. - * + * * by Oleg Kochetov */ -#ifndef _ATMOSPHERE_GOST4401_H_ -#define _ATMOSPHERE_GOST4401_H_ +#ifndef __ATMOSPHERE_GOST4401_H__ +#define __ATMOSPHERE_GOST4401_H__ #include - // Constants -#define GOST4401_G 9.80665 -#define GOST4401_R 287.05287 -#define GOST4401_E 6356766 +#define GOST4401_G 9.80665 +#define GOST4401_R 287.05287 +#define GOST4401_E 6356766 -#define GOST4401_MIN_PRESSURE 6.69384 -#define GOST4401_MAX_PRESSURE 101325.00 - -#define GOST4401_MIN_GPALT 0.00 -#define GOST4401_MAX_GPALT 51000.00 +#define GOST4401_MIN_PRESSURE 6.69384 +#define GOST4401_MAX_PRESSURE 101325.00 +#define GOST4401_MIN_GPALT 0.00 +#define GOST4401_MAX_GPALT 51000.00 // Export functions float GOST4401_getAltitude(float pressurePa); float GOST4401_getPressure(float altitude); float GOST4401_getTemperature(float altitude); - // Structure for lookup table record -typedef struct __attribute__((packed)) _GOST4401_RECORD{ - float alt; // Geopotentional altitude - float temp; // degrees K - float t_grad; // degrees K per meter - float press; // pascals +typedef struct __attribute__((packed)) _GOST4401_RECORD { + float alt; // Geopotentional altitude + float temp; // degrees K + float t_grad; // degrees K per meter + float press; // pascals } GOST4401_RECORD; -// Lookup table with averaged empirical parameters for +// Lookup table with averaged empirical parameters for // lower layers of atmosphere in accordance with GOST 4401-81 -#define GOST4401_LUT_RECORDS 6 +#define GOST4401_LUT_RECORDS 6 static const GOST4401_RECORD ag_table[] = { - { 0, 288.15, -0.0065, 101325.00 }, - { 11000, 216.65, 0.0, 22632.04 }, - { 20000, 216.65, 0.0010, 5474.87 }, - { 32000, 228.65, 0.0028, 868.0146 }, - { 47000, 270.65, 0.0, 110.9056 }, - { 51000, 270.65, -0.0028, 6.69384 } + { 0, 288.15, -0.0065, 101325.00 }, { 11000, 216.65, 0.0, 22632.04 }, + { 20000, 216.65, 0.0010, 5474.87 }, { 32000, 228.65, 0.0028, 868.0146 }, + { 47000, 270.65, 0.0, 110.9056 }, { 51000, 270.65, -0.0028, 6.69384 } }; -#endif \ No newline at end of file +#endif // __ATMOSPHERE_GOST4401_H__ \ No newline at end of file diff --git a/src/l3g4200d.cpp b/src/Gyroscope.cpp similarity index 73% rename from src/l3g4200d.cpp rename to src/Gyroscope.cpp index e10bad0..71a19dc 100644 --- a/src/l3g4200d.cpp +++ b/src/Gyroscope.cpp @@ -1,6 +1,4 @@ -#include -#include -#include "l3g4200d.h" +#include "Gyroscope.h" #define ADR_FS_250 0x00 #define ADR_FS_500 0x10 @@ -10,10 +8,10 @@ #define SENS_FS_500 0.0175 #define SENS_FS_2000 0.07 -L3G4200D_TWI::L3G4200D_TWI(uint8_t addr) : AxisHw(addr) { +L3G4200D::L3G4200D(uint8_t addr) : IMU(addr) { } -void L3G4200D_TWI::setRange(uint8_t range) { +void L3G4200D::setRange(uint8_t range) { switch (range) { case RANGE_250DPS: { _ctrlReg4 = ADR_FS_250; @@ -38,7 +36,7 @@ void L3G4200D_TWI::setRange(uint8_t range) { writeCtrlReg4(); } -void L3G4200D_TWI::begin() { +void L3G4200D::begin() { // подключаемся к шине I²C WIRE_IMU.begin(); // включаем координаты x, y, z @@ -53,7 +51,7 @@ void L3G4200D_TWI::begin() { } -void L3G4200D_TWI::sleep(bool enable) { +void L3G4200D::sleep(bool enable) { if (enable) _ctrlReg1 &= ~(1 << 3); else @@ -62,31 +60,31 @@ void L3G4200D_TWI::sleep(bool enable) { writeCtrlReg1(); } -float L3G4200D_TWI::readDegPerSecX() { +float L3G4200D::readDegPerSecX() { return readX() * _mult; } -float L3G4200D_TWI::readDegPerSecY() { +float L3G4200D::readDegPerSecY() { return readY() * _mult; } -float L3G4200D_TWI::readDegPerSecZ() { +float L3G4200D::readDegPerSecZ() { return readZ() * _mult; } -float L3G4200D_TWI::readRadPerSecX() { +float L3G4200D::readRadPerSecX() { return readDegPerSecX() * DEG_TO_RAD; } -float L3G4200D_TWI::readRadPerSecY() { +float L3G4200D::readRadPerSecY() { return readDegPerSecY() * DEG_TO_RAD; } -float L3G4200D_TWI::readRadPerSecZ() { +float L3G4200D::readRadPerSecZ() { return readDegPerSecZ() * DEG_TO_RAD; } -void L3G4200D_TWI::readDegPerSecXYZ(float *gx, float *gy, float *gz) { +void L3G4200D::readDegPerSecXYZ(float *gx, float *gy, float *gz) { int16_t x, y, z; readXYZ(&x, &y, &z); *gx = x * _mult; @@ -94,7 +92,7 @@ void L3G4200D_TWI::readDegPerSecXYZ(float *gx, float *gy, float *gz) { *gz = z * _mult; } -void L3G4200D_TWI::readRadPerSecXYZ(float *gx, float *gy, float *gz) { +void L3G4200D::readRadPerSecXYZ(float *gx, float *gy, float *gz) { readDegPerSecXYZ(gx, gy, gz); (*gx) *= DEG_TO_RAD; (*gy) *= DEG_TO_RAD; diff --git a/src/l3g4200d.h b/src/Gyroscope.h similarity index 66% rename from src/l3g4200d.h rename to src/Gyroscope.h index 0b53b82..91ba0f3 100644 --- a/src/l3g4200d.h +++ b/src/Gyroscope.h @@ -1,19 +1,19 @@ -#ifndef L3G4200D_H -#define L3G4200D_H +#ifndef __GYROSCOPE_H__ +#define __GYROSCOPE_H__ -#include "stmhw.h" +#include "IMU.h" -#define L3G4200D_TWI_ADDRESS 0b01101000 -#define L3G4200D_TWI_ADDRESS_V2 0b01101001 +#define L3G4200D_ADDRESS 0b01101000 +#define L3G4200D_ADDRESS_V2 0b01101001 #define RANGE_250DPS 0 #define RANGE_500DPS 1 #define RANGE_2000DPS 2 -class L3G4200D_TWI : public AxisHw +class L3G4200D : public IMU { public: - L3G4200D_TWI(uint8_t addr = L3G4200D_TWI_ADDRESS); + L3G4200D(uint8_t addr = L3G4200D_ADDRESS); void begin(); void sleep(bool enable); void setRange(uint8_t range); @@ -27,4 +27,4 @@ class L3G4200D_TWI : public AxisHw void readRadPerSecXYZ(float *gx, float *gy, float *gz); }; -#endif +#endif // __GYROSCOPE_H__ diff --git a/src/stmhw.cpp b/src/IMU.cpp similarity index 80% rename from src/stmhw.cpp rename to src/IMU.cpp index 332ffe3..4719e86 100644 --- a/src/stmhw.cpp +++ b/src/IMU.cpp @@ -1,5 +1,4 @@ -#include "stmhw.h" -#include +#include "IMU.h" #define CTRL_REG1 0x20 #define CTRL_REG2 0x21 @@ -11,28 +10,28 @@ #define OUT_Y 0x2A #define OUT_Z 0x2C -int16_t AxisHw::readX() { +int16_t IMU::readX() { return readAxis(OUT_X); } -int16_t AxisHw::readY() { +int16_t IMU::readY() { return readAxis(OUT_Y); } -int16_t AxisHw::readZ() { +int16_t IMU::readZ() { return readAxis(OUT_Z); } -int16_t AxisHw::readAxis(uint8_t reg) { +int16_t IMU::readAxis(uint8_t reg) { return ((int16_t)readByte(reg + 1) << 8) | readByte(reg); } -inline void AxisHw::waitForData() { +inline void IMU::waitForData() { while (WIRE_IMU.available() < 1) continue; } -uint8_t AxisHw::readByte(uint8_t reg) { +uint8_t IMU::readByte(uint8_t reg) { uint8_t value; WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(reg); @@ -43,7 +42,7 @@ uint8_t AxisHw::readByte(uint8_t reg) { return value; } -void AxisHw::readXYZ(int16_t *x, int16_t *y, int16_t *z) { +void IMU::readXYZ(int16_t *x, int16_t *y, int16_t *z) { WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(OUT_X | (1 << 7)); // assert MSB to enable register address auto-increment WIRE_IMU.endTransmission(); @@ -61,35 +60,35 @@ void AxisHw::readXYZ(int16_t *x, int16_t *y, int16_t *z) { *z = *((int16_t*)&values[4]); } -void AxisHw::writeCtrlReg1(){ +void IMU::writeCtrlReg1(){ WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG1); WIRE_IMU.write(_ctrlReg1); WIRE_IMU.endTransmission(); } -void AxisHw::writeCtrlReg2(){ +void IMU::writeCtrlReg2(){ WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG2); WIRE_IMU.write(_ctrlReg2); WIRE_IMU.endTransmission(); } -void AxisHw::writeCtrlReg3(){ +void IMU::writeCtrlReg3(){ WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG3); WIRE_IMU.write(_ctrlReg3); WIRE_IMU.endTransmission(); } -void AxisHw::writeCtrlReg4(){ +void IMU::writeCtrlReg4(){ WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG4); WIRE_IMU.write(_ctrlReg4); WIRE_IMU.endTransmission(); } -void AxisHw::writeCtrlReg5(){ +void IMU::writeCtrlReg5(){ WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG5); WIRE_IMU.write(_ctrlReg5); diff --git a/src/stmhw.h b/src/IMU.h similarity index 85% rename from src/stmhw.h rename to src/IMU.h index 2ae1e87..1466d0d 100644 --- a/src/stmhw.h +++ b/src/IMU.h @@ -1,5 +1,8 @@ -#ifndef HW_H -#define HW_H +#ifndef __IMU_H__ +#define __IMU_H__ + +#include +#include #if defined(__AVR__) || defined(__SAMD21G18A__) || defined(ESP8266) #define WIRE_IMU Wire @@ -8,9 +11,7 @@ #define WIRE_IMU Wire1 #endif -#include -#include -class AxisHw +class IMU { private: uint8_t _addr; @@ -18,7 +19,7 @@ class AxisHw static inline void waitForData(); public: - AxisHw (uint8_t addr) : _addr (addr) {} + IMU(uint8_t addr) : _addr (addr) {} uint8_t readByte(uint8_t reg); int16_t readX(); @@ -43,4 +44,4 @@ class AxisHw int16_t readAxis(uint8_t reg); }; -#endif //HW_H +#endif // __IMU_H__ diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 440af6f..99d0be6 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -1,5 +1,5 @@ -#ifndef MADGWICK_AHRS_H_ -#define MADGWICK_AHRS_H_ +#ifndef __MADGWICK_AHRS_H__ +#define __MADGWICK_AHRS_H__ #include @@ -31,4 +31,5 @@ class Madgwick { volatile float _q2 = 0.0f; volatile float _q3 = 0.0f; }; -#endif + +#endif // __MADGWICK_AHRS_H__ diff --git a/src/TroykaIMU.h b/src/TroykaIMU.h index 7b39cb3..e1490c6 100644 --- a/src/TroykaIMU.h +++ b/src/TroykaIMU.h @@ -1,17 +1,16 @@ -#include -#include "lis331dlh.h" -#include "l3g4200d.h" -#include "lis3mdl.h" -#include "LPS331.h" +#include "Accelerometer.h" +#include "Gyroscope.h" +#include "Compass.h" +#include "Barometer.h" #include "MadgwickAHRS.h" // Accelerometer -#define ACCEL_ADDRESS_V1 LIS331DLH_TWI_ADDRESS -#define ACCEL_ADDRESS_V2 LIS331DLH_TWI_ADDRESS_V2 +#define ACCEL_ADDRESS_V1 LIS331DLH_ADDRESS +#define ACCEL_ADDRESS_V2 LIS331DLH_ADDRESS_V2 // Gyroscope -#define GYRO_ADDRESS_V1 L3G4200D_TWI_ADDRESS -#define GYRO_ADDRESS_V2 L3G4200D_TWI_ADDRESS_V2 +#define GYRO_ADDRESS_V1 L3G4200D_ADDRESS +#define GYRO_ADDRESS_V2 L3G4200D_ADDRESS_V2 // Compass #define COMPASS_ADDRESS_V1 LIS3MDL_TWI_ADDRESS @@ -21,25 +20,25 @@ #define BARO_ADDRESS_V1 LPS331AP_TWI_ADDRESS #define BARO_ADDRESS_V2 LPS331AP_TWI_ADDRESS_V2 -class Accelerometer : public LIS331DLH_TWI +class Accelerometer : public LIS331DLH { public: Accelerometer(uint8_t addr = ACCEL_ADDRESS_V1) : - LIS331DLH_TWI(addr) {} + LIS331DLH(addr) {} }; -class Gyroscope : public L3G4200D_TWI +class Gyroscope : public L3G4200D { public: Gyroscope(uint8_t addr = GYRO_ADDRESS_V1) : - L3G4200D_TWI(addr) {} + L3G4200D(addr) {} }; -class Compass : public LIS3MDL_TWI +class Compass : public LIS3MDL { public: Compass(uint8_t addr = COMPASS_ADDRESS_V1) : - LIS3MDL_TWI(addr) {} + LIS3MDL(addr) {} }; class Barometer : public LPS331 diff --git a/src/troyka-imu.h b/src/troyka-imu.h deleted file mode 100644 index 21423aa..0000000 --- a/src/troyka-imu.h +++ /dev/null @@ -1,3 +0,0 @@ -#include "TroykaIMU.h" - -#warning "Deprecated: use TroykaIMU.h" \ No newline at end of file From 60b3972b750eb73cdf7c152243bf8ab5c3349d80 Mon Sep 17 00:00:00 2001 From: Igor89 Date: Thu, 24 Sep 2020 03:17:49 +0300 Subject: [PATCH 03/20] clang-format --- src/Accelerometer.cpp | 75 ++++++++---------- src/Accelerometer.h | 17 ++--- src/Barometer.cpp | 174 ++++++++++++++++++++---------------------- src/Barometer.h | 64 ++++++++-------- src/Compass.cpp | 106 ++++++++++++------------- src/Compass.h | 20 ++--- src/Gyroscope.cpp | 80 ++++++++----------- src/Gyroscope.h | 17 ++--- src/IMU.cpp | 50 ++++++------ src/IMU.h | 8 +- src/MadgwickAHRS.cpp | 121 +++++++++++++++++++---------- src/MadgwickAHRS.h | 11 +-- src/TroykaIMU.h | 58 +++++++------- 13 files changed, 391 insertions(+), 410 deletions(-) diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index 8bdf3c0..5ec0c77 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -1,13 +1,13 @@ #include "Accelerometer.h" -#define ADR_FS_2 0x00 -#define ADR_FS_4 0x10 -#define ADR_FS_8 0x30 +#define ADR_FS_2 0x00 +#define ADR_FS_4 0x10 +#define ADR_FS_8 0x30 -#define G 9.8 +#define G 9.8 -LIS331DLH::LIS331DLH(uint8_t addr) : IMU(addr) { -} +LIS331DLH::LIS331DLH(uint8_t addr) + : IMU(addr) { } void LIS331DLH::begin() { // подключаемся к шине I²C @@ -25,26 +25,25 @@ void LIS331DLH::begin() { void LIS331DLH::setRange(uint8_t range) { switch (range) { - case RANGE_2G: { - _ctrlReg4 = ADR_FS_2; - _mult = RANGE_2G / 32767.0; - break; - } - case RANGE_4G: { - _ctrlReg4 = ADR_FS_4; - _mult = RANGE_4G / 32767.0; - break; - } - case RANGE_8G: { - _ctrlReg4 = ADR_FS_8; - _mult = RANGE_8G / 32767.0; - break; - } - default: { - _mult = RANGE_2G / 32767.0; - } + case RANGE_2G: { + _ctrlReg4 = ADR_FS_2; + _mult = RANGE_2G / 32767.0; + break; + } + case RANGE_4G: { + _ctrlReg4 = ADR_FS_4; + _mult = RANGE_4G / 32767.0; break; } + case RANGE_8G: { + _ctrlReg4 = ADR_FS_8; + _mult = RANGE_8G / 32767.0; + break; + } + default: { + _mult = RANGE_2G / 32767.0; + } break; + } writeCtrlReg4(); } @@ -57,31 +56,19 @@ void LIS331DLH::sleep(bool enable) { writeCtrlReg1(); } -float LIS331DLH::readGX() { - return readX()*_mult; -} +float LIS331DLH::readGX() { return readX() * _mult; } -float LIS331DLH::readGY() { - return readY()*_mult; -} +float LIS331DLH::readGY() { return readY() * _mult; } -float LIS331DLH::readGZ() { - return readZ()*_mult; -} +float LIS331DLH::readGZ() { return readZ() * _mult; } -float LIS331DLH::readAX() { - return readGX() * G; -} +float LIS331DLH::readAX() { return readGX() * G; } -float LIS331DLH::readAY() { - return readGY() * G; -} +float LIS331DLH::readAY() { return readGY() * G; } -float LIS331DLH::readAZ() { - return readGZ() * G; -} +float LIS331DLH::readAZ() { return readGZ() * G; } -void LIS331DLH::readGXYZ(float *gx, float *gy, float *gz) { +void LIS331DLH::readGXYZ(float* gx, float* gy, float* gz) { int16_t x, y, z; readXYZ(&x, &y, &z); *gx = x * _mult; @@ -89,7 +76,7 @@ void LIS331DLH::readGXYZ(float *gx, float *gy, float *gz) { *gz = z * _mult; } -void LIS331DLH::readAXYZ(float *ax, float *ay, float *az) { +void LIS331DLH::readAXYZ(float* ax, float* ay, float* az) { readGXYZ(ax, ay, az); (*ax) *= G; (*ay) *= G; diff --git a/src/Accelerometer.h b/src/Accelerometer.h index dabd532..518f6a0 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -3,15 +3,14 @@ #include "IMU.h" -#define LIS331DLH_ADDRESS 0b0011000 -#define LIS331DLH_ADDRESS_V2 0b0011001 +#define LIS331DLH_ADDRESS 0b0011000 +#define LIS331DLH_ADDRESS_V2 0b0011001 -#define RANGE_2G 2 -#define RANGE_4G 4 -#define RANGE_8G 8 +#define RANGE_2G 2 +#define RANGE_4G 4 +#define RANGE_8G 8 -class LIS331DLH : public IMU -{ +class LIS331DLH : public IMU { public: LIS331DLH(uint8_t addr = LIS331DLH_ADDRESS); void begin(); @@ -23,8 +22,8 @@ class LIS331DLH : public IMU float readAX(); float readAY(); float readAZ(); - void readGXYZ(float *ax, float *ay, float *az); - void readAXYZ(float *gx, float *gy, float *gz); + void readGXYZ(float* ax, float* ay, float* az); + void readAXYZ(float* gx, float* gy, float* gz); }; #endif // __ACCELEROMETER_H__ diff --git a/src/Barometer.cpp b/src/Barometer.cpp index c5b0cb4..6c71e39 100644 --- a/src/Barometer.cpp +++ b/src/Barometer.cpp @@ -3,130 +3,118 @@ // Constructors ////////////////////////////////////////////////////// -LPS331::LPS331(uint8_t addr) -{ - address = addr; -} +LPS331::LPS331(uint8_t addr) { address = addr; } // Public Methods //////////////////////////////////////////////////// // sets or detects slave address; returns bool indicating success -void LPS331::begin() -{ - WIRE_IMU.begin(); - writeReg(LPS331_CTRL_REG1, 0b11100000); - delay(100); +void LPS331::begin() { + WIRE_IMU.begin(); + writeReg(LPS331_CTRL_REG1, 0b11100000); + delay(100); } // writes register -void LPS331::writeReg(byte reg, byte value) -{ - WIRE_IMU.beginTransmission(address); - WIRE_IMU.write(reg); - WIRE_IMU.write(value); - WIRE_IMU.endTransmission(); +void LPS331::writeReg(byte reg, byte value) { + WIRE_IMU.beginTransmission(address); + WIRE_IMU.write(reg); + WIRE_IMU.write(value); + WIRE_IMU.endTransmission(); } // reads register -byte LPS331::readReg(byte reg) -{ - byte value; +byte LPS331::readReg(byte reg) { + byte value; - WIRE_IMU.beginTransmission(address); - WIRE_IMU.write(reg); - WIRE_IMU.endTransmission(false); // restart - WIRE_IMU.requestFrom(address, (byte)1); - value = WIRE_IMU.read(); - WIRE_IMU.endTransmission(); + WIRE_IMU.beginTransmission(address); + WIRE_IMU.write(reg); + WIRE_IMU.endTransmission(false); // restart + WIRE_IMU.requestFrom(address, (byte)1); + value = WIRE_IMU.read(); + WIRE_IMU.endTransmission(); - return value; + return value; } // read pressure in pascals -float LPS331::readPressurePascals(void) -{ - return (float)readPressureRaw() / 40.96; +float LPS331::readPressurePascals(void) { + return (float)readPressureRaw() / 40.96; } // reads pressure in millibars (mbar)/hectopascals (hPa) -float LPS331::readPressureMillibars(void) -{ - return (float)readPressureRaw() / 4096; +float LPS331::readPressureMillibars(void) { + return (float)readPressureRaw() / 4096; } // reads pressure in inches of mercury (inHg) -float LPS331::readPressureInchesHg(void) -{ - return (float)readPressureRaw() / 138706.5; +float LPS331::readPressureInchesHg(void) { + return (float)readPressureRaw() / 138706.5; } // reads pressure in millimeters of mercury (mmHg) -float LPS331::readPressureMillimetersHg(void) -{ - // 1 mbar * 0,75006168270417 = 1 mmHg - return (float)(readPressureRaw()) * 0.75006375541921 / 4096.0; +float LPS331::readPressureMillimetersHg(void) { + // 1 mbar * 0,75006168270417 = 1 mmHg + return (float)(readPressureRaw()) * 0.75006375541921 / 4096.0; } // reads pressure and returns raw 24-bit sensor output -int32_t LPS331::readPressureRaw(void) -{ - WIRE_IMU.beginTransmission(address); - // assert MSB to enable register address auto-increment - WIRE_IMU.write(LPS331_PRESS_OUT_XL | (1 << 7)); - WIRE_IMU.endTransmission(); - WIRE_IMU.requestFrom(address, (byte)3); +int32_t LPS331::readPressureRaw(void) { + WIRE_IMU.beginTransmission(address); + // assert MSB to enable register address auto-increment + WIRE_IMU.write(LPS331_PRESS_OUT_XL | (1 << 7)); + WIRE_IMU.endTransmission(); + WIRE_IMU.requestFrom(address, (byte)3); - while (WIRE_IMU.available() < 3); + while (WIRE_IMU.available() < 3) + ; - uint8_t pxl = WIRE_IMU.read(); - uint8_t pl = WIRE_IMU.read(); - uint8_t ph = WIRE_IMU.read(); + uint8_t pxl = WIRE_IMU.read(); + uint8_t pl = WIRE_IMU.read(); + uint8_t ph = WIRE_IMU.read(); - // combine bytes - return (int32_t)(int8_t)ph << 16 | (uint16_t)pl << 8 | pxl; + // combine bytes + return (int32_t)(int8_t)ph << 16 | (uint16_t)pl << 8 | pxl; } // reads temperature in degrees K -float LPS331::readTemperatureK(){ - return readTemperatureC() + LPS331_CELSIUS_TO_KELVIN_OFFSET; +float LPS331::readTemperatureK() { + return readTemperatureC() + LPS331_CELSIUS_TO_KELVIN_OFFSET; } // reads temperature in degrees C -float LPS331::readTemperatureC(void) -{ - return 42.5 + (float)readTemperatureRaw() / 480; +float LPS331::readTemperatureC(void) { + return 42.5 + (float)readTemperatureRaw() / 480; } // reads temperature in degrees F -float LPS331::readTemperatureF(void) -{ - return 108.5 + (float)readTemperatureRaw() / 480 * 1.8; +float LPS331::readTemperatureF(void) { + return 108.5 + (float)readTemperatureRaw() / 480 * 1.8; } // reads temperature and returns raw 16-bit sensor output -int16_t LPS331::readTemperatureRaw(void) -{ - WIRE_IMU.beginTransmission(address); - // assert MSB to enable register address auto-increment - WIRE_IMU.write(LPS331_TEMP_OUT_L | (1 << 7)); - WIRE_IMU.endTransmission(); - WIRE_IMU.requestFrom(address, (byte)2); +int16_t LPS331::readTemperatureRaw(void) { + WIRE_IMU.beginTransmission(address); + // assert MSB to enable register address auto-increment + WIRE_IMU.write(LPS331_TEMP_OUT_L | (1 << 7)); + WIRE_IMU.endTransmission(); + WIRE_IMU.requestFrom(address, (byte)2); - while (WIRE_IMU.available() < 2); + while (WIRE_IMU.available() < 2) + ; - uint8_t tl = WIRE_IMU.read(); - uint8_t th = WIRE_IMU.read(); + uint8_t tl = WIRE_IMU.read(); + uint8_t th = WIRE_IMU.read(); - // combine bytes - return (int16_t)(th << 8 | tl); + // combine bytes + return (int16_t)(th << 8 | tl); } -// Calculates altitude in meters above MSL using GOST4401-81 +// Calculates altitude in meters above MSL using GOST4401-81 // atmosphere model from the given pressure in pascals // The model implemented for height up to 51km -// -float LPS331::GOST4401_altitude(float pressure_pascals){ - return GOST4401_getAltitude(pressure_pascals); +// +float LPS331::GOST4401_altitude(float pressure_pascals) { + return GOST4401_getAltitude(pressure_pascals); } // converts pressure in mbar to altitude in meters, using 1976 US @@ -137,31 +125,31 @@ float LPS331::GOST4401_altitude(float pressure_pascals){ // compensated for actual regional pressure; otherwise, it returns // the pressure altitude above the standard pressure level of 1013.25 // mbar or 29.9213 inHg -float LPS331::pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar) -{ - return (1 - pow(pressure_mbar / altimeter_setting_mbar, 0.190263)) * 44330.8; +float LPS331::pressureToAltitudeMeters(float pressure_mbar, + float altimeter_setting_mbar) { + return (1 - pow(pressure_mbar / altimeter_setting_mbar, 0.190263)) + * 44330.8; } // converts pressure in inHg to altitude in feet; see notes above -float LPS331::pressureToAltitudeFeet(float pressure_inHg, float altimeter_setting_inHg) -{ - return (1 - pow(pressure_inHg / altimeter_setting_inHg, 0.190263)) * 145442; +float LPS331::pressureToAltitudeFeet(float pressure_inHg, + float altimeter_setting_inHg) { + return (1 - pow(pressure_inHg / altimeter_setting_inHg, 0.190263)) * 145442; } // Private Methods /////////////////////////////////////////////////// -bool LPS331::autoDetectAddress(void) -{ - // try each possible address and stop if reading WHO_AM_I returns the expected response - address = LPS331AP_ADDRESS_SA0_LOW; - if (testWhoAmI()) return true; - address = LPS331AP_ADDRESS_SA0_HIGH; - if (testWhoAmI()) return true; +bool LPS331::autoDetectAddress(void) { + // try each possible address and stop if reading WHO_AM_I returns the + // expected response + address = LPS331AP_ADDRESS_SA0_LOW; + if (testWhoAmI()) + return true; + address = LPS331AP_ADDRESS_SA0_HIGH; + if (testWhoAmI()) + return true; - return false; + return false; } -bool LPS331::testWhoAmI(void) -{ - return (readReg(LPS331_WHO_AM_I) == 0xBB); -} +bool LPS331::testWhoAmI(void) { return (readReg(LPS331_WHO_AM_I) == 0xBB); } diff --git a/src/Barometer.h b/src/Barometer.h index 3064504..2fd9f78 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -5,14 +5,14 @@ // The Arduino two-WIRE_IMU interface uses a 7-bit number for the address, // and sets the last bit correctly based on reads and writes -#define LPS331AP_ADDRESS_SA0_LOW 0b1011100 +#define LPS331AP_ADDRESS_SA0_LOW 0b1011100 #define LPS331AP_ADDRESS_SA0_HIGH 0b1011101 -#define LPS331AP_TWI_ADDRESS 0b1011100 -#define LPS331AP_TWI_ADDRESS_V2 0b1011101 +#define LPS331AP_TWI_ADDRESS 0b1011100 +#define LPS331AP_TWI_ADDRESS_V2 0b1011101 // SA0 states -#define LPS331_SA0_LOW 0 +#define LPS331_SA0_LOW 0 #define LPS331_SA0_HIGH 1 #define LPS331_SA0_AUTO 2 @@ -22,42 +22,41 @@ // section 7. Where they differ, the names from section 7 have been // used here. -#define LPS331_REF_P_XL 0x08 -#define LPS331_REF_P_L 0x09 -#define LPS331_REF_P_H 0x0A +#define LPS331_REF_P_XL 0x08 +#define LPS331_REF_P_L 0x09 +#define LPS331_REF_P_H 0x0A -#define LPS331_WHO_AM_I 0x0F +#define LPS331_WHO_AM_I 0x0F -#define LPS331_RES_CONF 0x10 +#define LPS331_RES_CONF 0x10 -#define LPS331_CTRL_REG1 0x20 -#define LPS331_CTRL_REG2 0x21 -#define LPS331_CTRL_REG3 0x22 -#define LPS331_INTERRUPT_CFG 0x23 -#define LPS331_INT_SOURCE 0x24 -#define LPS331_THS_P_L 0x25 -#define LPS331_THS_P_H 0x26 -#define LPS331_STATUS_REG 0x27 +#define LPS331_CTRL_REG1 0x20 +#define LPS331_CTRL_REG2 0x21 +#define LPS331_CTRL_REG3 0x22 +#define LPS331_INTERRUPT_CFG 0x23 +#define LPS331_INT_SOURCE 0x24 +#define LPS331_THS_P_L 0x25 +#define LPS331_THS_P_H 0x26 +#define LPS331_STATUS_REG 0x27 -#define LPS331_PRESS_OUT_XL 0x28 -#define LPS331_PRESS_OUT_L 0x29 -#define LPS331_PRESS_OUT_H 0x2A +#define LPS331_PRESS_OUT_XL 0x28 +#define LPS331_PRESS_OUT_L 0x29 +#define LPS331_PRESS_OUT_H 0x2A -#define LPS331_TEMP_OUT_L 0x2B -#define LPS331_TEMP_OUT_H 0x2C +#define LPS331_TEMP_OUT_L 0x2B +#define LPS331_TEMP_OUT_H 0x2C -#define LPS331_AMP_CTRL 0x30 +#define LPS331_AMP_CTRL 0x30 #define LPS331_DELTA_PRESS_XL 0x3C -#define LPS331_DELTA_PRESS_L 0x3D -#define LPS331_DELTA_PRESS_H 0x3E +#define LPS331_DELTA_PRESS_L 0x3D +#define LPS331_DELTA_PRESS_H 0x3E // Some physical constants #define LPS331_CELSIUS_TO_KELVIN_OFFSET 273.15 -class LPS331 -{ - public: +class LPS331 { +public: LPS331(uint8_t addr = LPS331AP_TWI_ADDRESS); void begin(); @@ -76,10 +75,13 @@ class LPS331 int16_t readTemperatureRaw(void); static float GOST4401_altitude(float pressure_pascals); - static float pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar = 1013.25); - static float pressureToAltitudeFeet(float pressure_inHg, float altimeter_setting_inHg = 29.9213); + static float pressureToAltitudeMeters(float pressure_mbar, + float altimeter_setting_mbar + = 1013.25); + static float pressureToAltitudeFeet(float pressure_inHg, + float altimeter_setting_inHg = 29.9213); - private: +private: byte address; bool autoDetectAddress(void); diff --git a/src/Compass.cpp b/src/Compass.cpp index 5b03f74..fb5dff0 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -1,17 +1,17 @@ #include "Compass.h" -#define ADR_FS_4 0x00 -#define ADR_FS_8 0x20 -#define ADR_FS_12 0x40 -#define ADR_FS_16 0x60 +#define ADR_FS_4 0x00 +#define ADR_FS_8 0x20 +#define ADR_FS_12 0x40 +#define ADR_FS_16 0x60 -#define SENS_FS_4 6842 -#define SENS_FS_8 3421 -#define SENS_FS_12 2281 -#define SENS_FS_16 1711 +#define SENS_FS_4 6842 +#define SENS_FS_8 3421 +#define SENS_FS_12 2281 +#define SENS_FS_16 1711 -LIS3MDL::LIS3MDL(uint8_t addr) : IMU(addr) { -} +LIS3MDL::LIS3MDL(uint8_t addr) + : IMU(addr) { } void LIS3MDL::begin() { WIRE_IMU.begin(); @@ -22,31 +22,30 @@ void LIS3MDL::begin() { void LIS3MDL::setRange(uint8_t range) { switch (range) { - case RANGE_4_GAUSS: { - _ctrlReg2 = ADR_FS_4; - _mult = SENS_FS_4; - break; - } - case RANGE_8_GAUSS: { - _ctrlReg2 = ADR_FS_8; - _mult = SENS_FS_8; - break; - } - case RANGE_12_GAUSS: { - _ctrlReg2 = ADR_FS_12; - _mult = SENS_FS_12; - break; - } - case RANGE_16_GAUSS: { - _ctrlReg2 = ADR_FS_16; - _mult = SENS_FS_16; - break; - } - default: { - _mult = SENS_FS_4; - } + case RANGE_4_GAUSS: { + _ctrlReg2 = ADR_FS_4; + _mult = SENS_FS_4; + break; + } + case RANGE_8_GAUSS: { + _ctrlReg2 = ADR_FS_8; + _mult = SENS_FS_8; + break; + } + case RANGE_12_GAUSS: { + _ctrlReg2 = ADR_FS_12; + _mult = SENS_FS_12; break; } + case RANGE_16_GAUSS: { + _ctrlReg2 = ADR_FS_16; + _mult = SENS_FS_16; + break; + } + default: { + _mult = SENS_FS_4; + } break; + } writeCtrlReg2(); } @@ -59,17 +58,11 @@ void LIS3MDL::sleep(bool enable) { writeCtrlReg3(); } -float LIS3MDL::readGaussX() { - return readX() / _mult; -} +float LIS3MDL::readGaussX() { return readX() / _mult; } -float LIS3MDL::readGaussY() { - return readY() / _mult; -} +float LIS3MDL::readGaussY() { return readY() / _mult; } -float LIS3MDL::readGaussZ() { - return readZ() / _mult; -} +float LIS3MDL::readGaussZ() { return readZ() / _mult; } float LIS3MDL::readCalibrateX() { calibrate(); @@ -86,20 +79,14 @@ float LIS3MDL::readCalibrateZ() { return _zCalibrate; } -float LIS3MDL::readCalibrateGaussX() { - return readCalibrateX()/_mult; -} +float LIS3MDL::readCalibrateGaussX() { return readCalibrateX() / _mult; } -float LIS3MDL::readCalibrateGaussY() { - return readCalibrateY()/_mult; -} +float LIS3MDL::readCalibrateGaussY() { return readCalibrateY() / _mult; } -float LIS3MDL::readCalibrateGaussZ() { - return readCalibrateZ()/_mult; -} +float LIS3MDL::readCalibrateGaussZ() { return readCalibrateZ() / _mult; } void LIS3MDL::calibrate() { - float result[3] = {0, 0, 0}; + float result[3] = { 0, 0, 0 }; float uncalibratedValues[3]; uncalibratedValues[0] = readX() - _bias[0]; uncalibratedValues[1] = readY() - _bias[1]; @@ -107,7 +94,7 @@ void LIS3MDL::calibrate() { for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { - result[i] += _calibrationMatrix[i][j] * uncalibratedValues[j]; + result[i] += _calibrationMatrix[i][j] * uncalibratedValues[j]; } } @@ -116,12 +103,13 @@ void LIS3MDL::calibrate() { _zCalibrate = result[2]; } -void LIS3MDL::calibrateMatrix(const double calibrationMatrix[3][3], const double bias[3]) { - memcpy (_bias, bias, 3 * sizeof (double)); - memcpy (_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof (double)); +void LIS3MDL::calibrateMatrix(const double calibrationMatrix[3][3], + const double bias[3]) { + memcpy(_bias, bias, 3 * sizeof(double)); + memcpy(_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof(double)); } -void LIS3MDL::readCalibrateGaussXYZ(float *x, float *y, float *z) { +void LIS3MDL::readCalibrateGaussXYZ(float* x, float* y, float* z) { calibrate(); *x = _xCalibrate / _mult; *y = _yCalibrate / _mult; @@ -132,9 +120,9 @@ float LIS3MDL::readAzimut() { calibrate(); float heading = atan2(_yCalibrate, _xCalibrate); - if(heading < 0) + if (heading < 0) heading += TWO_PI; - else if(heading > TWO_PI) + else if (heading > TWO_PI) heading -= TWO_PI; float headingDegrees = heading * RAD_TO_DEG; return headingDegrees; diff --git a/src/Compass.h b/src/Compass.h index c3259b8..74ab565 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -3,22 +3,22 @@ #include "IMU.h" -#define LIS3MDL_TWI_ADDRESS 0b0011100 -#define LIS3MDL_TWI_ADDRESS_V2 0b0011110 +#define LIS3MDL_TWI_ADDRESS 0b0011100 +#define LIS3MDL_TWI_ADDRESS_V2 0b0011110 -#define RANGE_4_GAUSS 0 -#define RANGE_8_GAUSS 1 -#define RANGE_12_GAUSS 2 -#define RANGE_16_GAUSS 3 +#define RANGE_4_GAUSS 0 +#define RANGE_8_GAUSS 1 +#define RANGE_12_GAUSS 2 +#define RANGE_16_GAUSS 3 -class LIS3MDL : public IMU -{ +class LIS3MDL : public IMU { public: LIS3MDL(uint8_t addr = LIS3MDL_TWI_ADDRESS); void begin(); void sleep(bool enable); void setRange(uint8_t range); - void calibrateMatrix(const double calibrationMatrix[3][3], const double bias[3]); + void calibrateMatrix(const double calibrationMatrix[3][3], + const double bias[3]); void calibrate(); float readGaussX(); float readGaussY(); @@ -30,7 +30,7 @@ class LIS3MDL : public IMU float readCalibrateGaussY(); float readCalibrateGaussZ(); float readAzimut(); - void readCalibrateGaussXYZ(float *x, float *y, float *z); + void readCalibrateGaussXYZ(float* x, float* y, float* z); private: float _xCalibrate; diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp index 71a19dc..5ce2a3b 100644 --- a/src/Gyroscope.cpp +++ b/src/Gyroscope.cpp @@ -1,38 +1,37 @@ #include "Gyroscope.h" -#define ADR_FS_250 0x00 -#define ADR_FS_500 0x10 -#define ADR_FS_2000 0x20 +#define ADR_FS_250 0x00 +#define ADR_FS_500 0x10 +#define ADR_FS_2000 0x20 -#define SENS_FS_250 0.00875 -#define SENS_FS_500 0.0175 -#define SENS_FS_2000 0.07 +#define SENS_FS_250 0.00875 +#define SENS_FS_500 0.0175 +#define SENS_FS_2000 0.07 -L3G4200D::L3G4200D(uint8_t addr) : IMU(addr) { -} +L3G4200D::L3G4200D(uint8_t addr) + : IMU(addr) { } void L3G4200D::setRange(uint8_t range) { switch (range) { - case RANGE_250DPS: { - _ctrlReg4 = ADR_FS_250; - _mult = SENS_FS_250; - break; - } - case RANGE_500DPS: { - _ctrlReg4 = ADR_FS_500; - _mult = SENS_FS_500; - break; - } - case RANGE_2000DPS: { - _ctrlReg4 = ADR_FS_2000; - _mult = SENS_FS_2000; - break; - } - default: { - _mult = SENS_FS_250; - } + case RANGE_250DPS: { + _ctrlReg4 = ADR_FS_250; + _mult = SENS_FS_250; + break; + } + case RANGE_500DPS: { + _ctrlReg4 = ADR_FS_500; + _mult = SENS_FS_500; break; } + case RANGE_2000DPS: { + _ctrlReg4 = ADR_FS_2000; + _mult = SENS_FS_2000; + break; + } + default: { + _mult = SENS_FS_250; + } break; + } writeCtrlReg4(); } @@ -50,7 +49,6 @@ void L3G4200D::begin() { writeCtrlReg1(); } - void L3G4200D::sleep(bool enable) { if (enable) _ctrlReg1 &= ~(1 << 3); @@ -60,31 +58,19 @@ void L3G4200D::sleep(bool enable) { writeCtrlReg1(); } -float L3G4200D::readDegPerSecX() { - return readX() * _mult; -} +float L3G4200D::readDegPerSecX() { return readX() * _mult; } -float L3G4200D::readDegPerSecY() { - return readY() * _mult; -} +float L3G4200D::readDegPerSecY() { return readY() * _mult; } -float L3G4200D::readDegPerSecZ() { - return readZ() * _mult; -} +float L3G4200D::readDegPerSecZ() { return readZ() * _mult; } -float L3G4200D::readRadPerSecX() { - return readDegPerSecX() * DEG_TO_RAD; -} +float L3G4200D::readRadPerSecX() { return readDegPerSecX() * DEG_TO_RAD; } -float L3G4200D::readRadPerSecY() { - return readDegPerSecY() * DEG_TO_RAD; -} +float L3G4200D::readRadPerSecY() { return readDegPerSecY() * DEG_TO_RAD; } -float L3G4200D::readRadPerSecZ() { - return readDegPerSecZ() * DEG_TO_RAD; -} +float L3G4200D::readRadPerSecZ() { return readDegPerSecZ() * DEG_TO_RAD; } -void L3G4200D::readDegPerSecXYZ(float *gx, float *gy, float *gz) { +void L3G4200D::readDegPerSecXYZ(float* gx, float* gy, float* gz) { int16_t x, y, z; readXYZ(&x, &y, &z); *gx = x * _mult; @@ -92,7 +78,7 @@ void L3G4200D::readDegPerSecXYZ(float *gx, float *gy, float *gz) { *gz = z * _mult; } -void L3G4200D::readRadPerSecXYZ(float *gx, float *gy, float *gz) { +void L3G4200D::readRadPerSecXYZ(float* gx, float* gy, float* gz) { readDegPerSecXYZ(gx, gy, gz); (*gx) *= DEG_TO_RAD; (*gy) *= DEG_TO_RAD; diff --git a/src/Gyroscope.h b/src/Gyroscope.h index 91ba0f3..4588a62 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -3,15 +3,14 @@ #include "IMU.h" -#define L3G4200D_ADDRESS 0b01101000 -#define L3G4200D_ADDRESS_V2 0b01101001 +#define L3G4200D_ADDRESS 0b01101000 +#define L3G4200D_ADDRESS_V2 0b01101001 -#define RANGE_250DPS 0 -#define RANGE_500DPS 1 -#define RANGE_2000DPS 2 +#define RANGE_250DPS 0 +#define RANGE_500DPS 1 +#define RANGE_2000DPS 2 -class L3G4200D : public IMU -{ +class L3G4200D : public IMU { public: L3G4200D(uint8_t addr = L3G4200D_ADDRESS); void begin(); @@ -23,8 +22,8 @@ class L3G4200D : public IMU float readRadPerSecX(); float readRadPerSecY(); float readRadPerSecZ(); - void readDegPerSecXYZ(float *gx, float *gy, float *gz); - void readRadPerSecXYZ(float *gx, float *gy, float *gz); + void readDegPerSecXYZ(float* gx, float* gy, float* gz); + void readRadPerSecXYZ(float* gx, float* gy, float* gz); }; #endif // __GYROSCOPE_H__ diff --git a/src/IMU.cpp b/src/IMU.cpp index 4719e86..e48482c 100644 --- a/src/IMU.cpp +++ b/src/IMU.cpp @@ -1,26 +1,20 @@ #include "IMU.h" -#define CTRL_REG1 0x20 -#define CTRL_REG2 0x21 -#define CTRL_REG3 0x22 -#define CTRL_REG4 0x23 -#define CTRL_REG5 0x24 - -#define OUT_X 0x28 -#define OUT_Y 0x2A -#define OUT_Z 0x2C - -int16_t IMU::readX() { - return readAxis(OUT_X); -} +#define CTRL_REG1 0x20 +#define CTRL_REG2 0x21 +#define CTRL_REG3 0x22 +#define CTRL_REG4 0x23 +#define CTRL_REG5 0x24 -int16_t IMU::readY() { - return readAxis(OUT_Y); -} +#define OUT_X 0x28 +#define OUT_Y 0x2A +#define OUT_Z 0x2C -int16_t IMU::readZ() { - return readAxis(OUT_Z); -} +int16_t IMU::readX() { return readAxis(OUT_X); } + +int16_t IMU::readY() { return readAxis(OUT_Y); } + +int16_t IMU::readZ() { return readAxis(OUT_Z); } int16_t IMU::readAxis(uint8_t reg) { return ((int16_t)readByte(reg + 1) << 8) | readByte(reg); @@ -42,9 +36,11 @@ uint8_t IMU::readByte(uint8_t reg) { return value; } -void IMU::readXYZ(int16_t *x, int16_t *y, int16_t *z) { +void IMU::readXYZ(int16_t* x, int16_t* y, int16_t* z) { WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(OUT_X | (1 << 7)); // assert MSB to enable register address auto-increment + WIRE_IMU.write( + OUT_X + | (1 << 7)); // assert MSB to enable register address auto-increment WIRE_IMU.endTransmission(); uint8_t burstSize = 6; @@ -54,41 +50,41 @@ void IMU::readXYZ(int16_t *x, int16_t *y, int16_t *z) { waitForData(); values[i] = WIRE_IMU.read(); } - + *x = *((int16_t*)&values[0]); *y = *((int16_t*)&values[2]); *z = *((int16_t*)&values[4]); } -void IMU::writeCtrlReg1(){ +void IMU::writeCtrlReg1() { WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG1); WIRE_IMU.write(_ctrlReg1); WIRE_IMU.endTransmission(); } -void IMU::writeCtrlReg2(){ +void IMU::writeCtrlReg2() { WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG2); WIRE_IMU.write(_ctrlReg2); WIRE_IMU.endTransmission(); } -void IMU::writeCtrlReg3(){ +void IMU::writeCtrlReg3() { WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG3); WIRE_IMU.write(_ctrlReg3); WIRE_IMU.endTransmission(); } -void IMU::writeCtrlReg4(){ +void IMU::writeCtrlReg4() { WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG4); WIRE_IMU.write(_ctrlReg4); WIRE_IMU.endTransmission(); } -void IMU::writeCtrlReg5(){ +void IMU::writeCtrlReg5() { WIRE_IMU.beginTransmission(_addr); WIRE_IMU.write(CTRL_REG5); WIRE_IMU.write(_ctrlReg5); diff --git a/src/IMU.h b/src/IMU.h index 1466d0d..7aa657f 100644 --- a/src/IMU.h +++ b/src/IMU.h @@ -11,21 +11,21 @@ #define WIRE_IMU Wire1 #endif -class IMU -{ +class IMU { private: uint8_t _addr; static inline void waitForData(); public: - IMU(uint8_t addr) : _addr (addr) {} + IMU(uint8_t addr) + : _addr(addr) { } uint8_t readByte(uint8_t reg); int16_t readX(); int16_t readY(); int16_t readZ(); - void readXYZ(int16_t *x, int16_t *y, int16_t *z); + void readXYZ(int16_t* x, int16_t* y, int16_t* z); protected: uint8_t _ctrlReg1; diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index 3919e0b..4e16dba 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -1,10 +1,8 @@ #include "MadgwickAHRS.h" -#include #include +#include -Madgwick::Madgwick() { - -} +Madgwick::Madgwick() { } void Madgwick::setKoeff(float sampleFreq, float beta) { _beta = beta; @@ -17,7 +15,7 @@ void Madgwick::reset() { _q3 = 0; } -void Madgwick::readQuaternions(float *q0, float *q1, float *q2, float *q3) { +void Madgwick::readQuaternions(float* q0, float* q1, float* q2, float* q3) { *q0 = _q0; *q1 = _q1; *q2 = _q2; @@ -27,15 +25,19 @@ void Madgwick::readQuaternions(float *q0, float *q1, float *q2, float *q3) { //--------------------------------------------------------------------------------------------------- // AHRS algorithm update -void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, + float az, float mx, float my, float mz) { float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float hx, hy; - float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, + _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, + q2q2, q2q3, q3q3; - // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in + // magnetometer normalisation) + if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { update(gx, gy, gz, ax, ay, az); return; } @@ -46,14 +48,15 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az qDot3 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); qDot4 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + // Compute feedback only if accelerometer measurement valid (avoids NaN in + // accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; - az *= recipNorm; + az *= recipNorm; // Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); @@ -84,19 +87,53 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az q3q3 = _q3 * _q3; // Reference direction of Earth's magnetic field - hx = mx * q0q0 - _2q0my * _q3 + _2q0mz * _q2 + mx * q1q1 + _2q1 * my * _q2 + _2q1 * mz * _q3 - mx * q2q2 - mx * q3q3; - hy = _2q0mx * _q3 + my * q0q0 - _2q0mz * _q1 + _2q1mx * _q2 - my * q1q1 + my * q2q2 + _2q2 * mz * _q3 - my * q3q3; + hx = mx * q0q0 - _2q0my * _q3 + _2q0mz * _q2 + mx * q1q1 + + _2q1 * my * _q2 + _2q1 * mz * _q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * _q3 + my * q0q0 - _2q0mz * _q1 + _2q1mx * _q2 - my * q1q1 + + my * q2q2 + _2q2 * mz * _q3 - my * q3q3; _2bx = sqrt(hx * hx + hy * hy); - _2bz = -_2q0mx * _q2 + _2q0my * _q1 + mz * q0q0 + _2q1mx * _q3 - mz * q1q1 + _2q2 * my * _q3 - mz * q2q2 + mz * q3q3; + _2bz = -_2q0mx * _q2 + _2q0my * _q1 + mz * q0q0 + _2q1mx * _q3 + - mz * q1q1 + _2q2 * my * _q3 - mz * q2q2 + mz * q3q3; _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; // Gradient decent algorithm corrective step - s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * _q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * _q3 + _2bz * _q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * _q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * _q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * _q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * _q2 + _2bz * _q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * _q3 - _4bz * _q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * _q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * _q2 - _2bz * _q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * _q1 + _2bz * _q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * _q0 - _4bz * _q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * _q3 + _2bz * _q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * _q0 + _2bz * _q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * _q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) + - _2bz * _q2 + * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * _q3 + _2bz * _q1) + * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * _q2 + * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) + - 4.0f * _q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + _2bz * _q3 + * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * _q2 + _2bz * _q0) + * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * _q3 - _4bz * _q1) + * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) + - 4.0f * _q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + (-_4bx * _q2 - _2bz * _q0) + * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * _q1 + _2bz * _q3) + * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * _q0 - _4bz * _q2) + * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + + (-_4bx * _q3 + _2bz * _q1) + * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * _q0 + _2bz * _q2) + * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * _q1 + * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; @@ -126,11 +163,13 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az //--------------------------------------------------------------------------------------------------- // IMU algorithm update -void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az) { +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, + float az) { float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, + q2q2, q3q3; // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); @@ -138,8 +177,9 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az qDot3 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); qDot4 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + // Compute feedback only if accelerometer measurement valid (avoids NaN in + // accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); @@ -162,13 +202,15 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az q2q2 = _q2 * _q2; q3q3 = _q3 * _q3; - // Gradient decent algorithm corrective step s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * _q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * _q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * _q1 - _2q0 * ay - _4q1 + + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * _q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * _q3 - _2q1 * ax + 4.0f * q2q2 * _q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; @@ -181,7 +223,6 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az qDot4 -= _beta * s3; } - // Integrate rate of change of quaternion to yield quaternion _q0 += qDot1 * (1.0f / _sampleFreq); _q1 += qDot2 * (1.0f / _sampleFreq); @@ -211,25 +252,23 @@ float Madgwick::invSqrt(float x) { } float Madgwick::getYawRad() { - return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3, 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1); + return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3, + 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1); } float Madgwick::getPitchRad() { - return atan2(2 * _q2 * _q3 - 2 * _q0 * _q1, 2 * _q0 * _q0 + 2 * _q3 * _q3 - 1); + return atan2(2 * _q2 * _q3 - 2 * _q0 * _q1, + 2 * _q0 * _q0 + 2 * _q3 * _q3 - 1); } float Madgwick::getRollRad() { - return -1 * atan2(2.0f * (_q0 * _q2 - _q1 * _q3), 1.0f - 2.0f * (_q2 * _q2 + _q1 *_q1 )); + return -1 + * atan2(2.0f * (_q0 * _q2 - _q1 * _q3), + 1.0f - 2.0f * (_q2 * _q2 + _q1 * _q1)); } -float Madgwick::getYawDeg() { - return getYawRad() * RAD_TO_DEG; -} +float Madgwick::getYawDeg() { return getYawRad() * RAD_TO_DEG; } -float Madgwick::getPitchDeg() { - return getPitchRad() * RAD_TO_DEG; -} +float Madgwick::getPitchDeg() { return getPitchRad() * RAD_TO_DEG; } -float Madgwick::getRollDeg() { - return getRollRad() * RAD_TO_DEG; -} \ No newline at end of file +float Madgwick::getRollDeg() { return getRollRad() * RAD_TO_DEG; } \ No newline at end of file diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 99d0be6..8ed49b2 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -3,17 +3,18 @@ #include -#define SAMPLE_FREQ 1000.0f // sample frequency in Hz -#define BETA_DEF 0.5f // 2 * proportional gain +#define SAMPLE_FREQ 1000.0f // sample frequency in Hz +#define BETA_DEF 0.5f // 2 * proportional gain class Madgwick { public: Madgwick(); - void readQuaternions(float *q0, float *q1, float *q2, float *q3); + void readQuaternions(float* q0, float* q1, float* q2, float* q3); void reset(); void setKoeff(float sampleFreq, float beta); - void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); + void update(float gx, float gy, float gz, float ax, float ay, float az, + float mx, float my, float mz); void update(float gx, float gy, float gz, float ax, float ay, float az); float getPitchRad(); float getRollRad(); @@ -24,7 +25,7 @@ class Madgwick { private: float invSqrt(float x); - volatile float _beta = BETA_DEF; // algorithm gain + volatile float _beta = BETA_DEF; // algorithm gain volatile float _sampleFreq = SAMPLE_FREQ; volatile float _q0 = 1.0f; volatile float _q1 = 0.0f; diff --git a/src/TroykaIMU.h b/src/TroykaIMU.h index e1490c6..53ee3a5 100644 --- a/src/TroykaIMU.h +++ b/src/TroykaIMU.h @@ -1,49 +1,45 @@ #include "Accelerometer.h" -#include "Gyroscope.h" -#include "Compass.h" #include "Barometer.h" +#include "Compass.h" +#include "Gyroscope.h" #include "MadgwickAHRS.h" // Accelerometer -#define ACCEL_ADDRESS_V1 LIS331DLH_ADDRESS -#define ACCEL_ADDRESS_V2 LIS331DLH_ADDRESS_V2 +#define ACCEL_ADDRESS_V1 LIS331DLH_ADDRESS +#define ACCEL_ADDRESS_V2 LIS331DLH_ADDRESS_V2 // Gyroscope -#define GYRO_ADDRESS_V1 L3G4200D_ADDRESS -#define GYRO_ADDRESS_V2 L3G4200D_ADDRESS_V2 +#define GYRO_ADDRESS_V1 L3G4200D_ADDRESS +#define GYRO_ADDRESS_V2 L3G4200D_ADDRESS_V2 // Compass -#define COMPASS_ADDRESS_V1 LIS3MDL_TWI_ADDRESS -#define COMPASS_ADDRESS_V2 LIS3MDL_TWI_ADDRESS_V2 +#define COMPASS_ADDRESS_V1 LIS3MDL_TWI_ADDRESS +#define COMPASS_ADDRESS_V2 LIS3MDL_TWI_ADDRESS_V2 // Barometer -#define BARO_ADDRESS_V1 LPS331AP_TWI_ADDRESS -#define BARO_ADDRESS_V2 LPS331AP_TWI_ADDRESS_V2 - -class Accelerometer : public LIS331DLH -{ - public: - Accelerometer(uint8_t addr = ACCEL_ADDRESS_V1) : - LIS331DLH(addr) {} +#define BARO_ADDRESS_V1 LPS331AP_TWI_ADDRESS +#define BARO_ADDRESS_V2 LPS331AP_TWI_ADDRESS_V2 + +class Accelerometer : public LIS331DLH { +public: + Accelerometer(uint8_t addr = ACCEL_ADDRESS_V1) + : LIS331DLH(addr) { } }; -class Gyroscope : public L3G4200D -{ - public: - Gyroscope(uint8_t addr = GYRO_ADDRESS_V1) : - L3G4200D(addr) {} +class Gyroscope : public L3G4200D { +public: + Gyroscope(uint8_t addr = GYRO_ADDRESS_V1) + : L3G4200D(addr) { } }; -class Compass : public LIS3MDL -{ - public: - Compass(uint8_t addr = COMPASS_ADDRESS_V1) : - LIS3MDL(addr) {} +class Compass : public LIS3MDL { +public: + Compass(uint8_t addr = COMPASS_ADDRESS_V1) + : LIS3MDL(addr) { } }; -class Barometer : public LPS331 -{ - public: - Barometer(uint8_t addr = BARO_ADDRESS_V1) : - LPS331(addr) {} +class Barometer : public LPS331 { +public: + Barometer(uint8_t addr = BARO_ADDRESS_V1) + : LPS331(addr) { } }; From 277ecfe1f3694e52533a1e1fd7384b1888f97b1d Mon Sep 17 00:00:00 2001 From: Igor89 Date: Thu, 24 Sep 2020 04:43:53 +0300 Subject: [PATCH 04/20] add select i2c bus --- src/Accelerometer.cpp | 7 +-- src/Accelerometer.h | 6 +-- src/Barometer.cpp | 55 ++++++++++++----------- src/Barometer.h | 10 ++--- src/BaseIMU.cpp | 96 ++++++++++++++++++++++++++++++++++++++++ src/{IMU.h => BaseIMU.h} | 43 +++++++----------- src/Compass.cpp | 7 +-- src/Compass.h | 6 +-- src/Gyroscope.cpp | 7 +-- src/Gyroscope.h | 6 +-- src/IMU.cpp | 92 -------------------------------------- 11 files changed, 165 insertions(+), 170 deletions(-) create mode 100644 src/BaseIMU.cpp rename src/{IMU.h => BaseIMU.h} (63%) delete mode 100644 src/IMU.cpp diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index 5ec0c77..924b8ec 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -7,11 +7,12 @@ #define G 9.8 LIS331DLH::LIS331DLH(uint8_t addr) - : IMU(addr) { } + : BaseIMU(addr) { } -void LIS331DLH::begin() { +void LIS331DLH::begin(TwoWire& wire) { // подключаемся к шине I²C - WIRE_IMU.begin(); + _wire = &wire; + _wire->begin(); // включаем координаты x, y, z _ctrlReg1 |= (1 << 0); _ctrlReg1 |= (1 << 1); diff --git a/src/Accelerometer.h b/src/Accelerometer.h index 518f6a0..ca5cb01 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -1,7 +1,7 @@ #ifndef __ACCELEROMETER_H__ #define __ACCELEROMETER_H__ -#include "IMU.h" +#include "BaseIMU.h" #define LIS331DLH_ADDRESS 0b0011000 #define LIS331DLH_ADDRESS_V2 0b0011001 @@ -10,10 +10,10 @@ #define RANGE_4G 4 #define RANGE_8G 8 -class LIS331DLH : public IMU { +class LIS331DLH : public BaseIMU { public: LIS331DLH(uint8_t addr = LIS331DLH_ADDRESS); - void begin(); + void begin(TwoWire &wire = Wire); void sleep(bool enable); void setRange(uint8_t range); float readGX(); diff --git a/src/Barometer.cpp b/src/Barometer.cpp index 6c71e39..f805295 100644 --- a/src/Barometer.cpp +++ b/src/Barometer.cpp @@ -8,30 +8,31 @@ LPS331::LPS331(uint8_t addr) { address = addr; } // Public Methods //////////////////////////////////////////////////// // sets or detects slave address; returns bool indicating success -void LPS331::begin() { - WIRE_IMU.begin(); +void LPS331::begin(TwoWire& wire) { + _wire = &wire; + _wire->begin(); writeReg(LPS331_CTRL_REG1, 0b11100000); delay(100); } // writes register void LPS331::writeReg(byte reg, byte value) { - WIRE_IMU.beginTransmission(address); - WIRE_IMU.write(reg); - WIRE_IMU.write(value); - WIRE_IMU.endTransmission(); + _wire->beginTransmission(address); + _wire->write(reg); + _wire->write(value); + _wire->endTransmission(); } // reads register byte LPS331::readReg(byte reg) { byte value; - WIRE_IMU.beginTransmission(address); - WIRE_IMU.write(reg); - WIRE_IMU.endTransmission(false); // restart - WIRE_IMU.requestFrom(address, (byte)1); - value = WIRE_IMU.read(); - WIRE_IMU.endTransmission(); + _wire->beginTransmission(address); + _wire->write(reg); + _wire->endTransmission(false); // restart + _wire->requestFrom(address, (byte)1); + value = _wire->read(); + _wire->endTransmission(); return value; } @@ -59,18 +60,18 @@ float LPS331::readPressureMillimetersHg(void) { // reads pressure and returns raw 24-bit sensor output int32_t LPS331::readPressureRaw(void) { - WIRE_IMU.beginTransmission(address); + _wire->beginTransmission(address); // assert MSB to enable register address auto-increment - WIRE_IMU.write(LPS331_PRESS_OUT_XL | (1 << 7)); - WIRE_IMU.endTransmission(); - WIRE_IMU.requestFrom(address, (byte)3); + _wire->write(LPS331_PRESS_OUT_XL | (1 << 7)); + _wire->endTransmission(); + _wire->requestFrom(address, (byte)3); - while (WIRE_IMU.available() < 3) + while (_wire->available() < 3) ; - uint8_t pxl = WIRE_IMU.read(); - uint8_t pl = WIRE_IMU.read(); - uint8_t ph = WIRE_IMU.read(); + uint8_t pxl = _wire->read(); + uint8_t pl = _wire->read(); + uint8_t ph = _wire->read(); // combine bytes return (int32_t)(int8_t)ph << 16 | (uint16_t)pl << 8 | pxl; @@ -93,17 +94,17 @@ float LPS331::readTemperatureF(void) { // reads temperature and returns raw 16-bit sensor output int16_t LPS331::readTemperatureRaw(void) { - WIRE_IMU.beginTransmission(address); + _wire->beginTransmission(address); // assert MSB to enable register address auto-increment - WIRE_IMU.write(LPS331_TEMP_OUT_L | (1 << 7)); - WIRE_IMU.endTransmission(); - WIRE_IMU.requestFrom(address, (byte)2); + _wire->write(LPS331_TEMP_OUT_L | (1 << 7)); + _wire->endTransmission(); + _wire->requestFrom(address, (byte)2); - while (WIRE_IMU.available() < 2) + while (_wire->available() < 2) ; - uint8_t tl = WIRE_IMU.read(); - uint8_t th = WIRE_IMU.read(); + uint8_t tl = _wire->read(); + uint8_t th = _wire->read(); // combine bytes return (int16_t)(th << 8 | tl); diff --git a/src/Barometer.h b/src/Barometer.h index 2fd9f78..7770049 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -1,7 +1,7 @@ #ifndef __BAROMETER_H__ #define __BAROMETER_H__ -#include "IMU.h" +#include "BaseIMU.h" // The Arduino two-WIRE_IMU interface uses a 7-bit number for the address, // and sets the last bit correctly based on reads and writes @@ -58,12 +58,9 @@ class LPS331 { public: LPS331(uint8_t addr = LPS331AP_TWI_ADDRESS); - - void begin(); - + void begin(TwoWire &wire = Wire); void writeReg(byte reg, byte value); byte readReg(byte reg); - float readPressurePascals(void); float readPressureMillibars(void); float readPressureInchesHg(void); @@ -73,7 +70,6 @@ class LPS331 { float readTemperatureC(void); float readTemperatureF(void); int16_t readTemperatureRaw(void); - static float GOST4401_altitude(float pressure_pascals); static float pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar @@ -82,8 +78,8 @@ class LPS331 { float altimeter_setting_inHg = 29.9213); private: + TwoWire *_wire; byte address; - bool autoDetectAddress(void); bool testWhoAmI(void); }; diff --git a/src/BaseIMU.cpp b/src/BaseIMU.cpp new file mode 100644 index 0000000..c4f6d14 --- /dev/null +++ b/src/BaseIMU.cpp @@ -0,0 +1,96 @@ +#include "BaseIMU.h" + +#define CTRL_REG1 0x20 +#define CTRL_REG2 0x21 +#define CTRL_REG3 0x22 +#define CTRL_REG4 0x23 +#define CTRL_REG5 0x24 + +#define OUT_X 0x28 +#define OUT_Y 0x2A +#define OUT_Z 0x2C + +int16_t BaseIMU::readX() { + return readAxis(OUT_X); +} + +int16_t BaseIMU::readY() { + return readAxis(OUT_Y); +} + +int16_t BaseIMU::readZ() { + return readAxis(OUT_Z); +} + +int16_t BaseIMU::readAxis(uint8_t reg) { + return ((int16_t)readByte(reg + 1) << 8) | readByte(reg); +} + +void BaseIMU::waitForData() { + while (_wire->available() < 1) + continue; +} + +uint8_t BaseIMU::readByte(uint8_t reg) { + uint8_t value; + _wire->beginTransmission(_addr); + _wire->write(reg); + _wire->endTransmission(); + _wire->requestFrom(_addr, 1u); + waitForData(); + value = _wire->read(); + return value; +} + +void BaseIMU::readXYZ(int16_t *x, int16_t *y, int16_t *z) { + _wire->beginTransmission(_addr); + _wire->write(OUT_X | (1 << 7)); // assert MSB to enable register address auto-increment + _wire->endTransmission(); + + uint8_t burstSize = 6; + _wire->requestFrom(_addr, burstSize); + uint8_t values[burstSize]; + for (uint8_t i = 0; i < burstSize; i++) { + waitForData(); + values[i] = _wire->read(); + } + + *x = *((int16_t*)&values[0]); + *y = *((int16_t*)&values[2]); + *z = *((int16_t*)&values[4]); +} + +void BaseIMU::writeCtrlReg1(){ + _wire->beginTransmission(_addr); + _wire->write(CTRL_REG1); + _wire->write(_ctrlReg1); + _wire->endTransmission(); +} + +void BaseIMU::writeCtrlReg2(){ + _wire->beginTransmission(_addr); + _wire->write(CTRL_REG2); + _wire->write(_ctrlReg2); + _wire->endTransmission(); +} + +void BaseIMU::writeCtrlReg3(){ + _wire->beginTransmission(_addr); + _wire->write(CTRL_REG3); + _wire->write(_ctrlReg3); + _wire->endTransmission(); +} + +void BaseIMU::writeCtrlReg4(){ + _wire->beginTransmission(_addr); + _wire->write(CTRL_REG4); + _wire->write(_ctrlReg4); + _wire->endTransmission(); +} + +void BaseIMU::writeCtrlReg5(){ + _wire->beginTransmission(_addr); + _wire->write(CTRL_REG5); + _wire->write(_ctrlReg5); + _wire->endTransmission(); +} diff --git a/src/IMU.h b/src/BaseIMU.h similarity index 63% rename from src/IMU.h rename to src/BaseIMU.h index 7aa657f..4b17e80 100644 --- a/src/IMU.h +++ b/src/BaseIMU.h @@ -1,26 +1,13 @@ -#ifndef __IMU_H__ -#define __IMU_H__ +#ifndef __BaseIMU_H__ +#define __BaseIMU_H__ #include #include -#if defined(__AVR__) || defined(__SAMD21G18A__) || defined(ESP8266) -#define WIRE_IMU Wire - -#elif defined(__SAM3X8E__) || defined(__SAM3A8C__) || defined(__SAM3A4C__) -#define WIRE_IMU Wire1 -#endif - -class IMU { -private: - uint8_t _addr; - - static inline void waitForData(); - +class BaseIMU { public: - IMU(uint8_t addr) + BaseIMU(uint8_t addr) : _addr(addr) { } - uint8_t readByte(uint8_t reg); int16_t readX(); int16_t readY(); @@ -28,20 +15,24 @@ class IMU { void readXYZ(int16_t* x, int16_t* y, int16_t* z); protected: + void begin(TwoWire& wire = Wire); + void writeCtrlReg1(); + void writeCtrlReg2(); + void writeCtrlReg3(); + void writeCtrlReg4(); + void writeCtrlReg5(); uint8_t _ctrlReg1; uint8_t _ctrlReg2; uint8_t _ctrlReg3; uint8_t _ctrlReg4; uint8_t _ctrlReg5; - + int16_t readAxis(uint8_t reg); + TwoWire* _wire; float _mult; - void writeCtrlReg1(); - void writeCtrlReg2(); - void writeCtrlReg3(); - void writeCtrlReg4(); - void writeCtrlReg5(); - - int16_t readAxis(uint8_t reg); +private: + uint8_t _addr; + void waitForData(); }; -#endif // __IMU_H__ + +#endif // __BaseIMU_H__ diff --git a/src/Compass.cpp b/src/Compass.cpp index fb5dff0..f149859 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -11,10 +11,11 @@ #define SENS_FS_16 1711 LIS3MDL::LIS3MDL(uint8_t addr) - : IMU(addr) { } + : BaseIMU(addr) { } -void LIS3MDL::begin() { - WIRE_IMU.begin(); +void LIS3MDL::begin(TwoWire& wire) { + _wire = &wire; + _wire->begin(); // устанавливаем чувствительность setRange(RANGE_4_GAUSS); writeCtrlReg3(); diff --git a/src/Compass.h b/src/Compass.h index 74ab565..7d0d18a 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -1,7 +1,7 @@ #ifndef __COMPASS_H__ #define __COMPASS_H__ -#include "IMU.h" +#include "BaseIMU.h" #define LIS3MDL_TWI_ADDRESS 0b0011100 #define LIS3MDL_TWI_ADDRESS_V2 0b0011110 @@ -11,10 +11,10 @@ #define RANGE_12_GAUSS 2 #define RANGE_16_GAUSS 3 -class LIS3MDL : public IMU { +class LIS3MDL : public BaseIMU { public: LIS3MDL(uint8_t addr = LIS3MDL_TWI_ADDRESS); - void begin(); + void begin(TwoWire &wire = Wire); void sleep(bool enable); void setRange(uint8_t range); void calibrateMatrix(const double calibrationMatrix[3][3], diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp index 5ce2a3b..67c16e7 100644 --- a/src/Gyroscope.cpp +++ b/src/Gyroscope.cpp @@ -9,7 +9,7 @@ #define SENS_FS_2000 0.07 L3G4200D::L3G4200D(uint8_t addr) - : IMU(addr) { } + : BaseIMU(addr) { } void L3G4200D::setRange(uint8_t range) { switch (range) { @@ -35,9 +35,10 @@ void L3G4200D::setRange(uint8_t range) { writeCtrlReg4(); } -void L3G4200D::begin() { +void L3G4200D::begin(TwoWire& wire) { // подключаемся к шине I²C - WIRE_IMU.begin(); + _wire = &wire; + _wire->begin(); // включаем координаты x, y, z _ctrlReg1 |= (1 << 0); _ctrlReg1 |= (1 << 1); diff --git a/src/Gyroscope.h b/src/Gyroscope.h index 4588a62..d6d57a5 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -1,7 +1,7 @@ #ifndef __GYROSCOPE_H__ #define __GYROSCOPE_H__ -#include "IMU.h" +#include "BaseIMU.h" #define L3G4200D_ADDRESS 0b01101000 #define L3G4200D_ADDRESS_V2 0b01101001 @@ -10,10 +10,10 @@ #define RANGE_500DPS 1 #define RANGE_2000DPS 2 -class L3G4200D : public IMU { +class L3G4200D : public BaseIMU { public: L3G4200D(uint8_t addr = L3G4200D_ADDRESS); - void begin(); + void begin(TwoWire &wire = Wire); void sleep(bool enable); void setRange(uint8_t range); float readDegPerSecX(); diff --git a/src/IMU.cpp b/src/IMU.cpp deleted file mode 100644 index e48482c..0000000 --- a/src/IMU.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include "IMU.h" - -#define CTRL_REG1 0x20 -#define CTRL_REG2 0x21 -#define CTRL_REG3 0x22 -#define CTRL_REG4 0x23 -#define CTRL_REG5 0x24 - -#define OUT_X 0x28 -#define OUT_Y 0x2A -#define OUT_Z 0x2C - -int16_t IMU::readX() { return readAxis(OUT_X); } - -int16_t IMU::readY() { return readAxis(OUT_Y); } - -int16_t IMU::readZ() { return readAxis(OUT_Z); } - -int16_t IMU::readAxis(uint8_t reg) { - return ((int16_t)readByte(reg + 1) << 8) | readByte(reg); -} - -inline void IMU::waitForData() { - while (WIRE_IMU.available() < 1) - continue; -} - -uint8_t IMU::readByte(uint8_t reg) { - uint8_t value; - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(reg); - WIRE_IMU.endTransmission(); - WIRE_IMU.requestFrom(_addr, 1u); - waitForData(); - value = WIRE_IMU.read(); - return value; -} - -void IMU::readXYZ(int16_t* x, int16_t* y, int16_t* z) { - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write( - OUT_X - | (1 << 7)); // assert MSB to enable register address auto-increment - WIRE_IMU.endTransmission(); - - uint8_t burstSize = 6; - WIRE_IMU.requestFrom(_addr, burstSize); - uint8_t values[burstSize]; - for (uint8_t i = 0; i < burstSize; i++) { - waitForData(); - values[i] = WIRE_IMU.read(); - } - - *x = *((int16_t*)&values[0]); - *y = *((int16_t*)&values[2]); - *z = *((int16_t*)&values[4]); -} - -void IMU::writeCtrlReg1() { - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG1); - WIRE_IMU.write(_ctrlReg1); - WIRE_IMU.endTransmission(); -} - -void IMU::writeCtrlReg2() { - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG2); - WIRE_IMU.write(_ctrlReg2); - WIRE_IMU.endTransmission(); -} - -void IMU::writeCtrlReg3() { - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG3); - WIRE_IMU.write(_ctrlReg3); - WIRE_IMU.endTransmission(); -} - -void IMU::writeCtrlReg4() { - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG4); - WIRE_IMU.write(_ctrlReg4); - WIRE_IMU.endTransmission(); -} - -void IMU::writeCtrlReg5() { - WIRE_IMU.beginTransmission(_addr); - WIRE_IMU.write(CTRL_REG5); - WIRE_IMU.write(_ctrlReg5); - WIRE_IMU.endTransmission(); -} From 1c1d3c92681f8849317caa3f260707e00557700c Mon Sep 17 00:00:00 2001 From: Igor605ds Date: Tue, 6 Oct 2020 00:45:46 +0300 Subject: [PATCH 05/20] replace pointers to references --- examples/Madgwick6DOF/Madgwick6DOF.ino | 6 +- examples/Madgwick9DOF/Madgwick9DOF.ino | 8 +- .../MadgwickProcessing9DOF.ino | 8 +- examples/accel/accel.ino | 4 +- examples/barometer/barometer.ino | 2 +- examples/compass/compass/compass.ino | 2 +- .../compassCalibrateMatrix.ino | 2 +- src/Accelerometer.cpp | 30 +++---- src/Accelerometer.h | 10 ++- src/BaseIMU.cpp | 82 ++++++------------- src/BaseIMU.h | 25 +++--- src/Compass.cpp | 24 ++---- src/Compass.h | 12 ++- src/Gyroscope.cpp | 53 +++++------- src/Gyroscope.h | 12 ++- 15 files changed, 125 insertions(+), 155 deletions(-) diff --git a/examples/Madgwick6DOF/Madgwick6DOF.ino b/examples/Madgwick6DOF/Madgwick6DOF.ino index 6bd3e4a..0fbf79b 100755 --- a/examples/Madgwick6DOF/Madgwick6DOF.ino +++ b/examples/Madgwick6DOF/Madgwick6DOF.ino @@ -44,9 +44,9 @@ void loop() unsigned long startMillis = millis(); // считываем данные с акселерометра в единицах G - accel.readGXYZ(&ax, &ay, &az); + accel.readGXYZ(ax, ay, az); // считываем данные с акселерометра в радианах в секунду - gyro.readRadPerSecXYZ(&gx, &gy, &gz); + gyro.readRadPerSecXYZ(gx, gy, gz); // устанавливаем коэффициенты фильтра filter.setKoeff(fps, BETA); // обновляем входные данные в фильтр @@ -71,4 +71,4 @@ void loop() unsigned long deltaMillis = millis() - startMillis; // вычисляем частоту обработки фильтра fps = 1000 / deltaMillis; -} +} diff --git a/examples/Madgwick9DOF/Madgwick9DOF.ino b/examples/Madgwick9DOF/Madgwick9DOF.ino index bc28b01..6d12e90 100755 --- a/examples/Madgwick9DOF/Madgwick9DOF.ino +++ b/examples/Madgwick9DOF/Madgwick9DOF.ino @@ -67,11 +67,11 @@ void loop() unsigned long startMillis = millis(); // считываем данные с акселерометра в единицах G - accel.readGXYZ(&ax, &ay, &az); + accel.readGXYZ(ax, ay, az); // считываем данные с гироскопа в радианах в секунду - gyro.readRadPerSecXYZ(&gx, &gy, &gz); + gyro.readRadPerSecXYZ(gx, gy, gz); // считываем данные с компаса в Гауссах - compass.readCalibrateGaussXYZ(&mx, &my, &mz); + compass.readCalibrateGaussXYZ(mx, my, mz); // устанавливаем коэффициенты фильтра filter.setKoeff(fps, BETA); @@ -98,4 +98,4 @@ void loop() // вычисляем частоту обработки фильтра fps = 1000 / deltaMillis; } - + diff --git a/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino b/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino index 917ba60..cf20f55 100755 --- a/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino +++ b/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino @@ -65,11 +65,11 @@ void loop() // запоминаем текущее время unsigned long startMillis = millis(); // считываем данные с акселерометра в единицах G - accel.readGXYZ(&ax, &ay, &az); + accel.readGXYZ(ax, ay, az); // считываем данные с гироскопа в радианах в секунду - gyro.readRadPerSecXYZ(&gx, &gy, &gz); + gyro.readRadPerSecXYZ(gx, gy, gz); // считываем данные с компаса в Гауссах - compass.readCalibrateGaussXYZ(&mx, &my, &mz); + compass.readCalibrateGaussXYZ(mx, my, mz); // устанавливаем коэффициенты фильтра filter.setKoeff(fps, BETA); // обновляем входные данные в фильтр @@ -95,4 +95,4 @@ void loop() unsigned long deltaMillis = millis() - startMillis; // вычисляем частоту обработки фильтра fps = 1000 / deltaMillis; -} +} diff --git a/examples/accel/accel.ino b/examples/accel/accel.ino index fad5523..ae1f93f 100755 --- a/examples/accel/accel.ino +++ b/examples/accel/accel.ino @@ -38,7 +38,7 @@ void loop() /* // вывод направления и величины ускорения в м/с² по X, Y и Z float x, y, z; - accel.readAXYZ(&x, &y, &z); + accel.readAXYZ(x, y, z); Serial.print(x); Serial.print("\t\t"); Serial.print(y); @@ -47,4 +47,4 @@ void loop() Serial.println(""); delay(100); */ -} +} diff --git a/examples/barometer/barometer.ino b/examples/barometer/barometer.ino index c2424cc..a470d79 100755 --- a/examples/barometer/barometer.ino +++ b/examples/barometer/barometer.ino @@ -38,4 +38,4 @@ void loop() Serial.print(temperature); Serial.println(" C"); delay(100); -} +} diff --git a/examples/compass/compass/compass.ino b/examples/compass/compass/compass.ino index d2c8226..4d3ba86 100755 --- a/examples/compass/compass/compass.ino +++ b/examples/compass/compass/compass.ino @@ -45,4 +45,4 @@ void loop() Serial.print(compass.readAzimut()); Serial.println(" Degrees"); delay(100); -} +} diff --git a/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino b/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino index 9ed237d..ee4a2d5 100755 --- a/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino +++ b/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino @@ -28,4 +28,4 @@ void loop() Serial.print(","); Serial.println(compass.readZ()); delay(100); -} +} diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index 924b8ec..a3cb467 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -1,11 +1,5 @@ #include "Accelerometer.h" -#define ADR_FS_2 0x00 -#define ADR_FS_4 0x10 -#define ADR_FS_8 0x30 - -#define G 9.8 - LIS331DLH::LIS331DLH(uint8_t addr) : BaseIMU(addr) { } @@ -21,7 +15,7 @@ void LIS331DLH::begin(TwoWire& wire) { _ctrlReg1 |= (1 << 5); // устанавливаем максимальное измеряемое ускорение в G setRange(RANGE_2G); - writeCtrlReg1(); + _writeByte(CTRL_REG1, _ctrlReg1); } void LIS331DLH::setRange(uint8_t range) { @@ -45,7 +39,7 @@ void LIS331DLH::setRange(uint8_t range) { _mult = RANGE_2G / 32767.0; } break; } - writeCtrlReg4(); + _writeByte(CTRL_REG4, _ctrlReg4); } void LIS331DLH::sleep(bool enable) { @@ -54,7 +48,7 @@ void LIS331DLH::sleep(bool enable) { else _ctrlReg1 |= (1 << 5); - writeCtrlReg1(); + _writeByte(CTRL_REG1, _ctrlReg1); } float LIS331DLH::readGX() { return readX() * _mult; } @@ -69,17 +63,17 @@ float LIS331DLH::readAY() { return readGY() * G; } float LIS331DLH::readAZ() { return readGZ() * G; } -void LIS331DLH::readGXYZ(float* gx, float* gy, float* gz) { +void LIS331DLH::readGXYZ(float& gx, float& gy, float& gz) { int16_t x, y, z; - readXYZ(&x, &y, &z); - *gx = x * _mult; - *gy = y * _mult; - *gz = z * _mult; + readXYZ(x, y, z); + gx = x * _mult; + gy = y * _mult; + gz = z * _mult; } -void LIS331DLH::readAXYZ(float* ax, float* ay, float* az) { +void LIS331DLH::readAXYZ(float& ax, float& ay, float& az) { readGXYZ(ax, ay, az); - (*ax) *= G; - (*ay) *= G; - (*az) *= G; + ax *= G; + ay *= G; + az *= G; } diff --git a/src/Accelerometer.h b/src/Accelerometer.h index ca5cb01..579d069 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -10,6 +10,12 @@ #define RANGE_4G 4 #define RANGE_8G 8 +#define ADR_FS_2 0x00 +#define ADR_FS_4 0x10 +#define ADR_FS_8 0x30 + +#define G 9.8 + class LIS331DLH : public BaseIMU { public: LIS331DLH(uint8_t addr = LIS331DLH_ADDRESS); @@ -22,8 +28,8 @@ class LIS331DLH : public BaseIMU { float readAX(); float readAY(); float readAZ(); - void readGXYZ(float* ax, float* ay, float* az); - void readAXYZ(float* gx, float* gy, float* gz); + void readGXYZ(float& ax, float& ay, float& az); + void readAXYZ(float& gx, float& gy, float& gz); }; #endif // __ACCELEROMETER_H__ diff --git a/src/BaseIMU.cpp b/src/BaseIMU.cpp index c4f6d14..ef4a868 100644 --- a/src/BaseIMU.cpp +++ b/src/BaseIMU.cpp @@ -1,47 +1,18 @@ #include "BaseIMU.h" -#define CTRL_REG1 0x20 -#define CTRL_REG2 0x21 -#define CTRL_REG3 0x22 -#define CTRL_REG4 0x23 -#define CTRL_REG5 0x24 - -#define OUT_X 0x28 -#define OUT_Y 0x2A -#define OUT_Z 0x2C - int16_t BaseIMU::readX() { - return readAxis(OUT_X); + return ((int16_t)_readByte(OUT_X + 1) << 8) | _readByte(OUT_X); } int16_t BaseIMU::readY() { - return readAxis(OUT_Y); + return ((int16_t)_readByte(OUT_Y + 1) << 8) | _readByte(OUT_Y); } int16_t BaseIMU::readZ() { - return readAxis(OUT_Z); -} - -int16_t BaseIMU::readAxis(uint8_t reg) { - return ((int16_t)readByte(reg + 1) << 8) | readByte(reg); -} - -void BaseIMU::waitForData() { - while (_wire->available() < 1) - continue; -} - -uint8_t BaseIMU::readByte(uint8_t reg) { - uint8_t value; - _wire->beginTransmission(_addr); - _wire->write(reg); - _wire->endTransmission(); - _wire->requestFrom(_addr, 1u); - waitForData(); - value = _wire->read(); - return value; + return ((int16_t)_readByte(OUT_Z + 1) << 8) | _readByte(OUT_Z); } +/* void BaseIMU::readXYZ(int16_t *x, int16_t *y, int16_t *z) { _wire->beginTransmission(_addr); _wire->write(OUT_X | (1 << 7)); // assert MSB to enable register address auto-increment @@ -51,46 +22,47 @@ void BaseIMU::readXYZ(int16_t *x, int16_t *y, int16_t *z) { _wire->requestFrom(_addr, burstSize); uint8_t values[burstSize]; for (uint8_t i = 0; i < burstSize; i++) { - waitForData(); values[i] = _wire->read(); } *x = *((int16_t*)&values[0]); *y = *((int16_t*)&values[2]); *z = *((int16_t*)&values[4]); -} +}*/ -void BaseIMU::writeCtrlReg1(){ - _wire->beginTransmission(_addr); - _wire->write(CTRL_REG1); - _wire->write(_ctrlReg1); - _wire->endTransmission(); +void BaseIMU::readXYZ(int16_t &x, int16_t &y, int16_t &z) { + uint8_t data[6]; + _readBytes(0x80 | OUT_X, data, 6); + x = ((int16_t)data[1] << 8 ) | data[0]; + y = ((int16_t)data[3] << 8 ) | data[2]; + z = ((int16_t)data[5] << 8 ) | data[4]; } -void BaseIMU::writeCtrlReg2(){ +uint8_t BaseIMU::_readByte(uint8_t reg) { + uint8_t data; _wire->beginTransmission(_addr); - _wire->write(CTRL_REG2); - _wire->write(_ctrlReg2); + _wire->write(reg); _wire->endTransmission(); -} - -void BaseIMU::writeCtrlReg3(){ - _wire->beginTransmission(_addr); - _wire->write(CTRL_REG3); - _wire->write(_ctrlReg3); + _wire->requestFrom(_addr, 1u); + data = _wire->read(); _wire->endTransmission(); + return data; } -void BaseIMU::writeCtrlReg4(){ +void BaseIMU::_writeByte(uint8_t reg, uint8_t data) { _wire->beginTransmission(_addr); - _wire->write(CTRL_REG4); - _wire->write(_ctrlReg4); + _wire->write(reg); + _wire->write(data); _wire->endTransmission(); } -void BaseIMU::writeCtrlReg5(){ +void BaseIMU::_readBytes(uint8_t reg, uint8_t* data, uint8_t length) { _wire->beginTransmission(_addr); - _wire->write(CTRL_REG5); - _wire->write(_ctrlReg5); + _wire->write(reg); + _wire->endTransmission(); + _wire->requestFrom(_addr, length); + for (size_t i = 0; i < length; i++) { + *data++ = _wire->read(); + } _wire->endTransmission(); } diff --git a/src/BaseIMU.h b/src/BaseIMU.h index 4b17e80..1171efd 100644 --- a/src/BaseIMU.h +++ b/src/BaseIMU.h @@ -4,35 +4,40 @@ #include #include +#define CTRL_REG1 0x20 +#define CTRL_REG2 0x21 +#define CTRL_REG3 0x22 +#define CTRL_REG4 0x23 +#define CTRL_REG5 0x24 + +#define OUT_X 0x28 +#define OUT_Y 0x2A +#define OUT_Z 0x2C + class BaseIMU { public: BaseIMU(uint8_t addr) : _addr(addr) { } - uint8_t readByte(uint8_t reg); int16_t readX(); int16_t readY(); int16_t readZ(); - void readXYZ(int16_t* x, int16_t* y, int16_t* z); + void readXYZ(int16_t& x, int16_t& y, int16_t& z); protected: + TwoWire* _wire; void begin(TwoWire& wire = Wire); - void writeCtrlReg1(); - void writeCtrlReg2(); - void writeCtrlReg3(); - void writeCtrlReg4(); - void writeCtrlReg5(); + uint8_t _readByte(uint8_t reg); + void _readBytes(uint8_t reg, uint8_t* data, uint8_t length); + void _writeByte(uint8_t reg, uint8_t data); uint8_t _ctrlReg1; uint8_t _ctrlReg2; uint8_t _ctrlReg3; uint8_t _ctrlReg4; uint8_t _ctrlReg5; - int16_t readAxis(uint8_t reg); - TwoWire* _wire; float _mult; private: uint8_t _addr; - void waitForData(); }; #endif // __BaseIMU_H__ diff --git a/src/Compass.cpp b/src/Compass.cpp index f149859..8c7006f 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -1,15 +1,5 @@ #include "Compass.h" -#define ADR_FS_4 0x00 -#define ADR_FS_8 0x20 -#define ADR_FS_12 0x40 -#define ADR_FS_16 0x60 - -#define SENS_FS_4 6842 -#define SENS_FS_8 3421 -#define SENS_FS_12 2281 -#define SENS_FS_16 1711 - LIS3MDL::LIS3MDL(uint8_t addr) : BaseIMU(addr) { } @@ -18,7 +8,7 @@ void LIS3MDL::begin(TwoWire& wire) { _wire->begin(); // устанавливаем чувствительность setRange(RANGE_4_GAUSS); - writeCtrlReg3(); + _writeByte(CTRL_REG3, _ctrlReg3); } void LIS3MDL::setRange(uint8_t range) { @@ -47,7 +37,7 @@ void LIS3MDL::setRange(uint8_t range) { _mult = SENS_FS_4; } break; } - writeCtrlReg2(); + _writeByte(CTRL_REG2, _ctrlReg2); } void LIS3MDL::sleep(bool enable) { @@ -56,7 +46,7 @@ void LIS3MDL::sleep(bool enable) { else _ctrlReg3 &= ~(3 << 0); - writeCtrlReg3(); + _writeByte(CTRL_REG3, _ctrlReg3); } float LIS3MDL::readGaussX() { return readX() / _mult; } @@ -110,11 +100,11 @@ void LIS3MDL::calibrateMatrix(const double calibrationMatrix[3][3], memcpy(_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof(double)); } -void LIS3MDL::readCalibrateGaussXYZ(float* x, float* y, float* z) { +void LIS3MDL::readCalibrateGaussXYZ(float& x, float& y, float& z) { calibrate(); - *x = _xCalibrate / _mult; - *y = _yCalibrate / _mult; - *z = _zCalibrate / _mult; + x = _xCalibrate / _mult; + y = _yCalibrate / _mult; + z = _zCalibrate / _mult; } float LIS3MDL::readAzimut() { diff --git a/src/Compass.h b/src/Compass.h index 7d0d18a..67f3892 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -11,6 +11,16 @@ #define RANGE_12_GAUSS 2 #define RANGE_16_GAUSS 3 +#define ADR_FS_4 0x00 +#define ADR_FS_8 0x20 +#define ADR_FS_12 0x40 +#define ADR_FS_16 0x60 + +#define SENS_FS_4 6842 +#define SENS_FS_8 3421 +#define SENS_FS_12 2281 +#define SENS_FS_16 1711 + class LIS3MDL : public BaseIMU { public: LIS3MDL(uint8_t addr = LIS3MDL_TWI_ADDRESS); @@ -30,7 +40,7 @@ class LIS3MDL : public BaseIMU { float readCalibrateGaussY(); float readCalibrateGaussZ(); float readAzimut(); - void readCalibrateGaussXYZ(float* x, float* y, float* z); + void readCalibrateGaussXYZ(float& x, float& y, float& z); private: float _xCalibrate; diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp index 67c16e7..6b84266 100644 --- a/src/Gyroscope.cpp +++ b/src/Gyroscope.cpp @@ -1,30 +1,22 @@ #include "Gyroscope.h" -#define ADR_FS_250 0x00 -#define ADR_FS_500 0x10 -#define ADR_FS_2000 0x20 - -#define SENS_FS_250 0.00875 -#define SENS_FS_500 0.0175 -#define SENS_FS_2000 0.07 - L3G4200D::L3G4200D(uint8_t addr) : BaseIMU(addr) { } void L3G4200D::setRange(uint8_t range) { switch (range) { case RANGE_250DPS: { - _ctrlReg4 = ADR_FS_250; + _ctrlReg4 |= ADR_FS_250; _mult = SENS_FS_250; break; } case RANGE_500DPS: { - _ctrlReg4 = ADR_FS_500; + _ctrlReg4 |= ADR_FS_500; _mult = SENS_FS_500; break; } case RANGE_2000DPS: { - _ctrlReg4 = ADR_FS_2000; + _ctrlReg4 |= ADR_FS_2000; _mult = SENS_FS_2000; break; } @@ -32,31 +24,24 @@ void L3G4200D::setRange(uint8_t range) { _mult = SENS_FS_250; } break; } - writeCtrlReg4(); + _writeByte(CTRL_REG4, _ctrlReg4); } void L3G4200D::begin(TwoWire& wire) { - // подключаемся к шине I²C _wire = &wire; _wire->begin(); - // включаем координаты x, y, z - _ctrlReg1 |= (1 << 0); - _ctrlReg1 |= (1 << 1); - _ctrlReg1 |= (1 << 2); - // включаем гироскоп - _ctrlReg1 |= (1 << 3); - // устанавливаем чувствительность + _ctrlReg1 |= (1 << 0) | (1 << 2) | (1 << 3) | (1 << 4); + _writeByte(CTRL_REG1, _ctrlReg1); setRange(RANGE_250DPS); - writeCtrlReg1(); } void L3G4200D::sleep(bool enable) { - if (enable) + if (enable) { _ctrlReg1 &= ~(1 << 3); - else + } else { _ctrlReg1 |= (1 << 3); - - writeCtrlReg1(); + } + _writeByte(CTRL_REG1, _ctrlReg1); } float L3G4200D::readDegPerSecX() { return readX() * _mult; } @@ -71,17 +56,17 @@ float L3G4200D::readRadPerSecY() { return readDegPerSecY() * DEG_TO_RAD; } float L3G4200D::readRadPerSecZ() { return readDegPerSecZ() * DEG_TO_RAD; } -void L3G4200D::readDegPerSecXYZ(float* gx, float* gy, float* gz) { +void L3G4200D::readDegPerSecXYZ(float& gx, float& gy, float& gz) { int16_t x, y, z; - readXYZ(&x, &y, &z); - *gx = x * _mult; - *gy = y * _mult; - *gz = z * _mult; + readXYZ(x, y, z); + gx = x * _mult; + gy = y * _mult; + gz = z * _mult; } -void L3G4200D::readRadPerSecXYZ(float* gx, float* gy, float* gz) { +void L3G4200D::readRadPerSecXYZ(float& gx, float& gy, float& gz) { readDegPerSecXYZ(gx, gy, gz); - (*gx) *= DEG_TO_RAD; - (*gy) *= DEG_TO_RAD; - (*gz) *= DEG_TO_RAD; + gx *= DEG_TO_RAD; + gy *= DEG_TO_RAD; + gz *= DEG_TO_RAD; } diff --git a/src/Gyroscope.h b/src/Gyroscope.h index d6d57a5..8df82a1 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -10,6 +10,14 @@ #define RANGE_500DPS 1 #define RANGE_2000DPS 2 +#define ADR_FS_250 0x00 +#define ADR_FS_500 0x10 +#define ADR_FS_2000 0x20 + +#define SENS_FS_250 0.00875 +#define SENS_FS_500 0.0175 +#define SENS_FS_2000 0.07 + class L3G4200D : public BaseIMU { public: L3G4200D(uint8_t addr = L3G4200D_ADDRESS); @@ -22,8 +30,8 @@ class L3G4200D : public BaseIMU { float readRadPerSecX(); float readRadPerSecY(); float readRadPerSecZ(); - void readDegPerSecXYZ(float* gx, float* gy, float* gz); - void readRadPerSecXYZ(float* gx, float* gy, float* gz); + void readDegPerSecXYZ(float& gx, float& gy, float& gz); + void readRadPerSecXYZ(float& gx, float& gy, float& gz); }; #endif // __GYROSCOPE_H__ From ad8e1770920f908bc7c7454c6e21a4cee7157c4d Mon Sep 17 00:00:00 2001 From: Igor605ds Date: Wed, 7 Oct 2020 02:32:12 +0300 Subject: [PATCH 06/20] rename variables and fuctions --- examples/Madgwick6DOF/Madgwick6DOF.ino | 102 +++++++------ examples/Madgwick9DOF/Madgwick9DOF.ino | 136 ++++++++---------- .../MadgwickProcessing9DOF.ino | 122 ++++++++-------- examples/accel/accel.ino | 68 ++++----- examples/barometer/barometer.ino | 2 +- examples/compass/compass/compass.ino | 66 ++++----- .../compassCalibrateMatrix.ino | 39 +++-- examples/gyro/gyro.ino | 71 ++++----- examples/imu/imu.ino | 122 ++++++++-------- library.properties | 2 +- src/Accelerometer.cpp | 46 +++--- src/Accelerometer.h | 20 +-- src/Barometer.h | 6 +- src/BaseIMU.cpp | 40 ++---- src/BaseIMU.h | 34 ++--- src/Compass.cpp | 103 ++++++------- src/Compass.h | 39 +++-- src/Gyroscope.cpp | 40 +++--- src/Gyroscope.h | 20 +-- src/TroykaIMU.h | 24 ++-- 20 files changed, 498 insertions(+), 604 deletions(-) diff --git a/examples/Madgwick6DOF/Madgwick6DOF.ino b/examples/Madgwick6DOF/Madgwick6DOF.ino index 0fbf79b..46b061c 100755 --- a/examples/Madgwick6DOF/Madgwick6DOF.ino +++ b/examples/Madgwick6DOF/Madgwick6DOF.ino @@ -1,12 +1,12 @@ // библиотека для работы с модулями IMU #include - + // множитель фильтра #define BETA 0.22f - + // создаём объект для фильтра Madgwick Madgwick filter; - + // создаём объект для работы с акселерометром Accelerometer accel(ACCEL_ADDRESS_V1); // создаём объект для работы с гироскопом @@ -15,60 +15,58 @@ Gyroscope gyro(GYRO_ADDRESS_V1); // если напаяны перемычки, устройства доступны по новым адресам // Accelerometer accel(ACCEL_ADDRESS_V2); // Gyroscope gyro(GYRO_ADDRESS_V2); - + // переменные для данных с гироскопов, акселерометров float gx, gy, gz, ax, ay, az; - + // получаемые углы ориентации float yaw, pitch, roll; - + // переменная для хранения частоты выборок фильтра float fps = 100; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - Serial.println("Begin init..."); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); + +void setup() { + // открываем последовательный порт + Serial.begin(9600); + Serial.println("Begin init..."); + // инициализация акселерометра + accel.begin(); + // инициализация гироскопа + gyro.begin(); + // выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); } - -void loop() -{ - // запоминаем текущее время - unsigned long startMillis = millis(); - - // считываем данные с акселерометра в единицах G - accel.readGXYZ(ax, ay, az); - // считываем данные с акселерометра в радианах в секунду - gyro.readRadPerSecXYZ(gx, gy, gz); - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az); - - // получение углов yaw, pitch и roll из фильтра - yaw = filter.getYawDeg(); - pitch = filter.getPitchDeg(); - roll = filter.getRollDeg(); - - // выводим полученные углы в serial-порт - Serial.print("yaw: "); - Serial.print(yaw); - Serial.print("\t\t"); - Serial.print("pitch: "); - Serial.print(pitch); - Serial.print("\t\t"); - Serial.print("roll: "); - Serial.println(roll); - - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; + +void loop() { + // запоминаем текущее время + unsigned long startMillis = millis(); + + // считываем данные с акселерометра в единицах G + accel.readAccelerationGXYZ(ax, ay, az); + // считываем данные с акселерометра в радианах в секунду + gyro.readRotationRadXYZ(gx, gy, gz); + // устанавливаем коэффициенты фильтра + filter.setKoeff(fps, BETA); + // обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az); + + // получение углов yaw, pitch и roll из фильтра + yaw = filter.getYawDeg(); + pitch = filter.getPitchDeg(); + roll = filter.getRollDeg(); + + // выводим полученные углы в serial-порт + Serial.print("yaw: "); + Serial.print(yaw); + Serial.print("\t\t"); + Serial.print("pitch: "); + Serial.print(pitch); + Serial.print("\t\t"); + Serial.print("roll: "); + Serial.println(roll); + + // вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // вычисляем частоту обработки фильтра + fps = 1000 / deltaMillis; } diff --git a/examples/Madgwick9DOF/Madgwick9DOF.ino b/examples/Madgwick9DOF/Madgwick9DOF.ino index 6d12e90..3c16592 100755 --- a/examples/Madgwick9DOF/Madgwick9DOF.ino +++ b/examples/Madgwick9DOF/Madgwick9DOF.ino @@ -1,12 +1,12 @@ // библиотека для работы с модулями IMU #include - + // множитель фильтра #define BETA 0.22 - + // создаём объект для фильтра Madgwick Madgwick filter; - + // создаём объект для работы с акселерометром Accelerometer accel(ACCEL_ADDRESS_V1); // создаём объект для работы с гироскопом @@ -19,83 +19,73 @@ Compass compass(COMPASS_ADDRESS_V1); // Gyroscope gyro(GYRO_ADDRESS_V2); // Compass compass(COMPASS_ADDRESS_V2); - // переменные для данных с гироскопа, акселерометра и компаса float gx, gy, gz, ax, ay, az, mx, my, mz; - + // получаемые углы ориентации (Эйлера) float yaw, pitch, roll; - + // переменная для хранения частоты выборок фильтра float fps = 100; - + // калибровочные значения компаса // полученные в калибровочной матрице из примера «compassCalibrateMatrixx» -const double compassCalibrationBias[3] = { - 524.21, - 3352.214, - -1402.236 -}; - -const double compassCalibrationMatrix[3][3] = { - {1.757, 0.04, -0.028}, - {0.008, 1.767, -0.016}, - {-0.018, 0.077, 1.782} -}; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - Serial.println("Begin init..."); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // инициализация компаса - compass.begin(); - - // калибровка компаса - compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() -{ - // запоминаем текущее время - unsigned long startMillis = millis(); - - // считываем данные с акселерометра в единицах G - accel.readGXYZ(ax, ay, az); - // считываем данные с гироскопа в радианах в секунду - gyro.readRadPerSecXYZ(gx, gy, gz); - // считываем данные с компаса в Гауссах - compass.readCalibrateGaussXYZ(mx, my, mz); - - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); - - // получение углов yaw, pitch и roll из фильтра - yaw = filter.getYawDeg(); - pitch = filter.getPitchDeg(); - roll = filter.getRollDeg(); - - // выводим полученные углы в serial-порт - Serial.print("yaw: "); - Serial.print(yaw); - Serial.print("\t\t"); - Serial.print("pitch: "); - Serial.print(pitch); - Serial.print("\t\t"); - Serial.print("roll: "); - Serial.println(roll); - - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; +void setup() { + // открываем последовательный порт + Serial.begin(9600); + Serial.println("Begin init..."); + // инициализация акселерометра + accel.begin(); + // инициализация гироскопа + gyro.begin(); + // инициализация компаса + compass.begin(); + + // калибровка компаса + compass.setCalibrateMatrix(compassCalibrationMatrix, + compassCalibrationBias); + // выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); } +void loop() { + // запоминаем текущее время + unsigned long startMillis = millis(); + + // считываем данные с акселерометра в единицах G + accel.readAccelerationGXYZ(ax, ay, az); + // считываем данные с гироскопа в радианах в секунду + gyro.readRotationRadXYZ(gx, gy, gz); + // считываем данные с компаса в Гауссах + compass.readCalibrateMagneticGaussXYZ(mx, my, mz); + + // устанавливаем коэффициенты фильтра + filter.setKoeff(fps, BETA); + // обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); + + // получение углов yaw, pitch и roll из фильтра + yaw = filter.getYawDeg(); + pitch = filter.getPitchDeg(); + roll = filter.getRollDeg(); + + // выводим полученные углы в serial-порт + Serial.print("yaw: "); + Serial.print(yaw); + Serial.print("\t\t"); + Serial.print("pitch: "); + Serial.print(pitch); + Serial.print("\t\t"); + Serial.print("roll: "); + Serial.println(roll); + + // вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // вычисляем частоту обработки фильтра + fps = 1000 / deltaMillis; +} diff --git a/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino b/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino index cf20f55..bd668af 100755 --- a/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino +++ b/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino @@ -4,95 +4,87 @@ // библиотека для работы с модулями IMU #include - + // множитель фильтра #define BETA 0.22 - + // создаём объект для фильтра Madgwick Madgwick filter; - + // создаём объект для работы с акселерометром Accelerometer accel(ACCEL_ADDRESS_V1); // создаём объект для работы с гироскопом Gyroscope gyro(GYRO_ADDRESS_V1); // создаём объект для работы с компасом Compass compass(COMPASS_ADDRESS_V1); - + // если напаяны перемычки, устройства доступны по новым адресам // Accelerometer accel(ACCEL_ADDRESS_V2); // Gyroscope gyro(GYRO_ADDRESS_V2); // Compass compass(COMPASS_ADDRESS_V2); - + // переменные для данных с гироскопа, акселерометра и компаса float gx, gy, gz, ax, ay, az, mx, my, mz; - + // получаемые углы ориентации (Эйлера) float yaw, pitch, roll; - + // переменная для хранения частоты выборок фильтра float fps = 100; // калибровочные значения компаса // полученные в калибровочной матрице из примера «compassCalibrateMatrixx» -const double compassCalibrationBias[3] = { - 524.21, - 3352.214, - -1402.236 -}; - -const double compassCalibrationMatrix[3][3] = { - {1.757, 0.04, -0.028}, - {0.008, 1.767, -0.016}, - {-0.018, 0.077, 1.782} -}; +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // инициализация компаса - compass.begin(); - // калибровка компаса - compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); +void setup() { + // открываем последовательный порт + Serial.begin(9600); + // инициализация акселерометра + accel.begin(); + // инициализация гироскопа + gyro.begin(); + // инициализация компаса + compass.begin(); + // калибровка компаса + compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); } - -void loop() -{ - // запоминаем текущее время - unsigned long startMillis = millis(); - // считываем данные с акселерометра в единицах G - accel.readGXYZ(ax, ay, az); - // считываем данные с гироскопа в радианах в секунду - gyro.readRadPerSecXYZ(gx, gy, gz); - // считываем данные с компаса в Гауссах - compass.readCalibrateGaussXYZ(mx, my, mz); - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); - - if (Serial.available() > 0) { - int val = Serial.read(); - // если пришёл символ 's' - if (val == 's') { - float q0, q1, q2, q3; - filter.readQuaternions(&q0, &q1, &q2, &q3); - // выводим кватернионы в serial-порт - Serial.print(q0); - Serial.print(","); - Serial.print(q1); - Serial.print(","); - Serial.print(q2); - Serial.print(","); - Serial.println(q3); + +void loop() { + // запоминаем текущее время + unsigned long startMillis = millis(); + // считываем данные с акселерометра в единицах G + accel.readAccelerationGXYZ(ax, ay, az); + // считываем данные с гироскопа в радианах в секунду + gyro.readRotationRadXYZ(gx, gy, gz); + // считываем данные с компаса в Гауссах + compass.readCalibrateMagneticGaussXYZ(mx, my, mz); + // устанавливаем коэффициенты фильтра + filter.setKoeff(fps, BETA); + // обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); + + if (Serial.available() > 0) { + int val = Serial.read(); + // если пришёл символ 's' + if (val == 's') { + float q0, q1, q2, q3; + filter.readQuaternions(&q0, &q1, &q2, &q3); + // выводим кватернионы в serial-порт + Serial.print(q0); + Serial.print(","); + Serial.print(q1); + Serial.print(","); + Serial.print(q2); + Serial.print(","); + Serial.println(q3); + } } - } - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; + // вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // вычисляем частоту обработки фильтра + fps = 1000 / deltaMillis; } diff --git a/examples/accel/accel.ino b/examples/accel/accel.ino index ae1f93f..f4b74f9 100755 --- a/examples/accel/accel.ino +++ b/examples/accel/accel.ino @@ -1,50 +1,36 @@ // библиотека для работы с модулями IMU #include - + // создаём объект для работы с акселерометром Accelerometer accel(ACCEL_ADDRESS_V1); - + // если напаяна перемычка, устройство доступно по новому адресу // Accelerometer accel(ACCEL_ADDRESS_V2); -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // выводим сообщение о начале инициализации - Serial.println("Accelerometer init..."); - // инициализация акселерометра - accel.begin(); - // устанавливаем чувствительность акселерометра - // 2g — по умолчанию, 4g, 8g - accel.setRange(RANGE_2G); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); +void setup() { + // открываем последовательный порт + Serial.begin(9600); + // выводим сообщение о начале инициализации + Serial.println("Accelerometer init..."); + // инициализация акселерометра + accel.begin(); + // устанавливаем чувствительность акселерометра + // 2g — по умолчанию, 4g, 8g + accel.setRange(RANGE_2G); + // выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); } - -void loop() -{ - // вывод направления и величины ускорения в м/с² по оси X - Serial.print(accel.readAX()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси Y - Serial.print(accel.readAY()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси Z - Serial.print(accel.readAZ()); - Serial.print("\t\t"); - Serial.println(""); - delay(100); - /* - // вывод направления и величины ускорения в м/с² по X, Y и Z - float x, y, z; - accel.readAXYZ(x, y, z); - Serial.print(x); - Serial.print("\t\t"); - Serial.print(y); - Serial.print("\t\t"); - Serial.print(z); - Serial.println(""); - delay(100); - */ + +void loop() { + // вывод направления и величины ускорения в м/с² по оси X + Serial.print(accel.readAccelerationAX()); + Serial.print("\t\t"); + // вывод направления и величины ускорения в м/с² по оси Y + Serial.print(accel.readAccelerationAY()); + Serial.print("\t\t"); + // вывод направления и величины ускорения в м/с² по оси Z + Serial.print(accel.readAccelerationAZ()); + Serial.print("\t\t"); + Serial.println(""); + delay(100); } diff --git a/examples/barometer/barometer.ino b/examples/barometer/barometer.ino index a470d79..c9244e5 100755 --- a/examples/barometer/barometer.ino +++ b/examples/barometer/barometer.ino @@ -9,7 +9,7 @@ Barometer barometer(BARO_ADDRESS_V1); void setup() { // открываем последовательный порт - Serial.begin(115200); + Serial.begin(9600); // выводим сообщение о начале инициализации Serial.println("Begin init..."); // инициализация барометра diff --git a/examples/compass/compass/compass.ino b/examples/compass/compass/compass.ino index 4d3ba86..3e72dd3 100755 --- a/examples/compass/compass/compass.ino +++ b/examples/compass/compass/compass.ino @@ -1,48 +1,40 @@ // библиотека для работы с модулями IMU #include - + // создаём объект для работы с компасом Compass compass(COMPASS_ADDRESS_V1); // если напаяна перемычка, устройство доступно по новому адресу // Compass compass(COMPASS_ADDRESS_V2); - + // калибровочные значения, полученные в калибровочной матрице // из примера compass_cal_matrix -const double compassCalibrationBias[3] = { - 524.21, - 3352.214, - -1402.236 -}; - -const double compassCalibrationMatrix[3][3] = { - {1.757, 0.04, -0.028}, - {0.008, 1.767, -0.016}, - {-0.018, 0.077, 1.782} -}; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // пока не появились данные с USB - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация компаса - compass.begin(); - // устанавливаем чувствительность компаса - // ±4 gauss — по умолчанию, ±8 gauss, ±12 gauss, ±16 gauss - compass.setRange(RANGE_4_GAUSS); - // калибровка компаса - compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; + +void setup() { + // открываем последовательный порт + Serial.begin(9600); + // пока не появились данные с USB + // выводим сообщение о начале инициализации + Serial.println("Begin init..."); + // инициализация компаса + compass.begin(); + // устанавливаем чувствительность компаса + // ±4 gauss — по умолчанию, ±8 gauss, ±12 gauss, ±16 gauss + compass.setRange(RANGE_4_GAUSS); + // калибровка компаса + compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); + // выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); } - -void loop() -{ - // выводим азимут относительно оси Z - Serial.print(compass.readAzimut()); - Serial.println(" Degrees"); - delay(100); + +void loop() { + // выводим азимут относительно оси Z + Serial.print(compass.readAzimut()); + Serial.println(" Degrees"); + delay(100); } diff --git a/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino b/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino index ee4a2d5..9b08906 100755 --- a/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino +++ b/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino @@ -1,31 +1,28 @@ - // библиотека для работы с модулями IMU #include - + // создаём объект для работы с компасом Compass compass(COMPASS_ADDRESS_V1); // если напаяна перемычка, устройство доступно по новому адресу // Compass compass(COMPASS_ADDRESS_V2); -void setup() -{ - // открываем последовательный порт - Serial.begin(9600); - // инициализация компаса - compass.begin(); - // устанавливаем чувствительность компаса - // ±4 gauss — по умолчанию, ±8 gauss, ±12 gauss, ±16 gauss - compass.setRange(RANGE_4_GAUSS); +void setup() { + // открываем последовательный порт + Serial.begin(9600); + // инициализация компаса + compass.begin(); + // устанавливаем чувствительность компаса + // ±4 gauss — по умолчанию, ±8 gauss, ±12 gauss, ±16 gauss + compass.setRange(RANGE_4_GAUSS); } - -void loop() -{ - // Выводим «сырые» значения компаса по трём осям - Serial.print(compass.readX()); - Serial.print(","); - Serial.print(compass.readY()); - Serial.print(","); - Serial.println(compass.readZ()); - delay(100); + +void loop() { + // Выводим «сырые» значения компаса по трём осям + Serial.print(compass.readX()); + Serial.print(","); + Serial.print(compass.readY()); + Serial.print(","); + Serial.println(compass.readZ()); + delay(100); } diff --git a/examples/gyro/gyro.ino b/examples/gyro/gyro.ino index 6eda753..d8078cf 100755 --- a/examples/gyro/gyro.ino +++ b/examples/gyro/gyro.ino @@ -2,52 +2,37 @@ #include // библиотека для работы с модулями IMU #include - + // создаём объект для работы с гироскопом Gyroscope gyro(GYRO_ADDRESS_V1); // если напаяна перемычка, устройство доступно по новому адресу // Gyroscope gyro(GYRO_ADDRESS_V2); - - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // выводим сообщение о начале инициализации - Serial.println("Gyroscope init..."); - // инициализация гироскопа - gyro.begin(); - // устанавливаем чувствительность гироскопа - // 250dps — по умолчанию, 500dps, 2000dps - gyro.setRange(RANGE_250DPS); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); + +void setup() { + // открываем последовательный порт + Serial.begin(9600); + // выводим сообщение о начале инициализации + Serial.println("Gyroscope init..."); + // инициализация гироскопа + gyro.begin(); + // устанавливаем чувствительность гироскопа + // 250dps — по умолчанию, 500dps, 2000dps + gyro.setRange(RANGE_250DPS); + // выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // вывод угловой скорости в градусах в секунду относительно оси X + Serial.print(gyro.readRotationDegX()); + Serial.print("\t\t"); + // вывод угловой скорости в градусах в секунду относительно оси Y + Serial.print(gyro.readRotationDegY()); + Serial.print("\t\t"); + // вывод угловой скорости в градусах в секунду относительно оси Z + Serial.print(gyro.readRotationDegZ()); + Serial.print("\t\t"); + Serial.println(""); + delay(100); } - -void loop() -{ - // вывод угловой скорости в градусах в секунду относительно оси X - Serial.print(gyro.readDegPerSecX()); - Serial.print("\t\t"); - // вывод угловой скорости в градусах в секунду относительно оси Y - Serial.print(gyro.readDegPerSecY()); - Serial.print("\t\t"); - // вывод угловой скорости в градусах в секунду относительно оси Z - Serial.print(gyro.readDegPerSecZ()); - Serial.print("\t\t"); - Serial.println(""); - delay(100); -/* - // вывод угловой скорости в градусах в секунду относительно оси X, Y и Z - float x, y, z; - gyro.readDegPerSecXYZ(&x, &y, &z); - Serial.print(x); - Serial.print("\t\t"); - Serial.print(y); - Serial.print("\t\t"); - Serial.print(z); - Serial.println(""); - delay(100); -*/ -} diff --git a/examples/imu/imu.ino b/examples/imu/imu.ino index 999e3d8..38e998f 100755 --- a/examples/imu/imu.ino +++ b/examples/imu/imu.ino @@ -1,6 +1,6 @@ // библиотека для работы с модулями IMU #include - + // создаём объект для работы с гироскопом Gyroscope gyro(GYRO_ADDRESS_V1); // создаём объект для работы с акселерометром @@ -18,68 +18,60 @@ Barometer barometer(BARO_ADDRESS_V1); // калибровочные значения компаса // полученные в калибровочной матрице из примера «compassCalibrateMatrix» -const double compassCalibrationBias[3] = { - 524.21, - 3352.214, - -1402.236 -}; - -const double compassCalibrationMatrix[3][3] = { - {1.757, 0.04, -0.028}, - {0.008, 1.767, -0.016}, - {-0.018, 0.077, 1.782} -}; - -void setup() -{ - // открываем последовательный порт - Serial.begin(115200); - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация гироскопа - gyro.begin(); - // инициализация акселерометра - accel.begin(); - // инициализация компаса - compass.begin(); - // инициализация барометра - barometer.begin(); - // калибровка компаса - compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); - Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer"); +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; + +void setup() { + // открываем последовательный порт + Serial.begin(9600); + // выводим сообщение о начале инициализации + Serial.println("Begin init..."); + // инициализация гироскопа + gyro.begin(); + // инициализация акселерометра + accel.begin(); + // инициализация компаса + compass.begin(); + // инициализация барометра + barometer.begin(); + // калибровка компаса + compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); + // выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); + Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer"); +} + +void loop() { + // вывод угловой скорости в градусах в секунду относительно оси X + Serial.print(gyro.readRotationDegX()); + Serial.print("\t"); + // вывод угловой скорости в градусах в секунду относительно оси Y + Serial.print(gyro.readRotationDegY()); + Serial.print("\t"); + // вывод угловой скорости в градусах в секунду относительно оси Z + Serial.print(gyro.readRotationDegZ()); + Serial.print("\t\t"); + // вывод направления и величины ускорения в м/с² по оси X + Serial.print(accel.readAccelerationAX()); + Serial.print("\t"); + // вывод направления и величины ускорения в м/с² по оси Y + Serial.print(accel.readAccelerationAY()); + Serial.print("\t"); + // вывод направления и величины ускорения в м/с² по оси Z + Serial.print(accel.readAccelerationAZ()); + Serial.print("\t\t"); + // выводим азимут относительно оси Z + Serial.print(compass.readAzimut()); + Serial.print(" Degrees\t"); + // вывод значения абсолютного давления + Serial.print(barometer.readPressureMillibars()); + Serial.print("\t"); + // вывод значения температуры окружающей среды + Serial.print(barometer.readTemperatureC()); + Serial.print("\t"); + Serial.println(""); + delay(100); } - -void loop() -{ - // вывод угловой скорости в градусах в секунду относительно оси X - Serial.print(gyro.readDegPerSecX()); - Serial.print("\t"); - // вывод угловой скорости в градусах в секунду относительно оси Y - Serial.print(gyro.readDegPerSecY()); - Serial.print("\t"); - // вывод угловой скорости в градусах в секунду относительно оси Z - Serial.print(gyro.readDegPerSecZ()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси X - Serial.print(accel.readAX()); - Serial.print("\t"); - // вывод направления и величины ускорения в м/с² по оси Y - Serial.print(accel.readAY()); - Serial.print("\t"); - // вывод направления и величины ускорения в м/с² по оси Z - Serial.print(accel.readAZ()); - Serial.print("\t\t"); - // выводим азимут относительно оси Z - Serial.print(compass.readAzimut()); - Serial.print(" Degrees\t"); - // вывод значения абсолютного давления - Serial.print(barometer.readPressureMillibars()); - Serial.print("\t"); - // вывод значения температуры окружающей среды - Serial.print(barometer.readTemperatureC()); - Serial.print("\t"); - Serial.println(""); - delay(100); -} diff --git a/library.properties b/library.properties index 99ce4c9..4bed445 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Troyka-IMU -version=1.1.0 +version=2.0.0 author=Igor Dementiev maintainer=Amperka sentence=Arduino library for Amperka IMU-sensor. diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index a3cb467..a6216ba 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -1,49 +1,45 @@ #include "Accelerometer.h" -LIS331DLH::LIS331DLH(uint8_t addr) - : BaseIMU(addr) { } +LIS331DLH::LIS331DLH(uint8_t slaveAddress) + : BaseIMU(slaveAddress) { } void LIS331DLH::begin(TwoWire& wire) { - // подключаемся к шине I²C _wire = &wire; _wire->begin(); - // включаем координаты x, y, z _ctrlReg1 |= (1 << 0); _ctrlReg1 |= (1 << 1); _ctrlReg1 |= (1 << 2); - // включаем аксселерометр _ctrlReg1 |= (1 << 5); - // устанавливаем максимальное измеряемое ускорение в G - setRange(RANGE_2G); _writeByte(CTRL_REG1, _ctrlReg1); + setRange(RANGE_2G); } void LIS331DLH::setRange(uint8_t range) { switch (range) { case RANGE_2G: { _ctrlReg4 = ADR_FS_2; - _mult = RANGE_2G / 32767.0; + _scale = RANGE_2G / 32767.0; break; } case RANGE_4G: { _ctrlReg4 = ADR_FS_4; - _mult = RANGE_4G / 32767.0; + _scale = RANGE_4G / 32767.0; break; } case RANGE_8G: { _ctrlReg4 = ADR_FS_8; - _mult = RANGE_8G / 32767.0; + _scale = RANGE_8G / 32767.0; break; } default: { - _mult = RANGE_2G / 32767.0; + _scale = RANGE_2G / 32767.0; } break; } _writeByte(CTRL_REG4, _ctrlReg4); } -void LIS331DLH::sleep(bool enable) { - if (enable) +void LIS331DLH::sleep(bool state) { + if (state) _ctrlReg1 &= ~(1 << 5); else _ctrlReg1 |= (1 << 5); @@ -51,28 +47,28 @@ void LIS331DLH::sleep(bool enable) { _writeByte(CTRL_REG1, _ctrlReg1); } -float LIS331DLH::readGX() { return readX() * _mult; } +float LIS331DLH::readAccelerationGX() { return readX() * _scale; } -float LIS331DLH::readGY() { return readY() * _mult; } +float LIS331DLH::readAccelerationGY() { return readY() * _scale; } -float LIS331DLH::readGZ() { return readZ() * _mult; } +float LIS331DLH::readAccelerationGZ() { return readZ() * _scale; } -float LIS331DLH::readAX() { return readGX() * G; } +float LIS331DLH::readAccelerationAX() { return readAccelerationGX() * G; } -float LIS331DLH::readAY() { return readGY() * G; } +float LIS331DLH::readAccelerationAY() { return readAccelerationGY() * G; } -float LIS331DLH::readAZ() { return readGZ() * G; } +float LIS331DLH::readAccelerationAZ() { return readAccelerationGZ() * G; } -void LIS331DLH::readGXYZ(float& gx, float& gy, float& gz) { +void LIS331DLH::readAccelerationGXYZ(float& ax, float& ay, float& az) { int16_t x, y, z; readXYZ(x, y, z); - gx = x * _mult; - gy = y * _mult; - gz = z * _mult; + ax = x * _scale; + ay = y * _scale; + az = z * _scale; } -void LIS331DLH::readAXYZ(float& ax, float& ay, float& az) { - readGXYZ(ax, ay, az); +void LIS331DLH::readAccelerationAXYZ(float& ax, float& ay, float& az) { + readAccelerationGXYZ(ax, ay, az); ax *= G; ay *= G; az *= G; diff --git a/src/Accelerometer.h b/src/Accelerometer.h index 579d069..2f3f434 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -18,18 +18,18 @@ class LIS331DLH : public BaseIMU { public: - LIS331DLH(uint8_t addr = LIS331DLH_ADDRESS); + LIS331DLH(uint8_t slaveAddress = LIS331DLH_ADDRESS); void begin(TwoWire &wire = Wire); - void sleep(bool enable); + void sleep(bool state); void setRange(uint8_t range); - float readGX(); - float readGY(); - float readGZ(); - float readAX(); - float readAY(); - float readAZ(); - void readGXYZ(float& ax, float& ay, float& az); - void readAXYZ(float& gx, float& gy, float& gz); + float readAccelerationGX(); + float readAccelerationGY(); + float readAccelerationGZ(); + float readAccelerationAX(); + float readAccelerationAY(); + float readAccelerationAZ(); + void readAccelerationGXYZ(float& ax, float& ay, float& az); + void readAccelerationAXYZ(float& ax, float& ay, float& az); }; #endif // __ACCELEROMETER_H__ diff --git a/src/Barometer.h b/src/Barometer.h index 7770049..63d4dab 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -7,8 +7,8 @@ // and sets the last bit correctly based on reads and writes #define LPS331AP_ADDRESS_SA0_LOW 0b1011100 #define LPS331AP_ADDRESS_SA0_HIGH 0b1011101 -#define LPS331AP_TWI_ADDRESS 0b1011100 -#define LPS331AP_TWI_ADDRESS_V2 0b1011101 +#define LPS331AP_ADDRESS 0b1011100 +#define LPS331AP_ADDRESS_V2 0b1011101 // SA0 states @@ -57,7 +57,7 @@ class LPS331 { public: - LPS331(uint8_t addr = LPS331AP_TWI_ADDRESS); + LPS331(uint8_t addr = LPS331AP_ADDRESS); void begin(TwoWire &wire = Wire); void writeReg(byte reg, byte value); byte readReg(byte reg); diff --git a/src/BaseIMU.cpp b/src/BaseIMU.cpp index ef4a868..c232462 100644 --- a/src/BaseIMU.cpp +++ b/src/BaseIMU.cpp @@ -12,24 +12,6 @@ int16_t BaseIMU::readZ() { return ((int16_t)_readByte(OUT_Z + 1) << 8) | _readByte(OUT_Z); } -/* -void BaseIMU::readXYZ(int16_t *x, int16_t *y, int16_t *z) { - _wire->beginTransmission(_addr); - _wire->write(OUT_X | (1 << 7)); // assert MSB to enable register address auto-increment - _wire->endTransmission(); - - uint8_t burstSize = 6; - _wire->requestFrom(_addr, burstSize); - uint8_t values[burstSize]; - for (uint8_t i = 0; i < burstSize; i++) { - values[i] = _wire->read(); - } - - *x = *((int16_t*)&values[0]); - *y = *((int16_t*)&values[2]); - *z = *((int16_t*)&values[4]); -}*/ - void BaseIMU::readXYZ(int16_t &x, int16_t &y, int16_t &z) { uint8_t data[6]; _readBytes(0x80 | OUT_X, data, 6); @@ -38,29 +20,29 @@ void BaseIMU::readXYZ(int16_t &x, int16_t &y, int16_t &z) { z = ((int16_t)data[5] << 8 ) | data[4]; } -uint8_t BaseIMU::_readByte(uint8_t reg) { +uint8_t BaseIMU::_readByte(uint8_t regAddress) { uint8_t data; - _wire->beginTransmission(_addr); - _wire->write(reg); + _wire->beginTransmission(_slaveAddress); + _wire->write(regAddress); _wire->endTransmission(); - _wire->requestFrom(_addr, 1u); + _wire->requestFrom(_slaveAddress, 1u); data = _wire->read(); _wire->endTransmission(); return data; } -void BaseIMU::_writeByte(uint8_t reg, uint8_t data) { - _wire->beginTransmission(_addr); - _wire->write(reg); +void BaseIMU::_writeByte(uint8_t regAddress, uint8_t data) { + _wire->beginTransmission(_slaveAddress); + _wire->write(regAddress); _wire->write(data); _wire->endTransmission(); } -void BaseIMU::_readBytes(uint8_t reg, uint8_t* data, uint8_t length) { - _wire->beginTransmission(_addr); - _wire->write(reg); +void BaseIMU::_readBytes(uint8_t regAddress, uint8_t* data, uint8_t length) { + _wire->beginTransmission(_slaveAddress); + _wire->write(regAddress); _wire->endTransmission(); - _wire->requestFrom(_addr, length); + _wire->requestFrom(_slaveAddress, length); for (size_t i = 0; i < length; i++) { *data++ = _wire->read(); } diff --git a/src/BaseIMU.h b/src/BaseIMU.h index 1171efd..cedde84 100644 --- a/src/BaseIMU.h +++ b/src/BaseIMU.h @@ -4,40 +4,40 @@ #include #include -#define CTRL_REG1 0x20 -#define CTRL_REG2 0x21 -#define CTRL_REG3 0x22 -#define CTRL_REG4 0x23 -#define CTRL_REG5 0x24 +#define CTRL_REG1 0x20 +#define CTRL_REG2 0x21 +#define CTRL_REG3 0x22 +#define CTRL_REG4 0x23 +#define CTRL_REG5 0x24 -#define OUT_X 0x28 -#define OUT_Y 0x2A -#define OUT_Z 0x2C +#define OUT_X 0x28 +#define OUT_Y 0x2A +#define OUT_Z 0x2C class BaseIMU { public: - BaseIMU(uint8_t addr) - : _addr(addr) { } + BaseIMU(uint8_t slaveAddress) + : _slaveAddress(slaveAddress) { } + void begin(TwoWire& wire = Wire); int16_t readX(); int16_t readY(); int16_t readZ(); void readXYZ(int16_t& x, int16_t& y, int16_t& z); protected: - TwoWire* _wire; - void begin(TwoWire& wire = Wire); - uint8_t _readByte(uint8_t reg); - void _readBytes(uint8_t reg, uint8_t* data, uint8_t length); - void _writeByte(uint8_t reg, uint8_t data); + uint8_t _readByte(uint8_t regAddress); + void _writeByte(uint8_t regAddress, uint8_t data); + void _readBytes(uint8_t regAddress, uint8_t* data, uint8_t length); uint8_t _ctrlReg1; uint8_t _ctrlReg2; uint8_t _ctrlReg3; uint8_t _ctrlReg4; uint8_t _ctrlReg5; - float _mult; + float _scale; + TwoWire* _wire; private: - uint8_t _addr; + uint8_t _slaveAddress; }; #endif // __BaseIMU_H__ diff --git a/src/Compass.cpp b/src/Compass.cpp index 8c7006f..5c52c88 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -1,47 +1,46 @@ #include "Compass.h" -LIS3MDL::LIS3MDL(uint8_t addr) - : BaseIMU(addr) { } +LIS3MDL::LIS3MDL(uint8_t slaveAddress) + : BaseIMU(slaveAddress) { } void LIS3MDL::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - // устанавливаем чувствительность - setRange(RANGE_4_GAUSS); _writeByte(CTRL_REG3, _ctrlReg3); + setRange(RANGE_4_GAUSS); } void LIS3MDL::setRange(uint8_t range) { switch (range) { case RANGE_4_GAUSS: { _ctrlReg2 = ADR_FS_4; - _mult = SENS_FS_4; + _scale = SENS_FS_4; break; } case RANGE_8_GAUSS: { _ctrlReg2 = ADR_FS_8; - _mult = SENS_FS_8; + _scale = SENS_FS_8; break; } case RANGE_12_GAUSS: { _ctrlReg2 = ADR_FS_12; - _mult = SENS_FS_12; + _scale = SENS_FS_12; break; } case RANGE_16_GAUSS: { _ctrlReg2 = ADR_FS_16; - _mult = SENS_FS_16; + _scale = SENS_FS_16; break; } default: { - _mult = SENS_FS_4; + _scale = SENS_FS_4; } break; } _writeByte(CTRL_REG2, _ctrlReg2); } -void LIS3MDL::sleep(bool enable) { - if (enable) +void LIS3MDL::sleep(bool state) { + if (state) _ctrlReg3 |= (3 << 0); else _ctrlReg3 &= ~(3 << 0); @@ -49,67 +48,59 @@ void LIS3MDL::sleep(bool enable) { _writeByte(CTRL_REG3, _ctrlReg3); } -float LIS3MDL::readGaussX() { return readX() / _mult; } +float LIS3MDL::readMagneticGaussX() { return readX() / _scale; } -float LIS3MDL::readGaussY() { return readY() / _mult; } +float LIS3MDL::readMagneticGaussY() { return readY() / _scale; } -float LIS3MDL::readGaussZ() { return readZ() / _mult; } +float LIS3MDL::readMagneticGaussZ() { return readZ() / _scale; } -float LIS3MDL::readCalibrateX() { - calibrate(); - return _xCalibrate; +void LIS3MDL::readMagneticGaussXYZ(float& mx, float& my, float& mz) { + int16_t x, y, z; + readXYZ(x, y, z); + mx = x / _scale; + my = y / _scale; + mz = z / _scale; } -float LIS3MDL::readCalibrateY() { - calibrate(); - return _yCalibrate; +void LIS3MDL::readCalibrateMagneticGaussXYZ(float& mx, float& my, float& mz) { + int16_t x, y, z; + readXYZ(x, y, z); + _calibrate((float&)x, (float&)y, (float&)z); + mx = x / _scale; + my = y / _scale; + mz = z / _scale; } -float LIS3MDL::readCalibrateZ() { - calibrate(); - return _zCalibrate; +void LIS3MDL::setCalibrateMatrix(const float calibrationMatrix[3][3], + const float calibrationBias[3]) { + memcpy(_calibrationBias, calibrationBias, 3 * sizeof(float)); + memcpy(_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof(float)); } -float LIS3MDL::readCalibrateGaussX() { return readCalibrateX() / _mult; } - -float LIS3MDL::readCalibrateGaussY() { return readCalibrateY() / _mult; } - -float LIS3MDL::readCalibrateGaussZ() { return readCalibrateZ() / _mult; } - -void LIS3MDL::calibrate() { - float result[3] = { 0, 0, 0 }; - float uncalibratedValues[3]; - uncalibratedValues[0] = readX() - _bias[0]; - uncalibratedValues[1] = readY() - _bias[1]; - uncalibratedValues[2] = readZ() - _bias[2]; +void LIS3MDL::_calibrate(float& x, float& y, float& z) { + float calibrationValues[3] = { 0, 0, 0 }; + float nonCalibrationValues[3] = { 0, 0, 0 }; + nonCalibrationValues[0] = x - _calibrationBias[0]; + nonCalibrationValues[1] = y - _calibrationBias[1]; + nonCalibrationValues[2] = z - _calibrationBias[2]; - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - result[i] += _calibrationMatrix[i][j] * uncalibratedValues[j]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + calibrationValues[i] += _calibrationMatrix[i][j] * nonCalibrationValues[j]; } } - _xCalibrate = result[0]; - _yCalibrate = result[1]; - _zCalibrate = result[2]; -} - -void LIS3MDL::calibrateMatrix(const double calibrationMatrix[3][3], - const double bias[3]) { - memcpy(_bias, bias, 3 * sizeof(double)); - memcpy(_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof(double)); -} - -void LIS3MDL::readCalibrateGaussXYZ(float& x, float& y, float& z) { - calibrate(); - x = _xCalibrate / _mult; - y = _yCalibrate / _mult; - z = _zCalibrate / _mult; + x = calibrationValues[0]; + y = calibrationValues[1]; + z = calibrationValues[2]; } float LIS3MDL::readAzimut() { - calibrate(); - float heading = atan2(_yCalibrate, _xCalibrate); + float x = readX(); + float y = readY(); + float z = readZ(); + _calibrate(x, y, z); + float heading = atan2(x, y); if (heading < 0) heading += TWO_PI; diff --git a/src/Compass.h b/src/Compass.h index 67f3892..fbd890d 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -3,8 +3,8 @@ #include "BaseIMU.h" -#define LIS3MDL_TWI_ADDRESS 0b0011100 -#define LIS3MDL_TWI_ADDRESS_V2 0b0011110 +#define LIS3MDL_ADDRESS 0b0011100 +#define LIS3MDL_ADDRESS_V2 0b0011110 #define RANGE_4_GAUSS 0 #define RANGE_8_GAUSS 1 @@ -23,31 +23,24 @@ class LIS3MDL : public BaseIMU { public: - LIS3MDL(uint8_t addr = LIS3MDL_TWI_ADDRESS); - void begin(TwoWire &wire = Wire); - void sleep(bool enable); + LIS3MDL(uint8_t slaveAddress = LIS3MDL_ADDRESS); + void begin(TwoWire& wire = Wire); + void sleep(bool state); void setRange(uint8_t range); - void calibrateMatrix(const double calibrationMatrix[3][3], - const double bias[3]); - void calibrate(); - float readGaussX(); - float readGaussY(); - float readGaussZ(); - float readCalibrateX(); - float readCalibrateY(); - float readCalibrateZ(); - float readCalibrateGaussX(); - float readCalibrateGaussY(); - float readCalibrateGaussZ(); + float readMagneticGaussX(); + float readMagneticGaussY(); + float readMagneticGaussZ(); + void readMagneticGaussXYZ(float& x, float& y, float& z); + void readCalibrateMagneticGaussXYZ(float& x, float& y, float& z); + void setCalibrateMatrix(const float calibrationMatrix[3][3], + const float bias[3]); float readAzimut(); - void readCalibrateGaussXYZ(float& x, float& y, float& z); + private: - float _xCalibrate; - float _yCalibrate; - float _zCalibrate; - double _calibrationMatrix[3][3]; - double _bias[3]; + void _calibrate(float& x, float& y, float& z); + float _calibrationMatrix[3][3]; + float _calibrationBias[3]; }; #endif // __COMPASS_H__ diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp index 6b84266..9895fdb 100644 --- a/src/Gyroscope.cpp +++ b/src/Gyroscope.cpp @@ -1,27 +1,27 @@ #include "Gyroscope.h" -L3G4200D::L3G4200D(uint8_t addr) - : BaseIMU(addr) { } +L3G4200D::L3G4200D(uint8_t slaveAddress) + : BaseIMU(slaveAddress) { } void L3G4200D::setRange(uint8_t range) { switch (range) { case RANGE_250DPS: { _ctrlReg4 |= ADR_FS_250; - _mult = SENS_FS_250; + _scale = SENS_FS_250; break; } case RANGE_500DPS: { _ctrlReg4 |= ADR_FS_500; - _mult = SENS_FS_500; + _scale = SENS_FS_500; break; } case RANGE_2000DPS: { _ctrlReg4 |= ADR_FS_2000; - _mult = SENS_FS_2000; + _scale = SENS_FS_2000; break; } default: { - _mult = SENS_FS_250; + _scale = SENS_FS_250; } break; } _writeByte(CTRL_REG4, _ctrlReg4); @@ -35,8 +35,8 @@ void L3G4200D::begin(TwoWire& wire) { setRange(RANGE_250DPS); } -void L3G4200D::sleep(bool enable) { - if (enable) { +void L3G4200D::sleep(bool state) { + if (state) { _ctrlReg1 &= ~(1 << 3); } else { _ctrlReg1 |= (1 << 3); @@ -44,28 +44,28 @@ void L3G4200D::sleep(bool enable) { _writeByte(CTRL_REG1, _ctrlReg1); } -float L3G4200D::readDegPerSecX() { return readX() * _mult; } +float L3G4200D::readRotationDegX() { return readX() * _scale; } -float L3G4200D::readDegPerSecY() { return readY() * _mult; } +float L3G4200D::readRotationDegY() { return readY() * _scale; } -float L3G4200D::readDegPerSecZ() { return readZ() * _mult; } +float L3G4200D::readRotationDegZ() { return readZ() * _scale; } -float L3G4200D::readRadPerSecX() { return readDegPerSecX() * DEG_TO_RAD; } +float L3G4200D::readRotationRadX() { return readRotationDegX() * DEG_TO_RAD; } -float L3G4200D::readRadPerSecY() { return readDegPerSecY() * DEG_TO_RAD; } +float L3G4200D::readRotationRadY() { return readRotationDegY() * DEG_TO_RAD; } -float L3G4200D::readRadPerSecZ() { return readDegPerSecZ() * DEG_TO_RAD; } +float L3G4200D::readRotationRadZ() { return readRotationDegZ() * DEG_TO_RAD; } -void L3G4200D::readDegPerSecXYZ(float& gx, float& gy, float& gz) { +void L3G4200D::readRotationDegXYZ(float& gx, float& gy, float& gz) { int16_t x, y, z; readXYZ(x, y, z); - gx = x * _mult; - gy = y * _mult; - gz = z * _mult; + gx = x * _scale; + gy = y * _scale; + gz = z * _scale; } -void L3G4200D::readRadPerSecXYZ(float& gx, float& gy, float& gz) { - readDegPerSecXYZ(gx, gy, gz); +void L3G4200D::readRotationRadXYZ(float& gx, float& gy, float& gz) { + readRotationDegXYZ(gx, gy, gz); gx *= DEG_TO_RAD; gy *= DEG_TO_RAD; gz *= DEG_TO_RAD; diff --git a/src/Gyroscope.h b/src/Gyroscope.h index 8df82a1..d5c90bf 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -20,18 +20,18 @@ class L3G4200D : public BaseIMU { public: - L3G4200D(uint8_t addr = L3G4200D_ADDRESS); + L3G4200D(uint8_t slaveAddress = L3G4200D_ADDRESS); void begin(TwoWire &wire = Wire); - void sleep(bool enable); + void sleep(bool state); void setRange(uint8_t range); - float readDegPerSecX(); - float readDegPerSecY(); - float readDegPerSecZ(); - float readRadPerSecX(); - float readRadPerSecY(); - float readRadPerSecZ(); - void readDegPerSecXYZ(float& gx, float& gy, float& gz); - void readRadPerSecXYZ(float& gx, float& gy, float& gz); + float readRotationDegX(); + float readRotationDegY(); + float readRotationDegZ(); + float readRotationRadX(); + float readRotationRadY(); + float readRotationRadZ(); + void readRotationDegXYZ(float& gx, float& gy, float& gz); + void readRotationRadXYZ(float& gx, float& gy, float& gz); }; #endif // __GYROSCOPE_H__ diff --git a/src/TroykaIMU.h b/src/TroykaIMU.h index 53ee3a5..fd316b4 100644 --- a/src/TroykaIMU.h +++ b/src/TroykaIMU.h @@ -13,33 +13,33 @@ #define GYRO_ADDRESS_V2 L3G4200D_ADDRESS_V2 // Compass -#define COMPASS_ADDRESS_V1 LIS3MDL_TWI_ADDRESS -#define COMPASS_ADDRESS_V2 LIS3MDL_TWI_ADDRESS_V2 +#define COMPASS_ADDRESS_V1 LIS3MDL_ADDRESS +#define COMPASS_ADDRESS_V2 LIS3MDL_ADDRESS_V2 // Barometer -#define BARO_ADDRESS_V1 LPS331AP_TWI_ADDRESS -#define BARO_ADDRESS_V2 LPS331AP_TWI_ADDRESS_V2 +#define BARO_ADDRESS_V1 LPS331AP_ADDRESS +#define BARO_ADDRESS_V2 LPS331AP_ADDRESS_V2 class Accelerometer : public LIS331DLH { public: - Accelerometer(uint8_t addr = ACCEL_ADDRESS_V1) - : LIS331DLH(addr) { } + Accelerometer(uint8_t slaveAddress = ACCEL_ADDRESS_V1) + : LIS331DLH(slaveAddress) { } }; class Gyroscope : public L3G4200D { public: - Gyroscope(uint8_t addr = GYRO_ADDRESS_V1) - : L3G4200D(addr) { } + Gyroscope(uint8_t slaveAddress = GYRO_ADDRESS_V1) + : L3G4200D(slaveAddress) { } }; class Compass : public LIS3MDL { public: - Compass(uint8_t addr = COMPASS_ADDRESS_V1) - : LIS3MDL(addr) { } + Compass(uint8_t slaveAddress = COMPASS_ADDRESS_V1) + : LIS3MDL(slaveAddress) { } }; class Barometer : public LPS331 { public: - Barometer(uint8_t addr = BARO_ADDRESS_V1) - : LPS331(addr) { } + Barometer(uint8_t slaveAddress = BARO_ADDRESS_V1) + : LPS331(slaveAddress) { } }; From 65e5aad3a392e66254c9074aa1b403b3452b8e18 Mon Sep 17 00:00:00 2001 From: Igor605ds Date: Wed, 7 Oct 2020 05:02:42 +0300 Subject: [PATCH 07/20] fix examples --- .../AccelerometerSimple.ino | 33 +++ .../BarometerSimple/BarometerSimple.ino | 37 ++++ .../CompassCalibration/CompassCalibration.ino | 25 +++ .../Compass/CompassSimple/CompassSimple.ino | 37 ++++ .../GyroscopeSimple/GyroscopeSimple.ino | 33 +++ examples/IMU/IMUSimple/IMUSimple.ino | 71 +++++++ examples/IMU/Madgwick6DOF/Madgwick6DOF.ino | 67 ++++++ examples/IMU/Madgwick9DOF/Madgwick9DOF.ino | 86 ++++++++ .../Madgwick9DOFVisualization.ino | 83 ++++++++ .../Madgwick9DOFVisualizationDraw.pde | 188 +++++++++++++++++ examples/Madgwick6DOF/Madgwick6DOF.ino | 72 ------- examples/Madgwick9DOF/Madgwick9DOF.ino | 91 --------- .../MadgwickProcessing9DOF.ino | 90 --------- .../MadgwickProcessingDraw9DOF.pde | 190 ------------------ examples/accel/accel.ino | 36 ---- examples/barometer/barometer.ino | 41 ---- examples/compass/compass/compass.ino | 40 ---- .../compassCalibrateMatrix.ino | 28 --- examples/gyro/gyro.ino | 38 ---- examples/imu/imu.ino | 77 ------- src/Accelerometer.h | 5 +- src/Compass.h | 6 +- src/Gyroscope.h | 5 +- src/TroykaIMU.h | 29 +-- 24 files changed, 672 insertions(+), 736 deletions(-) create mode 100644 examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino create mode 100644 examples/Barometer/BarometerSimple/BarometerSimple.ino create mode 100644 examples/Compass/CompassCalibration/CompassCalibration.ino create mode 100644 examples/Compass/CompassSimple/CompassSimple.ino create mode 100644 examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino create mode 100644 examples/IMU/IMUSimple/IMUSimple.ino create mode 100644 examples/IMU/Madgwick6DOF/Madgwick6DOF.ino create mode 100644 examples/IMU/Madgwick9DOF/Madgwick9DOF.ino create mode 100644 examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino create mode 100644 examples/IMU/Madgwick9DOFVisualization/Processing/Madgwick9DOFVisualizationDraw/Madgwick9DOFVisualizationDraw.pde delete mode 100755 examples/Madgwick6DOF/Madgwick6DOF.ino delete mode 100755 examples/Madgwick9DOF/Madgwick9DOF.ino delete mode 100755 examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino delete mode 100755 examples/MadgwickProcessing9DOF/MadgwickProcessingDraw9DOF/MadgwickProcessingDraw9DOF.pde delete mode 100755 examples/accel/accel.ino delete mode 100755 examples/barometer/barometer.ino delete mode 100755 examples/compass/compass/compass.ino delete mode 100755 examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino delete mode 100755 examples/gyro/gyro.ino delete mode 100755 examples/imu/imu.ino diff --git a/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino b/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino new file mode 100644 index 0000000..b5197a8 --- /dev/null +++ b/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino @@ -0,0 +1,33 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с акселерометром +Accelerometer accel; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Accelerometer begin"); + // Инициализируем акселерометр + accel.begin(); + // Устанавливаем чувствительность акселерометра + // 2g (по умолчанию), 4g, 8g + accel.setRange(RANGE_2G); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Выводим направления и величины ускорения в м/с² по оси X + Serial.print(accel.readAccelerationAX()); + Serial.print("\t\t"); + // Выводим направления и величины ускорения в м/с² по оси Y + Serial.print(accel.readAccelerationAY()); + Serial.print("\t\t"); + // Выводим направления и величины ускорения в м/с² по оси Z + Serial.print(accel.readAccelerationAZ()); + Serial.print("\t\t"); + Serial.println(""); + delay(100); +} diff --git a/examples/Barometer/BarometerSimple/BarometerSimple.ino b/examples/Barometer/BarometerSimple/BarometerSimple.ino new file mode 100644 index 0000000..360b453 --- /dev/null +++ b/examples/Barometer/BarometerSimple/BarometerSimple.ino @@ -0,0 +1,37 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с акселерометром +Barometer barometer; + +void setup() { + // открываем последовательный порт + Serial.begin(9600); + // выводим сообщение о начале инициализации + Serial.println("Begin init..."); + // инициализация барометра + barometer.begin(); + // выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // создаём переменную и присваиваем ей значения абсолютного давления + float pressure = barometer.readPressureMillibars(); + // создаём переменную и присваиваем ей значения высоты над уровнем море + float altitude = barometer.pressureToAltitudeMeters(pressure); + // создаём переменную и присваиваем ей температуру окружающей среды + float temperature = barometer.readTemperatureC(); + + // Вывод данных в Serial порт + Serial.print("p: "); + Serial.print(pressure); + Serial.print(" mbar \t"); + Serial.print("h: "); + Serial.print(altitude); + Serial.print(" m \t"); + Serial.print("t: "); + Serial.print(temperature); + Serial.println(" C"); + delay(100); +} diff --git a/examples/Compass/CompassCalibration/CompassCalibration.ino b/examples/Compass/CompassCalibration/CompassCalibration.ino new file mode 100644 index 0000000..5fcdbfb --- /dev/null +++ b/examples/Compass/CompassCalibration/CompassCalibration.ino @@ -0,0 +1,25 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с иагнитометром/компасом +Compass compass; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Инициализируем компас + compass.begin(); + // Устанавливаем чувствительность компаса + // ±4 gauss (по умолчанию), ±8 gauss, ±12 gauss, ±16 gauss + compass.setRange(RANGE_4_GAUSS); +} + +void loop() { + // Выводим «сырые» значения компаса по трём осям + Serial.print(compass.readX()); + Serial.print(","); + Serial.print(compass.readY()); + Serial.print(","); + Serial.println(compass.readZ()); + delay(100); +} diff --git a/examples/Compass/CompassSimple/CompassSimple.ino b/examples/Compass/CompassSimple/CompassSimple.ino new file mode 100644 index 0000000..ee52823 --- /dev/null +++ b/examples/Compass/CompassSimple/CompassSimple.ino @@ -0,0 +1,37 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с иагнитометром/компасом +Compass compass; + +// Калибровочные значения, полученные в калибровочной матрице +// из примера compassCalibration +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Compass begin"); + // Инициализируем компас + compass.begin(); + // Устанавливаем чувствительность компаса + // ±4 gauss (по умолчанию), ±8 gauss, ±12 gauss, ±16 gauss + compass.setRange(RANGE_4_GAUSS); + // Устанавливаем калибровочные данные + compass.setCalibrateMatrix(compassCalibrationMatrix, + compassCalibrationBias); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Выводим азимут относительно оси Z + Serial.print(compass.readAzimut()); + Serial.println(" Degrees"); + delay(100); +} diff --git a/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino b/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino new file mode 100644 index 0000000..834f890 --- /dev/null +++ b/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino @@ -0,0 +1,33 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с гироскопом +Gyroscope gyro; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Gyroscope begin"); + // Инициализируем гироскоп + gyro.begin(); + // Устанавливаем чувствительность гироскопа + // 250dps (по умолчанию), 500dps, 2000dps + gyro.setRange(RANGE_250DPS); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Выводим угловую скорость в градусах в секунду относительно оси X + Serial.print(gyro.readRotationDegX()); + Serial.print("\t\t"); + // Выводим угловую скорость в градусах в секунду относительно оси Y + Serial.print(gyro.readRotationDegY()); + Serial.print("\t\t"); + // Выводим угловую скорость в градусах в секунду относительно оси Z + Serial.print(gyro.readRotationDegZ()); + Serial.print("\t\t"); + Serial.println(); + delay(100); +} diff --git a/examples/IMU/IMUSimple/IMUSimple.ino b/examples/IMU/IMUSimple/IMUSimple.ino new file mode 100644 index 0000000..ab7b72f --- /dev/null +++ b/examples/IMU/IMUSimple/IMUSimple.ino @@ -0,0 +1,71 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с гироскопом +Gyroscope gyro; +// Создаём объект для работы с акселерометром +Accelerometer accel; +// Создаём объект для работы с магнитометром/компасом +Compass compass; +// Создаём объект для работы с акселерометром +Barometer barometer; + +// Калибровочные значения, полученные в калибровочной матрице +// из примера compassCalibration +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("IMU Begin"); + // Инициализируем гироскоп + gyro.begin(); + // Инициализируем акселерометр + accel.begin(); + // Инициализируем компас + compass.begin(); + // Инициализируем барометр + barometer.begin(); + // Устанавливаем калибровочные данные + compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); + Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer"); +} + +void loop() { + // Выводим угловую скорость в градусах в секунду относительно оси X + Serial.print(gyro.readRotationDegX()); + Serial.print("\t"); + // Выводим угловую скорость в градусах в секунду относительно оси Y + Serial.print(gyro.readRotationDegY()); + Serial.print("\t"); + // Выводим угловую скорость в градусах в секунду относительно оси Z + Serial.print(gyro.readRotationDegZ()); + Serial.print("\t\t"); + // Выводим направления и величины ускорения в м/с² по оси X + Serial.print(accel.readAccelerationAX()); + Serial.print("\t"); + // Выводим направления и величины ускорения в м/с² по оси Y + Serial.print(accel.readAccelerationAY()); + Serial.print("\t"); + // Выводим направления и величины ускорения в м/с² по оси Z + Serial.print(accel.readAccelerationAZ()); + Serial.print("\t\t"); + // Выводим азимут относительно оси Z + Serial.print(compass.readAzimut()); + Serial.print(" Degrees\t"); + // Выводим значения абсолютного давления + Serial.print(barometer.readPressureMillibars()); + Serial.print("\t"); + // Выодим значения температуры окружающей среды + Serial.print(barometer.readTemperatureC()); + Serial.print("\t"); + Serial.println(""); + delay(100); +} diff --git a/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino new file mode 100644 index 0000000..03cd2f1 --- /dev/null +++ b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino @@ -0,0 +1,67 @@ +// Библиотека для работы с модулями IMU +#include + +// Множитель фильтра +#define BETA 0.22f + +// Создаём объект для фильтра Madgwick +Madgwick filter; +// Создаём объект для работы с гироскопом +Gyroscope gyro; +// Создаём объект для работы с акселерометром +Accelerometer accel; + +// Переменные для данных с гироска и акселерометра +float gx, gy, gz, ax, ay, az; + +// Переменные для хранения самолётных углов ориентации +float yaw, pitch, roll; + +// Переменная для хранения частоты выборок фильтра +float fps = 100; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("IMU Begin"); + // Инициализируем гироскоп + gyro.begin(); + // Инициализируем акселерометр + accel.begin(); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Запоминаем текущее время + unsigned long startMillis = millis(); + // Считываем данные с акселерометра в единицах G + accel.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с акселерометра в радианах в секунду + gyro.readRotationRadXYZ(gx, gy, gz); + // Устанавливаем коэффициенты фильтра + filter.setKoeff(fps, BETA); + // Обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az); + + // Получаем из фильтра углы: yaw, pitch и roll + yaw = filter.getYawDeg(); + pitch = filter.getPitchDeg(); + roll = filter.getRollDeg(); + + // Выводим полученные углы Эйлера в serial-порт + Serial.print("yaw: "); + Serial.print(yaw); + Serial.print("\t\t"); + Serial.print("pitch: "); + Serial.print(pitch); + Serial.print("\t\t"); + Serial.print("roll: "); + Serial.println(roll); + + // Вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // Вычисляем частоту обработки фильтра + fps = 1000 / deltaMillis; +} diff --git a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino new file mode 100644 index 0000000..44369da --- /dev/null +++ b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino @@ -0,0 +1,86 @@ +// Библиотека для работы с модулями IMU +#include + +// Множитель фильтра +#define BETA 0.22f + +// Создаём объект для фильтра Madgwick +Madgwick filter; +// Создаём объект для работы с гироскопом +Gyroscope gyro; +// Создаём объект для работы с акселерометром +Accelerometer accel; +// Создаём объект для работы с компасом +Compass compass; + +// Переменные для данных с гироска, акселерометра и компаса +float gx, gy, gz, ax, ay, az, mx, my, mz; + +// Переменные для хранения самолётных углов ориентации +float yaw, pitch, roll; + +// Переменная для хранения частоты выборок фильтра +float fps = 100; + +// Калибровочные значения, полученные в калибровочной матрице +// из примера compassCalibration +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("IMU Begin"); + // Инициализируем гироскоп + gyro.begin(); + // Инициализируем акселерометр + accel.begin(); + // Инициализируем компас + compass.begin(); + // Устанавливаем калибровочные данные + compass.setCalibrateMatrix(compassCalibrationMatrix, + compassCalibrationBias); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Запоминаем текущее время + unsigned long startMillis = millis(); + + // Считываем данные с акселерометра в единицах G + accel.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с гироскопа в радианах в секунду + gyro.readRotationRadXYZ(gx, gy, gz); + // Считываем данные с компаса в Гауссах + compass.readCalibrateMagneticGaussXYZ(mx, my, mz); + + // Устанавливаем коэффициенты фильтра + filter.setKoeff(fps, BETA); + // Обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); + + // Получаем из фильтра углы: yaw, pitch и roll + yaw = filter.getYawDeg(); + pitch = filter.getPitchDeg(); + roll = filter.getRollDeg(); + + // Выводим полученные углы Эйлера в serial-порт + Serial.print("yaw: "); + Serial.print(yaw); + Serial.print("\t\t"); + Serial.print("pitch: "); + Serial.print(pitch); + Serial.print("\t\t"); + Serial.print("roll: "); + Serial.println(roll); + + // Вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // Вычисляем частоту обработки фильтра + fps = 1000 / deltaMillis; +} diff --git a/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino b/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino new file mode 100644 index 0000000..714add5 --- /dev/null +++ b/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino @@ -0,0 +1,83 @@ +// Библиотека для работы с модулями IMU +#include + +// Множитель фильтра +#define BETA 0.22f + +// Создаём объект для фильтра Madgwick +Madgwick filter; +// Создаём объект для работы с гироскопом +Gyroscope gyro; +// Создаём объект для работы с акселерометром +Accelerometer accel; +// Создаём объект для работы с компасом +Compass compass; + +// Переменные для данных с гироска, акселерометра и компаса +float gx, gy, gz, ax, ay, az, mx, my, mz; + +// Переменные для хранения самолётных углов ориентации +float yaw, pitch, roll; + +// Переменная для хранения частоты выборок фильтра +float fps = 100; + +// Калибровочные значения, полученные в калибровочной матрице +// из примера compassCalibration +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Инициализируем гироскоп + gyro.begin(); + // Инициализируем акселерометр + accel.begin(); + // Инициализируем компас + compass.begin(); + // Устанавливаем калибровочные данные + compass.setCalibrateMatrix(compassCalibrationMatrix, + compassCalibrationBias); +} + +void loop() { + // Запоминаем текущее время + unsigned long startMillis = millis(); + + // Считываем данные с акселерометра в единицах G + accel.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с гироскопа в радианах в секунду + gyro.readRotationRadXYZ(gx, gy, gz); + // Считываем данные с компаса в Гауссах + compass.readCalibrateMagneticGaussXYZ(mx, my, mz); + + // Устанавливаем коэффициенты фильтра + filter.setKoeff(fps, BETA); + // Обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); + + if (Serial.available() > 0) { + int val = Serial.read(); + // Если пришёл символ 's' + if (val == 's') { + float q0, q1, q2, q3; + filter.readQuaternions(&q0, &q1, &q2, &q3); + // Выводим кватернионы в serial-порт + Serial.print(q0); + Serial.print(","); + Serial.print(q1); + Serial.print(","); + Serial.print(q2); + Serial.print(","); + Serial.println(q3); + } + } + // Вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // Вычисляем частоту обработки фильтра + fps = 1000 / deltaMillis; +} diff --git a/examples/IMU/Madgwick9DOFVisualization/Processing/Madgwick9DOFVisualizationDraw/Madgwick9DOFVisualizationDraw.pde b/examples/IMU/Madgwick9DOFVisualization/Processing/Madgwick9DOFVisualizationDraw/Madgwick9DOFVisualizationDraw.pde new file mode 100644 index 0000000..d89dcb1 --- /dev/null +++ b/examples/IMU/Madgwick9DOFVisualization/Processing/Madgwick9DOFVisualizationDraw/Madgwick9DOFVisualizationDraw.pde @@ -0,0 +1,188 @@ +import processing.serial.*; +import toxi.geom.*; +import toxi.processing.*; + +// NOTE: requires ToxicLibs to be installed in order to run properly. +// 1. Download from http://toxiclibs.org/downloads +// 2. Extract into [userdir]/Processing/libraries +// (location may be different on Mac/Linux) +// 3. Run and bask in awesomeness + +// The serial port +Serial port; + +String message; + +float[] q = new float[4]; +Quaternion quat = new Quaternion(1, 0, 0, 0); +// New line character in ASCII +int newLine = 13; +String [] massQ = new String [4]; +float[] ypr = new float[3]; + +void setup() { + // Size form 300x300 + size(400, 400, P3D); + // Open serial port + // Replace "COM7" with the COM port on which your arduino is connected + port = new Serial(this, "COM7", 9600); +} + +void draw() { + // Read and parse incoming serial message + serialEvent(); + // Set background to black + background(0); + printQuaternions(); + printYawPitchRoll(); + // Set position to centre + translate(width / 2, height / 2); + // Begin object + pushMatrix(); + float[] axis = quat.toAxisAngle(); + rotate(axis[0], axis[2], axis[3], axis[1]); + // Draw main body in red + drawBody(); + // Draw front-facing tip in blue + drawCylinder(); + // Draw Triangles + drawTriangles(); + // Draw Quards + drawQuards(); + // End of object + popMatrix(); + // Send character 's' to Arduino + port.write('s'); +} + +void serialEvent() { + // Read from port until new line (ASCII code 13) + message = port.readStringUntil(newLine); + if (message != null) { + // Split message by commas and store in String array + massQ = split(message, ","); + q[0] = float(massQ[0]); + q[1] = float(massQ[1]); + q[2] = float(massQ[2]); + q[3] = float(massQ[3]); + } + // Print values to console + print(q[0]); + print("\t"); + print(q[1]); + print("\t"); + print(q[2]); + print("\t"); + print(q[3]); + println("\t"); + // Set our toxilibs quaternion to new data + quat.set(q[0], q[1], q[2], q[3]); + +} + +void drawCylinder() { + float topRadius = 0; + float bottomRadius = 20; + float tall = 20; + int sides = 8; + // Begin object + pushMatrix(); + translate(0, 0, -120); + rotateX(PI/2); + fill(0, 0, 255, 200); + + float angle = 0; + float angleIncrement = TWO_PI / sides; + beginShape(QUAD_STRIP); + for (int i = 0; i < sides + 1; ++i) { + vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); + vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); + angle += angleIncrement; + } + + endShape(); + + // if it is not a cone, draw the circular top cap + if (topRadius != 0) { + angle = 0; + beginShape(TRIANGLE_FAN); + // Center point + vertex(0, 0, 0); + for (int i = 0; i < sides + 1; i++) { + vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); + angle += angleIncrement; + } + endShape(); + } + + // If it is not a cone, draw the circular bottom cap + if (bottomRadius != 0) { + angle = 0; + beginShape(TRIANGLE_FAN); + // Center point + vertex(0, tall, 0); + for (int i = 0; i < sides + 1; i++) { + vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); + angle += angleIncrement; + } + endShape(); + } + popMatrix(); +} + +void drawBody() { + fill(255, 0, 0, 200); + box(10, 10, 200); +} + +void drawTriangles() { + // Draw wings and tail fin in green + fill(0, 255, 0, 200); + beginShape(TRIANGLES); + // Wing top layer + vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); + // Wing bottom layer + vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); + // Tail left layer + vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); + // Tail right layer + vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); + endShape(); +} + +void drawQuards() { + beginShape(QUADS); + vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); + vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); + vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); + vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); + vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); + vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); + endShape(); +} + +void printQuaternions() { + // Set text mode to shape + textMode(SHAPE); + textSize(13); + fill(255, 255, 255); + text("Quaternions:", 20, 20, 10); + text(q[0], 20, 40, 10); + text(q[1], 20, 60, 10); + text(q[2], 20, 80, 10); + text(q[3], 20, 100, 10); +} + +void printYawPitchRoll() { + // Calculate yaw/pitch/roll angles + ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1) * 57.2; + ypr[1] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 57.2; + ypr[2] = -atan2(2.0f * (q[0] * q[2] - q[1] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[1] *q[1] )) * 57.2; + + text("Yaw:", 150, 20, 10); + text(ypr[0], 150, 40, 10); + text("Pitch:", 220, 20, 10); + text(ypr[1], 220, 40, 10); + text("Roll:", 290, 20, 10); + text(ypr[2], 290, 40, 10); +} \ No newline at end of file diff --git a/examples/Madgwick6DOF/Madgwick6DOF.ino b/examples/Madgwick6DOF/Madgwick6DOF.ino deleted file mode 100755 index 46b061c..0000000 --- a/examples/Madgwick6DOF/Madgwick6DOF.ino +++ /dev/null @@ -1,72 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// множитель фильтра -#define BETA 0.22f - -// создаём объект для фильтра Madgwick -Madgwick filter; - -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); - -// если напаяны перемычки, устройства доступны по новым адресам -// Accelerometer accel(ACCEL_ADDRESS_V2); -// Gyroscope gyro(GYRO_ADDRESS_V2); - -// переменные для данных с гироскопов, акселерометров -float gx, gy, gz, ax, ay, az; - -// получаемые углы ориентации -float yaw, pitch, roll; - -// переменная для хранения частоты выборок фильтра -float fps = 100; - -void setup() { - // открываем последовательный порт - Serial.begin(9600); - Serial.println("Begin init..."); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() { - // запоминаем текущее время - unsigned long startMillis = millis(); - - // считываем данные с акселерометра в единицах G - accel.readAccelerationGXYZ(ax, ay, az); - // считываем данные с акселерометра в радианах в секунду - gyro.readRotationRadXYZ(gx, gy, gz); - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az); - - // получение углов yaw, pitch и roll из фильтра - yaw = filter.getYawDeg(); - pitch = filter.getPitchDeg(); - roll = filter.getRollDeg(); - - // выводим полученные углы в serial-порт - Serial.print("yaw: "); - Serial.print(yaw); - Serial.print("\t\t"); - Serial.print("pitch: "); - Serial.print(pitch); - Serial.print("\t\t"); - Serial.print("roll: "); - Serial.println(roll); - - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; -} diff --git a/examples/Madgwick9DOF/Madgwick9DOF.ino b/examples/Madgwick9DOF/Madgwick9DOF.ino deleted file mode 100755 index 3c16592..0000000 --- a/examples/Madgwick9DOF/Madgwick9DOF.ino +++ /dev/null @@ -1,91 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// множитель фильтра -#define BETA 0.22 - -// создаём объект для фильтра Madgwick -Madgwick filter; - -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); - -// если напаяны перемычки, устройства доступны по новым адресам -// Accelerometer accel(ACCEL_ADDRESS_V2); -// Gyroscope gyro(GYRO_ADDRESS_V2); -// Compass compass(COMPASS_ADDRESS_V2); - -// переменные для данных с гироскопа, акселерометра и компаса -float gx, gy, gz, ax, ay, az, mx, my, mz; - -// получаемые углы ориентации (Эйлера) -float yaw, pitch, roll; - -// переменная для хранения частоты выборок фильтра -float fps = 100; - -// калибровочные значения компаса -// полученные в калибровочной матрице из примера «compassCalibrateMatrixx» -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; - -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; -void setup() { - // открываем последовательный порт - Serial.begin(9600); - Serial.println("Begin init..."); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // инициализация компаса - compass.begin(); - - // калибровка компаса - compass.setCalibrateMatrix(compassCalibrationMatrix, - compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() { - // запоминаем текущее время - unsigned long startMillis = millis(); - - // считываем данные с акселерометра в единицах G - accel.readAccelerationGXYZ(ax, ay, az); - // считываем данные с гироскопа в радианах в секунду - gyro.readRotationRadXYZ(gx, gy, gz); - // считываем данные с компаса в Гауссах - compass.readCalibrateMagneticGaussXYZ(mx, my, mz); - - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); - - // получение углов yaw, pitch и roll из фильтра - yaw = filter.getYawDeg(); - pitch = filter.getPitchDeg(); - roll = filter.getRollDeg(); - - // выводим полученные углы в serial-порт - Serial.print("yaw: "); - Serial.print(yaw); - Serial.print("\t\t"); - Serial.print("pitch: "); - Serial.print(pitch); - Serial.print("\t\t"); - Serial.print("roll: "); - Serial.println(roll); - - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; -} diff --git a/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino b/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino deleted file mode 100755 index bd668af..0000000 --- a/examples/MadgwickProcessing9DOF/MadgwickProcessing9DOF.ino +++ /dev/null @@ -1,90 +0,0 @@ -// скетч для вывода кватернионов в serial-порт -// для дальнейшего графического просмотра ориентации объекта -// в среде processing «MadgwickProcessingDraw9DOF.pde» - -// библиотека для работы с модулями IMU -#include - -// множитель фильтра -#define BETA 0.22 - -// создаём объект для фильтра Madgwick -Madgwick filter; - -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); - -// если напаяны перемычки, устройства доступны по новым адресам -// Accelerometer accel(ACCEL_ADDRESS_V2); -// Gyroscope gyro(GYRO_ADDRESS_V2); -// Compass compass(COMPASS_ADDRESS_V2); - -// переменные для данных с гироскопа, акселерометра и компаса -float gx, gy, gz, ax, ay, az, mx, my, mz; - -// получаемые углы ориентации (Эйлера) -float yaw, pitch, roll; - -// переменная для хранения частоты выборок фильтра -float fps = 100; - -// калибровочные значения компаса -// полученные в калибровочной матрице из примера «compassCalibrateMatrixx» -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; - -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; - -void setup() { - // открываем последовательный порт - Serial.begin(9600); - // инициализация акселерометра - accel.begin(); - // инициализация гироскопа - gyro.begin(); - // инициализация компаса - compass.begin(); - // калибровка компаса - compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); -} - -void loop() { - // запоминаем текущее время - unsigned long startMillis = millis(); - // считываем данные с акселерометра в единицах G - accel.readAccelerationGXYZ(ax, ay, az); - // считываем данные с гироскопа в радианах в секунду - gyro.readRotationRadXYZ(gx, gy, gz); - // считываем данные с компаса в Гауссах - compass.readCalibrateMagneticGaussXYZ(mx, my, mz); - // устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); - // обновляем входные данные в фильтр - filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); - - if (Serial.available() > 0) { - int val = Serial.read(); - // если пришёл символ 's' - if (val == 's') { - float q0, q1, q2, q3; - filter.readQuaternions(&q0, &q1, &q2, &q3); - // выводим кватернионы в serial-порт - Serial.print(q0); - Serial.print(","); - Serial.print(q1); - Serial.print(","); - Serial.print(q2); - Serial.print(","); - Serial.println(q3); - } - } - // вычисляем затраченное время на обработку данных - unsigned long deltaMillis = millis() - startMillis; - // вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; -} diff --git a/examples/MadgwickProcessing9DOF/MadgwickProcessingDraw9DOF/MadgwickProcessingDraw9DOF.pde b/examples/MadgwickProcessing9DOF/MadgwickProcessingDraw9DOF/MadgwickProcessingDraw9DOF.pde deleted file mode 100755 index ada68a7..0000000 --- a/examples/MadgwickProcessing9DOF/MadgwickProcessingDraw9DOF/MadgwickProcessingDraw9DOF.pde +++ /dev/null @@ -1,190 +0,0 @@ -import processing.serial.*; -import toxi.geom.*; -import toxi.processing.*; - -// NOTE: requires ToxicLibs to be installed in order to run properly. -// 1. Download from http://toxiclibs.org/downloads -// 2. Extract into [userdir]/Processing/libraries -// (location may be different on Mac/Linux) -// 3. Run and bask in awesomeness - -// The serial port -Serial port; - -String message; - -float[] q = new float[4]; -Quaternion quat = new Quaternion(1, 0, 0, 0); -// new line character in ASCII -int newLine = 13; -String [] massQ = new String [4]; -float[] ypr = new float[3]; - -void setup() -{ - // size form 300x300 - size(400, 400, P3D); - // open serial port - // replace "COM29" with the COM port on which your arduino is connected - port = new Serial(this, "COM61", 115200); -} - -void draw() -{ - // read and parse incoming serial message - serialEvent(); - // set background to black - background(0); - printQuaternions(); - printYawPitchRoll(); - // set position to centre - translate(width / 2, height / 2); - // begin object - pushMatrix(); - float[] axis = quat.toAxisAngle(); - rotate(axis[0], axis[2], axis[3], axis[1]); - // draw main body in red - drawBody(); - // draw front-facing tip in blue - drawCylinder(); - // draw Triangles - drawTriangles(); - // draw Quards - drawQuards(); - // end of object - popMatrix(); - // send 's' to Arduino - port.write('s'); -} - -void serialEvent() { - // read from port until new line (ASCII code 13) - message = port.readStringUntil(newLine); - if (message != null) { - // split message by commas and store in String array - massQ = split(message, ","); - q[0] = float(massQ[0]); - q[1] = float(massQ[1]); - q[2] = float(massQ[2]); - q[3] = float(massQ[3]); - } - // print values to console - print(q[0]); - print("\t"); - print(q[1]); - print("\t"); - print(q[2]); - print("\t"); - print(q[3]); - println("\t"); - // set our toxilibs quaternion to new data - quat.set(q[0], q[1], q[2], q[3]); - -} - -void drawCylinder() { - float topRadius = 0; - float bottomRadius = 20; - float tall = 20; - int sides = 8; - // begin object - pushMatrix(); - translate(0, 0, -120); - rotateX(PI/2); - fill(0, 0, 255, 200); - - float angle = 0; - float angleIncrement = TWO_PI / sides; - beginShape(QUAD_STRIP); - for (int i = 0; i < sides + 1; ++i) { - vertex(topRadius*cos(angle), 0, topRadius*sin(angle)); - vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle)); - angle += angleIncrement; - } - endShape(); - - // if it is not a cone, draw the circular top cap - if (topRadius != 0) { - angle = 0; - beginShape(TRIANGLE_FAN); - - // center point - vertex(0, 0, 0); - for (int i = 0; i < sides + 1; i++) { - vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); - angle += angleIncrement; - } - endShape(); - } - - // if it is not a cone, draw the circular bottom cap - if (bottomRadius != 0) { - angle = 0; - beginShape(TRIANGLE_FAN); - - // center point - vertex(0, tall, 0); - for (int i = 0; i < sides + 1; i++) { - vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); - angle += angleIncrement; - } - endShape(); - } - popMatrix(); -} - -void drawBody() { - fill(255, 0, 0, 200); - box(10, 10, 200); -} - -void drawTriangles() { - // draw wings and tail fin in green - fill(0, 255, 0, 200); - beginShape(TRIANGLES); - // wing top layer - vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); - // wing bottom layer - vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); - // tail left layer - vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); - // tail right layer - vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); - endShape(); -} - -void drawQuards() { - beginShape(QUADS); - vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); - vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); - vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); - vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); - vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); - vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); - endShape(); -} - -void printQuaternions() { - textMode(SHAPE); // set text mode to shape - textSize(13); - fill(255, 255, 255); - text("Quaternions:", 20, 20, 10); - text(q[0], 20, 40, 10); - text(q[1], 20, 60, 10); - text(q[2], 20, 80, 10); - text(q[3], 20, 100, 10); -} - -void printYawPitchRoll() { - // calculate yaw/pitch/roll angles - ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1) * 57.2; - ypr[1] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1) * 57.2; - ypr[2] = -atan2(2.0f * (q[0] * q[2] - q[1] * q[3]), 1.0f - 2.0f * (q[2] * q[2] + q[1] *q[1] )) * 57.2; - - text("Yaw:", 150, 20, 10); - text(ypr[0], 150, 40, 10); - text("Pitch:", 220, 20, 10); - text(ypr[1], 220, 40, 10); - text("Roll:", 290, 20, 10); - text(ypr[2], 290, 40, 10); -} \ No newline at end of file diff --git a/examples/accel/accel.ino b/examples/accel/accel.ino deleted file mode 100755 index f4b74f9..0000000 --- a/examples/accel/accel.ino +++ /dev/null @@ -1,36 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); - -// если напаяна перемычка, устройство доступно по новому адресу -// Accelerometer accel(ACCEL_ADDRESS_V2); - -void setup() { - // открываем последовательный порт - Serial.begin(9600); - // выводим сообщение о начале инициализации - Serial.println("Accelerometer init..."); - // инициализация акселерометра - accel.begin(); - // устанавливаем чувствительность акселерометра - // 2g — по умолчанию, 4g, 8g - accel.setRange(RANGE_2G); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() { - // вывод направления и величины ускорения в м/с² по оси X - Serial.print(accel.readAccelerationAX()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси Y - Serial.print(accel.readAccelerationAY()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси Z - Serial.print(accel.readAccelerationAZ()); - Serial.print("\t\t"); - Serial.println(""); - delay(100); -} diff --git a/examples/barometer/barometer.ino b/examples/barometer/barometer.ino deleted file mode 100755 index c9244e5..0000000 --- a/examples/barometer/barometer.ino +++ /dev/null @@ -1,41 +0,0 @@ -// библиотека для работы с модулями IMU -#include -// создаём объект для работы с барометром -Barometer barometer(BARO_ADDRESS_V1); - -// если напаяны перемычки, устройство доступно по новому дресу -// Barometer barometer(BARO_ADDRESS_V2); - -void setup() -{ - // открываем последовательный порт - Serial.begin(9600); - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация барометра - barometer.begin(); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() -{ - // создаём переменную и присваиваем ей значения абсолютного давления - float pressure = barometer.readPressureMillibars(); - // создаём переменную и присваиваем ей значения высоты над уровнем море - float altitude = barometer.pressureToAltitudeMeters(pressure); - // создаём переменную и присваиваем ей температуру окружающей среды - float temperature = barometer.readTemperatureC(); - - // Вывод данных в Serial порт - Serial.print("p: "); - Serial.print(pressure); - Serial.print(" mbar \t"); - Serial.print("h: "); - Serial.print(altitude); - Serial.print(" m \t"); - Serial.print("t: "); - Serial.print(temperature); - Serial.println(" C"); - delay(100); -} diff --git a/examples/compass/compass/compass.ino b/examples/compass/compass/compass.ino deleted file mode 100755 index 3e72dd3..0000000 --- a/examples/compass/compass/compass.ino +++ /dev/null @@ -1,40 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); - -// если напаяна перемычка, устройство доступно по новому адресу -// Compass compass(COMPASS_ADDRESS_V2); - -// калибровочные значения, полученные в калибровочной матрице -// из примера compass_cal_matrix -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; - -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; - -void setup() { - // открываем последовательный порт - Serial.begin(9600); - // пока не появились данные с USB - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация компаса - compass.begin(); - // устанавливаем чувствительность компаса - // ±4 gauss — по умолчанию, ±8 gauss, ±12 gauss, ±16 gauss - compass.setRange(RANGE_4_GAUSS); - // калибровка компаса - compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() { - // выводим азимут относительно оси Z - Serial.print(compass.readAzimut()); - Serial.println(" Degrees"); - delay(100); -} diff --git a/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino b/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino deleted file mode 100755 index 9b08906..0000000 --- a/examples/compass/compassCalibrateMatrix/compassCalibrateMatrix.ino +++ /dev/null @@ -1,28 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); - -// если напаяна перемычка, устройство доступно по новому адресу -// Compass compass(COMPASS_ADDRESS_V2); - -void setup() { - // открываем последовательный порт - Serial.begin(9600); - // инициализация компаса - compass.begin(); - // устанавливаем чувствительность компаса - // ±4 gauss — по умолчанию, ±8 gauss, ±12 gauss, ±16 gauss - compass.setRange(RANGE_4_GAUSS); -} - -void loop() { - // Выводим «сырые» значения компаса по трём осям - Serial.print(compass.readX()); - Serial.print(","); - Serial.print(compass.readY()); - Serial.print(","); - Serial.println(compass.readZ()); - delay(100); -} diff --git a/examples/gyro/gyro.ino b/examples/gyro/gyro.ino deleted file mode 100755 index d8078cf..0000000 --- a/examples/gyro/gyro.ino +++ /dev/null @@ -1,38 +0,0 @@ -// библиотека для работы I²C -#include -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); - -// если напаяна перемычка, устройство доступно по новому адресу -// Gyroscope gyro(GYRO_ADDRESS_V2); - -void setup() { - // открываем последовательный порт - Serial.begin(9600); - // выводим сообщение о начале инициализации - Serial.println("Gyroscope init..."); - // инициализация гироскопа - gyro.begin(); - // устанавливаем чувствительность гироскопа - // 250dps — по умолчанию, 500dps, 2000dps - gyro.setRange(RANGE_250DPS); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); -} - -void loop() { - // вывод угловой скорости в градусах в секунду относительно оси X - Serial.print(gyro.readRotationDegX()); - Serial.print("\t\t"); - // вывод угловой скорости в градусах в секунду относительно оси Y - Serial.print(gyro.readRotationDegY()); - Serial.print("\t\t"); - // вывод угловой скорости в градусах в секунду относительно оси Z - Serial.print(gyro.readRotationDegZ()); - Serial.print("\t\t"); - Serial.println(""); - delay(100); -} diff --git a/examples/imu/imu.ino b/examples/imu/imu.ino deleted file mode 100755 index 38e998f..0000000 --- a/examples/imu/imu.ino +++ /dev/null @@ -1,77 +0,0 @@ -// библиотека для работы с модулями IMU -#include - -// создаём объект для работы с гироскопом -Gyroscope gyro(GYRO_ADDRESS_V1); -// создаём объект для работы с акселерометром -Accelerometer accel(ACCEL_ADDRESS_V1); -// создаём объект для работы с компасом -Compass compass(COMPASS_ADDRESS_V1); -// создаём объект для работы с барометром -Barometer barometer(BARO_ADDRESS_V1); - -// если напаяны перемычки, устройства доступны по новому адресу -// Gyroscope gyro(GYRO_ADDRESS_V2); -// Accelerometer accel(ACCEL_ADDRESS_V2); -// Compass compass(COMPASS_ADDRESS_V2); -// Barometer barometer(BARO_ADDRESS_V2); - -// калибровочные значения компаса -// полученные в калибровочной матрице из примера «compassCalibrateMatrix» -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; - -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; - -void setup() { - // открываем последовательный порт - Serial.begin(9600); - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация гироскопа - gyro.begin(); - // инициализация акселерометра - accel.begin(); - // инициализация компаса - compass.begin(); - // инициализация барометра - barometer.begin(); - // калибровка компаса - compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); - // выводим сообщение об удачной инициализации - Serial.println("Initialization completed"); - Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer"); -} - -void loop() { - // вывод угловой скорости в градусах в секунду относительно оси X - Serial.print(gyro.readRotationDegX()); - Serial.print("\t"); - // вывод угловой скорости в градусах в секунду относительно оси Y - Serial.print(gyro.readRotationDegY()); - Serial.print("\t"); - // вывод угловой скорости в градусах в секунду относительно оси Z - Serial.print(gyro.readRotationDegZ()); - Serial.print("\t\t"); - // вывод направления и величины ускорения в м/с² по оси X - Serial.print(accel.readAccelerationAX()); - Serial.print("\t"); - // вывод направления и величины ускорения в м/с² по оси Y - Serial.print(accel.readAccelerationAY()); - Serial.print("\t"); - // вывод направления и величины ускорения в м/с² по оси Z - Serial.print(accel.readAccelerationAZ()); - Serial.print("\t\t"); - // выводим азимут относительно оси Z - Serial.print(compass.readAzimut()); - Serial.print(" Degrees\t"); - // вывод значения абсолютного давления - Serial.print(barometer.readPressureMillibars()); - Serial.print("\t"); - // вывод значения температуры окружающей среды - Serial.print(barometer.readTemperatureC()); - Serial.print("\t"); - Serial.println(""); - delay(100); -} diff --git a/src/Accelerometer.h b/src/Accelerometer.h index 2f3f434..e92a6c3 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -3,9 +3,6 @@ #include "BaseIMU.h" -#define LIS331DLH_ADDRESS 0b0011000 -#define LIS331DLH_ADDRESS_V2 0b0011001 - #define RANGE_2G 2 #define RANGE_4G 4 #define RANGE_8G 8 @@ -18,7 +15,7 @@ class LIS331DLH : public BaseIMU { public: - LIS331DLH(uint8_t slaveAddress = LIS331DLH_ADDRESS); + LIS331DLH(uint8_t slaveAddress); void begin(TwoWire &wire = Wire); void sleep(bool state); void setRange(uint8_t range); diff --git a/src/Compass.h b/src/Compass.h index fbd890d..e17552c 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -3,9 +3,6 @@ #include "BaseIMU.h" -#define LIS3MDL_ADDRESS 0b0011100 -#define LIS3MDL_ADDRESS_V2 0b0011110 - #define RANGE_4_GAUSS 0 #define RANGE_8_GAUSS 1 #define RANGE_12_GAUSS 2 @@ -23,7 +20,7 @@ class LIS3MDL : public BaseIMU { public: - LIS3MDL(uint8_t slaveAddress = LIS3MDL_ADDRESS); + LIS3MDL(uint8_t slaveAddress); void begin(TwoWire& wire = Wire); void sleep(bool state); void setRange(uint8_t range); @@ -36,7 +33,6 @@ class LIS3MDL : public BaseIMU { const float bias[3]); float readAzimut(); - private: void _calibrate(float& x, float& y, float& z); float _calibrationMatrix[3][3]; diff --git a/src/Gyroscope.h b/src/Gyroscope.h index d5c90bf..102bdcb 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -3,9 +3,6 @@ #include "BaseIMU.h" -#define L3G4200D_ADDRESS 0b01101000 -#define L3G4200D_ADDRESS_V2 0b01101001 - #define RANGE_250DPS 0 #define RANGE_500DPS 1 #define RANGE_2000DPS 2 @@ -20,7 +17,7 @@ class L3G4200D : public BaseIMU { public: - L3G4200D(uint8_t slaveAddress = L3G4200D_ADDRESS); + L3G4200D(uint8_t slaveAddress); void begin(TwoWire &wire = Wire); void sleep(bool state); void setRange(uint8_t range); diff --git a/src/TroykaIMU.h b/src/TroykaIMU.h index fd316b4..546b1db 100644 --- a/src/TroykaIMU.h +++ b/src/TroykaIMU.h @@ -1,45 +1,34 @@ +#ifndef __TROYKA_IMU_H__ +#define __TROYKA_IMU_H__ + #include "Accelerometer.h" #include "Barometer.h" #include "Compass.h" #include "Gyroscope.h" #include "MadgwickAHRS.h" -// Accelerometer -#define ACCEL_ADDRESS_V1 LIS331DLH_ADDRESS -#define ACCEL_ADDRESS_V2 LIS331DLH_ADDRESS_V2 - -// Gyroscope -#define GYRO_ADDRESS_V1 L3G4200D_ADDRESS -#define GYRO_ADDRESS_V2 L3G4200D_ADDRESS_V2 - -// Compass -#define COMPASS_ADDRESS_V1 LIS3MDL_ADDRESS -#define COMPASS_ADDRESS_V2 LIS3MDL_ADDRESS_V2 - -// Barometer -#define BARO_ADDRESS_V1 LPS331AP_ADDRESS -#define BARO_ADDRESS_V2 LPS331AP_ADDRESS_V2 - class Accelerometer : public LIS331DLH { public: - Accelerometer(uint8_t slaveAddress = ACCEL_ADDRESS_V1) + Accelerometer(uint8_t slaveAddress = 0x18) : LIS331DLH(slaveAddress) { } }; class Gyroscope : public L3G4200D { public: - Gyroscope(uint8_t slaveAddress = GYRO_ADDRESS_V1) + Gyroscope(uint8_t slaveAddress = 0x68) : L3G4200D(slaveAddress) { } }; class Compass : public LIS3MDL { public: - Compass(uint8_t slaveAddress = COMPASS_ADDRESS_V1) + Compass(uint8_t slaveAddress = 0x1C) : LIS3MDL(slaveAddress) { } }; class Barometer : public LPS331 { public: - Barometer(uint8_t slaveAddress = BARO_ADDRESS_V1) + Barometer(uint8_t slaveAddress = 0x5C) : LPS331(slaveAddress) { } }; + +#endif // __TROYKA_IMU_H__ \ No newline at end of file From 4198157f0a716cc797dafff367b99083f4bdd189 Mon Sep 17 00:00:00 2001 From: Igor605ds Date: Thu, 8 Oct 2020 01:48:05 +0300 Subject: [PATCH 08/20] fix barometer files --- .../BarometerSimple/BarometerSimple.ino | 36 ++-- src/Barometer.cpp | 155 +++--------------- src/Barometer.h | 64 ++------ 3 files changed, 59 insertions(+), 196 deletions(-) diff --git a/examples/Barometer/BarometerSimple/BarometerSimple.ino b/examples/Barometer/BarometerSimple/BarometerSimple.ino index 360b453..2fe731d 100644 --- a/examples/Barometer/BarometerSimple/BarometerSimple.ino +++ b/examples/Barometer/BarometerSimple/BarometerSimple.ino @@ -5,32 +5,36 @@ Barometer barometer; void setup() { - // открываем последовательный порт + // Окрываем Serial-порт Serial.begin(9600); - // выводим сообщение о начале инициализации - Serial.println("Begin init..."); - // инициализация барометра + // Выводим сообщение о начале инициализации + Serial.println("Barometer begin"); + // Инициализируем барометр barometer.begin(); - // выводим сообщение об удачной инициализации + // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } void loop() { - // создаём переменную и присваиваем ей значения абсолютного давления - float pressure = barometer.readPressureMillibars(); - // создаём переменную и присваиваем ей значения высоты над уровнем море - float altitude = barometer.pressureToAltitudeMeters(pressure); - // создаём переменную и присваиваем ей температуру окружающей среды + // Создаём переменную для значения атмосферного давления в Паскалях + float pressurePascals = barometer.readPressurePascals(); + // Создаём переменную для значения атмосферного давления в мм рт.ст. + float pressureMillimetersHg = barometer.readPressureMillimetersHg(); + // Создаём переменную для значения высоты над уровнем море + float altitude = barometer.readAltitude(); + // Создаём переменную для значения температуры окружающей среды float temperature = barometer.readTemperatureC(); - // Вывод данных в Serial порт - Serial.print("p: "); - Serial.print(pressure); - Serial.print(" mbar \t"); - Serial.print("h: "); + // Вывод данных в Serial-порт + Serial.print("Pressure: "); + Serial.print(pressurePascals); + Serial.print(" Pa\t"); + Serial.print(pressureMillimetersHg); + Serial.print(" mmHg\t"); + Serial.print("Height: "); Serial.print(altitude); Serial.print(" m \t"); - Serial.print("t: "); + Serial.print("Temperature: "); Serial.print(temperature); Serial.println(" C"); delay(100); diff --git a/src/Barometer.cpp b/src/Barometer.cpp index f805295..85466f4 100644 --- a/src/Barometer.cpp +++ b/src/Barometer.cpp @@ -1,156 +1,55 @@ #include #include -// Constructors ////////////////////////////////////////////////////// +LPS331::LPS331(uint8_t slaveAddress) + : BaseIMU(slaveAddress) { } -LPS331::LPS331(uint8_t addr) { address = addr; } - -// Public Methods //////////////////////////////////////////////////// - -// sets or detects slave address; returns bool indicating success void LPS331::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - writeReg(LPS331_CTRL_REG1, 0b11100000); - delay(100); + _ctrlReg1 |= (1 << 7); + _ctrlReg1 |= (1 << 6); + _ctrlReg1 |= (1 << 5); + _writeByte(CTRL_REG1, _ctrlReg1); } -// writes register -void LPS331::writeReg(byte reg, byte value) { - _wire->beginTransmission(address); - _wire->write(reg); - _wire->write(value); - _wire->endTransmission(); +float LPS331::readPressurePascals() { + return (float)_readPressureRaw() / 40.96; } -// reads register -byte LPS331::readReg(byte reg) { - byte value; - - _wire->beginTransmission(address); - _wire->write(reg); - _wire->endTransmission(false); // restart - _wire->requestFrom(address, (byte)1); - value = _wire->read(); - _wire->endTransmission(); - - return value; +float LPS331::readPressureMillibars() { + return (float)_readPressureRaw() / 4096; } -// read pressure in pascals -float LPS331::readPressurePascals(void) { - return (float)readPressureRaw() / 40.96; +float LPS331::readPressureMillimetersHg() { + return (float)(_readPressureRaw()) * 0.75006375541921 / 4096.0; } -// reads pressure in millibars (mbar)/hectopascals (hPa) -float LPS331::readPressureMillibars(void) { - return (float)readPressureRaw() / 4096; -} - -// reads pressure in inches of mercury (inHg) -float LPS331::readPressureInchesHg(void) { - return (float)readPressureRaw() / 138706.5; -} - -// reads pressure in millimeters of mercury (mmHg) -float LPS331::readPressureMillimetersHg(void) { - // 1 mbar * 0,75006168270417 = 1 mmHg - return (float)(readPressureRaw()) * 0.75006375541921 / 4096.0; -} - -// reads pressure and returns raw 24-bit sensor output -int32_t LPS331::readPressureRaw(void) { - _wire->beginTransmission(address); - // assert MSB to enable register address auto-increment - _wire->write(LPS331_PRESS_OUT_XL | (1 << 7)); - _wire->endTransmission(); - _wire->requestFrom(address, (byte)3); - - while (_wire->available() < 3) - ; - - uint8_t pxl = _wire->read(); - uint8_t pl = _wire->read(); - uint8_t ph = _wire->read(); - - // combine bytes - return (int32_t)(int8_t)ph << 16 | (uint16_t)pl << 8 | pxl; +float LPS331::readTemperatureC() { + return 42.5 + (float)_readTemperatureRaw() / 480; } -// reads temperature in degrees K float LPS331::readTemperatureK() { return readTemperatureC() + LPS331_CELSIUS_TO_KELVIN_OFFSET; } -// reads temperature in degrees C -float LPS331::readTemperatureC(void) { - return 42.5 + (float)readTemperatureRaw() / 480; +float LPS331::readTemperatureF() { + return 108.5 + (float)_readTemperatureRaw() / 480 * 1.8; } -// reads temperature in degrees F -float LPS331::readTemperatureF(void) { - return 108.5 + (float)readTemperatureRaw() / 480 * 1.8; +uint32_t LPS331::_readPressureRaw() { + uint8_t data[3]; + _readBytes(0x80 | LPS331_PRESS_OUT_XL, data, 3); + return (uint32_t)data[2] << 16 | (uint16_t)data[1] << 8 | data[0]; } -// reads temperature and returns raw 16-bit sensor output -int16_t LPS331::readTemperatureRaw(void) { - _wire->beginTransmission(address); - // assert MSB to enable register address auto-increment - _wire->write(LPS331_TEMP_OUT_L | (1 << 7)); - _wire->endTransmission(); - _wire->requestFrom(address, (byte)2); - - while (_wire->available() < 2) - ; - - uint8_t tl = _wire->read(); - uint8_t th = _wire->read(); - - // combine bytes - return (int16_t)(th << 8 | tl); -} - -// Calculates altitude in meters above MSL using GOST4401-81 -// atmosphere model from the given pressure in pascals -// The model implemented for height up to 51km -// -float LPS331::GOST4401_altitude(float pressure_pascals) { - return GOST4401_getAltitude(pressure_pascals); -} - -// converts pressure in mbar to altitude in meters, using 1976 US -// Standard Atmosphere model (note that this formula only applies to a -// height of 11 km, or about 36000 ft) -// If altimeter setting (QNH, barometric pressure adjusted to sea -// level) is given, this function returns an indicated altitude -// compensated for actual regional pressure; otherwise, it returns -// the pressure altitude above the standard pressure level of 1013.25 -// mbar or 29.9213 inHg -float LPS331::pressureToAltitudeMeters(float pressure_mbar, - float altimeter_setting_mbar) { - return (1 - pow(pressure_mbar / altimeter_setting_mbar, 0.190263)) - * 44330.8; -} - -// converts pressure in inHg to altitude in feet; see notes above -float LPS331::pressureToAltitudeFeet(float pressure_inHg, - float altimeter_setting_inHg) { - return (1 - pow(pressure_inHg / altimeter_setting_inHg, 0.190263)) * 145442; +int16_t LPS331::_readTemperatureRaw() { + uint8_t data[2]; + _readBytes(0x80 | LPS331_TEMP_OUT_L, data, 2); + return (int16_t)data[1] << 8 | data[0]; } -// Private Methods /////////////////////////////////////////////////// - -bool LPS331::autoDetectAddress(void) { - // try each possible address and stop if reading WHO_AM_I returns the - // expected response - address = LPS331AP_ADDRESS_SA0_LOW; - if (testWhoAmI()) - return true; - address = LPS331AP_ADDRESS_SA0_HIGH; - if (testWhoAmI()) - return true; - - return false; +float LPS331::readAltitude() { + float pressurePascals = readPressurePascals(); + return GOST4401_getAltitude(pressurePascals); } - -bool LPS331::testWhoAmI(void) { return (readReg(LPS331_WHO_AM_I) == 0xBB); } diff --git a/src/Barometer.h b/src/Barometer.h index 63d4dab..0b62d98 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -3,33 +3,11 @@ #include "BaseIMU.h" -// The Arduino two-WIRE_IMU interface uses a 7-bit number for the address, -// and sets the last bit correctly based on reads and writes -#define LPS331AP_ADDRESS_SA0_LOW 0b1011100 -#define LPS331AP_ADDRESS_SA0_HIGH 0b1011101 -#define LPS331AP_ADDRESS 0b1011100 -#define LPS331AP_ADDRESS_V2 0b1011101 - -// SA0 states - -#define LPS331_SA0_LOW 0 -#define LPS331_SA0_HIGH 1 -#define LPS331_SA0_AUTO 2 - -// register addresses -// Note: Some of the register names in the datasheet are inconsistent -// between Table 14 in section 6 and the register descriptions in -// section 7. Where they differ, the names from section 7 have been -// used here. - #define LPS331_REF_P_XL 0x08 #define LPS331_REF_P_L 0x09 #define LPS331_REF_P_H 0x0A - #define LPS331_WHO_AM_I 0x0F - #define LPS331_RES_CONF 0x10 - #define LPS331_CTRL_REG1 0x20 #define LPS331_CTRL_REG2 0x21 #define LPS331_CTRL_REG3 0x22 @@ -38,50 +16,32 @@ #define LPS331_THS_P_L 0x25 #define LPS331_THS_P_H 0x26 #define LPS331_STATUS_REG 0x27 - #define LPS331_PRESS_OUT_XL 0x28 #define LPS331_PRESS_OUT_L 0x29 #define LPS331_PRESS_OUT_H 0x2A - #define LPS331_TEMP_OUT_L 0x2B #define LPS331_TEMP_OUT_H 0x2C - #define LPS331_AMP_CTRL 0x30 - #define LPS331_DELTA_PRESS_XL 0x3C #define LPS331_DELTA_PRESS_L 0x3D #define LPS331_DELTA_PRESS_H 0x3E - -// Some physical constants #define LPS331_CELSIUS_TO_KELVIN_OFFSET 273.15 -class LPS331 { +class LPS331 : public BaseIMU { public: - LPS331(uint8_t addr = LPS331AP_ADDRESS); - void begin(TwoWire &wire = Wire); - void writeReg(byte reg, byte value); - byte readReg(byte reg); - float readPressurePascals(void); - float readPressureMillibars(void); - float readPressureInchesHg(void); - float readPressureMillimetersHg(void); - int32_t readPressureRaw(void); - float readTemperatureK(void); - float readTemperatureC(void); - float readTemperatureF(void); - int16_t readTemperatureRaw(void); - static float GOST4401_altitude(float pressure_pascals); - static float pressureToAltitudeMeters(float pressure_mbar, - float altimeter_setting_mbar - = 1013.25); - static float pressureToAltitudeFeet(float pressure_inHg, - float altimeter_setting_inHg = 29.9213); + LPS331(uint8_t slaveAddress); + void begin(TwoWire& wire = Wire); + float readPressurePascals(); + float readPressureMillibars(); + float readPressureMillimetersHg(); + float readTemperatureC(); + float readTemperatureK(); + float readTemperatureF(); + float readAltitude(); private: - TwoWire *_wire; - byte address; - bool autoDetectAddress(void); - bool testWhoAmI(void); + uint32_t _readPressureRaw(); + int16_t _readTemperatureRaw(); }; #endif // __BAROMETER_H__ From ee94e82a6e464374497675ed2ebe485f1705c0b8 Mon Sep 17 00:00:00 2001 From: igor605ds Date: Mon, 26 Oct 2020 15:47:26 +0300 Subject: [PATCH 09/20] replace define to constexpr and enum class --- .../AccelerometerSimple.ino | 17 ++--- .../Compass/CompassAzimuth/CompassAzimuth.ino | 34 ++++++++++ .../CompassCalibration/CompassCalibration.ino | 3 - .../Compass/CompassSimple/CompassSimple.ino | 26 +++----- .../GyroscopeSimple/GyroscopeSimple.ino | 13 ++-- examples/IMU/IMUSimple/IMUSimple.ino | 64 ++++++++---------- examples/IMU/Madgwick6DOF/Madgwick6DOF.ino | 18 ++--- examples/IMU/Madgwick9DOF/Madgwick9DOF.ino | 14 ++-- .../Madgwick9DOFVisualization.ino | 15 +++-- src/Accelerometer.cpp | 54 ++++++++------- src/Accelerometer.h | 23 ++++--- src/Barometer.cpp | 27 ++++---- src/Barometer.h | 32 +++------ src/BaseIMU.cpp | 16 ++--- src/BaseIMU.h | 19 +++--- src/Compass.cpp | 66 ++++++++++--------- src/Compass.h | 33 +++++----- src/GOST4401_81.h | 6 +- src/Gyroscope.cpp | 44 ++++++------- src/Gyroscope.h | 26 ++++---- 20 files changed, 283 insertions(+), 267 deletions(-) create mode 100644 examples/Compass/CompassAzimuth/CompassAzimuth.ino diff --git a/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino b/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino index b5197a8..23ecc31 100644 --- a/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino +++ b/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino @@ -2,7 +2,7 @@ #include // Создаём объект для работы с акселерометром -Accelerometer accel; +Accelerometer accelerometer; void setup() { // Открываем последовательный порт @@ -10,24 +10,21 @@ void setup() { // Выводим сообщение о начале инициализации Serial.println("Accelerometer begin"); // Инициализируем акселерометр - accel.begin(); - // Устанавливаем чувствительность акселерометра - // 2g (по умолчанию), 4g, 8g - accel.setRange(RANGE_2G); + accelerometer.begin(); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } void loop() { // Выводим направления и величины ускорения в м/с² по оси X - Serial.print(accel.readAccelerationAX()); + Serial.print(accelerometer.readAccelerationAX()); Serial.print("\t\t"); // Выводим направления и величины ускорения в м/с² по оси Y - Serial.print(accel.readAccelerationAY()); + Serial.print(accelerometer.readAccelerationAY()); Serial.print("\t\t"); // Выводим направления и величины ускорения в м/с² по оси Z - Serial.print(accel.readAccelerationAZ()); + Serial.print(accelerometer.readAccelerationAZ()); Serial.print("\t\t"); - Serial.println(""); + Serial.println(); delay(100); -} +} \ No newline at end of file diff --git a/examples/Compass/CompassAzimuth/CompassAzimuth.ino b/examples/Compass/CompassAzimuth/CompassAzimuth.ino new file mode 100644 index 0000000..f04ea7a --- /dev/null +++ b/examples/Compass/CompassAzimuth/CompassAzimuth.ino @@ -0,0 +1,34 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для работы с иагнитометром/компасом +Compass compass; + +// Калибровочные значения, полученные в калибровочной матрице +// из примера compassCalibration +const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; + +const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, + { 0.09, 3.081, 0.016 }, + { -0.003, -0.225, 2.922 } }; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Выводим сообщение о начале инициализации + Serial.println("Compass begin"); + // Инициализируем компас + compass.begin(); + // Устанавливаем калибровочные данные + compass.setCalibrateMatrix(compassCalibrationMatrix, + compassCalibrationBias); + // Выводим сообщение об удачной инициализации + Serial.println("Initialization completed"); +} + +void loop() { + // Выводим азимут относительно оси Z + Serial.print(compass.readAzimut()); + Serial.println(" Degrees"); + delay(100); +} diff --git a/examples/Compass/CompassCalibration/CompassCalibration.ino b/examples/Compass/CompassCalibration/CompassCalibration.ino index 5fcdbfb..a524f13 100644 --- a/examples/Compass/CompassCalibration/CompassCalibration.ino +++ b/examples/Compass/CompassCalibration/CompassCalibration.ino @@ -9,9 +9,6 @@ void setup() { Serial.begin(9600); // Инициализируем компас compass.begin(); - // Устанавливаем чувствительность компаса - // ±4 gauss (по умолчанию), ±8 gauss, ±12 gauss, ±16 gauss - compass.setRange(RANGE_4_GAUSS); } void loop() { diff --git a/examples/Compass/CompassSimple/CompassSimple.ino b/examples/Compass/CompassSimple/CompassSimple.ino index ee52823..be77baa 100644 --- a/examples/Compass/CompassSimple/CompassSimple.ino +++ b/examples/Compass/CompassSimple/CompassSimple.ino @@ -4,14 +4,6 @@ // Создаём объект для работы с иагнитометром/компасом Compass compass; -// Калибровочные значения, полученные в калибровочной матрице -// из примера compassCalibration -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; - -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; - void setup() { // Открываем последовательный порт Serial.begin(9600); @@ -19,19 +11,19 @@ void setup() { Serial.println("Compass begin"); // Инициализируем компас compass.begin(); - // Устанавливаем чувствительность компаса - // ±4 gauss (по умолчанию), ±8 gauss, ±12 gauss, ±16 gauss - compass.setRange(RANGE_4_GAUSS); - // Устанавливаем калибровочные данные - compass.setCalibrateMatrix(compassCalibrationMatrix, - compassCalibrationBias); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } void loop() { - // Выводим азимут относительно оси Z - Serial.print(compass.readAzimut()); - Serial.println(" Degrees"); + // Выводим напряженность магнитного поля в Гауссах по оси X + Serial.print(compass.readMagneticGaussX()); + Serial.print("\t\t"); + // Выводим напряженность магнитного поля в Гауссах по оси Y + Serial.print(compass.readMagneticGaussY()); + Serial.print("\t\t"); + // Выводим напряженность магнитного поля в Гауссах по оси Z + Serial.print(compass.readMagneticGaussZ()); + Serial.println(); delay(100); } diff --git a/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino b/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino index 834f890..e69af7d 100644 --- a/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino +++ b/examples/Gyroscope/GyroscopeSimple/GyroscopeSimple.ino @@ -2,7 +2,7 @@ #include // Создаём объект для работы с гироскопом -Gyroscope gyro; +Gyroscope gyroscope; void setup() { // Открываем последовательный порт @@ -10,23 +10,20 @@ void setup() { // Выводим сообщение о начале инициализации Serial.println("Gyroscope begin"); // Инициализируем гироскоп - gyro.begin(); - // Устанавливаем чувствительность гироскопа - // 250dps (по умолчанию), 500dps, 2000dps - gyro.setRange(RANGE_250DPS); + gyroscope.begin(); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } void loop() { // Выводим угловую скорость в градусах в секунду относительно оси X - Serial.print(gyro.readRotationDegX()); + Serial.print(gyroscope.readRotationDegX()); Serial.print("\t\t"); // Выводим угловую скорость в градусах в секунду относительно оси Y - Serial.print(gyro.readRotationDegY()); + Serial.print(gyroscope.readRotationDegY()); Serial.print("\t\t"); // Выводим угловую скорость в градусах в секунду относительно оси Z - Serial.print(gyro.readRotationDegZ()); + Serial.print(gyroscope.readRotationDegZ()); Serial.print("\t\t"); Serial.println(); delay(100); diff --git a/examples/IMU/IMUSimple/IMUSimple.ino b/examples/IMU/IMUSimple/IMUSimple.ino index ab7b72f..d6ba7fe 100644 --- a/examples/IMU/IMUSimple/IMUSimple.ino +++ b/examples/IMU/IMUSimple/IMUSimple.ino @@ -2,70 +2,62 @@ #include // Создаём объект для работы с гироскопом -Gyroscope gyro; +Gyroscope gyroscope; // Создаём объект для работы с акселерометром -Accelerometer accel; -// Создаём объект для работы с магнитометром/компасом +Accelerometer accelerometer; +// Создаём объект для работы с компасом Compass compass; -// Создаём объект для работы с акселерометром +// Создаём объект для работы с барометром Barometer barometer; -// Калибровочные значения, полученные в калибровочной матрице -// из примера compassCalibration -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; - -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; - void setup() { // Открываем последовательный порт Serial.begin(9600); // Выводим сообщение о начале инициализации Serial.println("IMU Begin"); // Инициализируем гироскоп - gyro.begin(); + gyroscope.begin(); // Инициализируем акселерометр - accel.begin(); + accelerometer.begin(); // Инициализируем компас compass.begin(); // Инициализируем барометр barometer.begin(); - // Устанавливаем калибровочные данные - compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); Serial.println("Gyroscope\t\t\tAccelerometer\t\t\tCompass\t\tBarometer"); } void loop() { - // Выводим угловую скорость в градусах в секунду относительно оси X - Serial.print(gyro.readRotationDegX()); + // Выводим угловую скорость в градусах в секунду относительно оси X, Y и Z + Serial.print(gyroscope.readRotationDegX()); Serial.print("\t"); - // Выводим угловую скорость в градусах в секунду относительно оси Y - Serial.print(gyro.readRotationDegY()); + Serial.print(gyroscope.readRotationDegY()); Serial.print("\t"); - // Выводим угловую скорость в градусах в секунду относительно оси Z - Serial.print(gyro.readRotationDegZ()); + Serial.print(gyroscope.readRotationDegZ()); Serial.print("\t\t"); - // Выводим направления и величины ускорения в м/с² по оси X - Serial.print(accel.readAccelerationAX()); + // Выводим направления и величины ускорения в м/с² относительно оси X, Y и Z + Serial.print(accelerometer.readAccelerationAX()); Serial.print("\t"); - // Выводим направления и величины ускорения в м/с² по оси Y - Serial.print(accel.readAccelerationAY()); + Serial.print(accelerometer.readAccelerationAY()); Serial.print("\t"); - // Выводим направления и величины ускорения в м/с² по оси Z - Serial.print(accel.readAccelerationAZ()); + Serial.print(accelerometer.readAccelerationAZ()); Serial.print("\t\t"); - // Выводим азимут относительно оси Z - Serial.print(compass.readAzimut()); - Serial.print(" Degrees\t"); - // Выводим значения абсолютного давления - Serial.print(barometer.readPressureMillibars()); + // Выводим напряженность магнитного поля в Гауссах относительно оси X, Y и Z + Serial.print(compass.readMagneticGaussX()); Serial.print("\t"); - // Выодим значения температуры окружающей среды - Serial.print(barometer.readTemperatureC()); + Serial.print(compass.readMagneticGaussY()); Serial.print("\t"); - Serial.println(""); + Serial.print(compass.readMagneticGaussZ()); + Serial.print("\t\t"); + // Выводим значения атмосферного давления в мм рт.ст. + Serial.print(barometer.readPressureMillimetersHg()); + Serial.print("\t"); + // Выводим значения высоты над уровнем море + Serial.print(barometer.readAltitude()); + Serial.print("\t"); + // Выводим значения температуры окружающей среды + Serial.print(barometer.readTemperatureC()); + Serial.println(); delay(100); } diff --git a/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino index 03cd2f1..2009878 100644 --- a/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino +++ b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino @@ -2,14 +2,14 @@ #include // Множитель фильтра -#define BETA 0.22f +constexpr float BETA = 0.22; // Создаём объект для фильтра Madgwick Madgwick filter; // Создаём объект для работы с гироскопом -Gyroscope gyro; +Gyroscope gyroscope; // Создаём объект для работы с акселерометром -Accelerometer accel; +Accelerometer accelerometer; // Переменные для данных с гироска и акселерометра float gx, gy, gz, ax, ay, az; @@ -26,9 +26,9 @@ void setup() { // Выводим сообщение о начале инициализации Serial.println("IMU Begin"); // Инициализируем гироскоп - gyro.begin(); + gyroscope.begin(); // Инициализируем акселерометр - accel.begin(); + accelerometer.begin(); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } @@ -37,9 +37,9 @@ void loop() { // Запоминаем текущее время unsigned long startMillis = millis(); // Считываем данные с акселерометра в единицах G - accel.readAccelerationGXYZ(ax, ay, az); - // Считываем данные с акселерометра в радианах в секунду - gyro.readRotationRadXYZ(gx, gy, gz); + accelerometer.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с гироскопа в радианах в секунду + gyroscope.readRotationRadXYZ(gx, gy, gz); // Устанавливаем коэффициенты фильтра filter.setKoeff(fps, BETA); // Обновляем входные данные в фильтр @@ -50,7 +50,7 @@ void loop() { pitch = filter.getPitchDeg(); roll = filter.getRollDeg(); - // Выводим полученные углы Эйлера в serial-порт + // Выводим полученные углы Эйлера в Serial-порт Serial.print("yaw: "); Serial.print(yaw); Serial.print("\t\t"); diff --git a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino index 44369da..dfaa032 100644 --- a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino +++ b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino @@ -2,14 +2,14 @@ #include // Множитель фильтра -#define BETA 0.22f +constexpr float BETA = 0.22; // Создаём объект для фильтра Madgwick Madgwick filter; // Создаём объект для работы с гироскопом -Gyroscope gyro; +Gyroscope gyroscope; // Создаём объект для работы с акселерометром -Accelerometer accel; +Accelerometer accelerometer; // Создаём объект для работы с компасом Compass compass; @@ -36,9 +36,9 @@ void setup() { // Выводим сообщение о начале инициализации Serial.println("IMU Begin"); // Инициализируем гироскоп - gyro.begin(); + gyroscope.begin(); // Инициализируем акселерометр - accel.begin(); + accelerometer.begin(); // Инициализируем компас compass.begin(); // Устанавливаем калибровочные данные @@ -53,9 +53,9 @@ void loop() { unsigned long startMillis = millis(); // Считываем данные с акселерометра в единицах G - accel.readAccelerationGXYZ(ax, ay, az); + accelerometer.readAccelerationGXYZ(ax, ay, az); // Считываем данные с гироскопа в радианах в секунду - gyro.readRotationRadXYZ(gx, gy, gz); + gyroscope.readRotationRadXYZ(gx, gy, gz); // Считываем данные с компаса в Гауссах compass.readCalibrateMagneticGaussXYZ(mx, my, mz); diff --git a/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino b/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino index 714add5..869c198 100644 --- a/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino +++ b/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino @@ -2,14 +2,15 @@ #include // Множитель фильтра -#define BETA 0.22f +constexpr float BETA = 0.22; // Создаём объект для фильтра Madgwick Madgwick filter; + // Создаём объект для работы с гироскопом -Gyroscope gyro; +Gyroscope gyroscope; // Создаём объект для работы с акселерометром -Accelerometer accel; +Accelerometer accelerometer; // Создаём объект для работы с компасом Compass compass; @@ -34,9 +35,9 @@ void setup() { // Открываем последовательный порт Serial.begin(9600); // Инициализируем гироскоп - gyro.begin(); + gyroscope.begin(); // Инициализируем акселерометр - accel.begin(); + accelerometer.begin(); // Инициализируем компас compass.begin(); // Устанавливаем калибровочные данные @@ -49,9 +50,9 @@ void loop() { unsigned long startMillis = millis(); // Считываем данные с акселерометра в единицах G - accel.readAccelerationGXYZ(ax, ay, az); + accelerometer.readAccelerationGXYZ(ax, ay, az); // Считываем данные с гироскопа в радианах в секунду - gyro.readRotationRadXYZ(gx, gy, gz); + gyroscope.readRotationRadXYZ(gx, gy, gz); // Считываем данные с компаса в Гауссах compass.readCalibrateMagneticGaussXYZ(mx, my, mz); diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index a6216ba..5bbd41b 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -11,28 +11,28 @@ void LIS331DLH::begin(TwoWire& wire) { _ctrlReg1 |= (1 << 2); _ctrlReg1 |= (1 << 5); _writeByte(CTRL_REG1, _ctrlReg1); - setRange(RANGE_2G); + setRange(AccelerometerRange::RANGE_2G); } -void LIS331DLH::setRange(uint8_t range) { +void LIS331DLH::setRange(AccelerometerRange range) { switch (range) { - case RANGE_2G: { - _ctrlReg4 = ADR_FS_2; - _scale = RANGE_2G / 32767.0; + case AccelerometerRange::RANGE_2G: { + _ctrlReg4 = 0; + _scalingFactor = SENS_2G * 4 / pow(2, 16); break; } - case RANGE_4G: { - _ctrlReg4 = ADR_FS_4; - _scale = RANGE_4G / 32767.0; + case AccelerometerRange::RANGE_4G: { + _ctrlReg4 = LIS331DLH_CTRL_REG4_FS0; + _scalingFactor = SENS_4G * 4 / pow(2, 16); break; } - case RANGE_8G: { - _ctrlReg4 = ADR_FS_8; - _scale = RANGE_8G / 32767.0; + case AccelerometerRange::RANGE_8G: { + _ctrlReg4 = LIS331DLH_CTRL_REG4_FS0 | LIS331DLH_CTRL_REG4_FS1; + _scalingFactor = SENS_8G * 4 / pow(2, 16); break; } default: { - _scale = RANGE_2G / 32767.0; + _scalingFactor = SENS_2G * 4 / pow(2, 16); } break; } _writeByte(CTRL_REG4, _ctrlReg4); @@ -47,29 +47,35 @@ void LIS331DLH::sleep(bool state) { _writeByte(CTRL_REG1, _ctrlReg1); } -float LIS331DLH::readAccelerationGX() { return readX() * _scale; } +float LIS331DLH::readAccelerationGX() { return readX() * _scalingFactor; } -float LIS331DLH::readAccelerationGY() { return readY() * _scale; } +float LIS331DLH::readAccelerationGY() { return readY() * _scalingFactor; } -float LIS331DLH::readAccelerationGZ() { return readZ() * _scale; } +float LIS331DLH::readAccelerationGZ() { return readZ() * _scalingFactor; } -float LIS331DLH::readAccelerationAX() { return readAccelerationGX() * G; } +float LIS331DLH::readAccelerationAX() { + return readAccelerationGX() * GRAVITY_EARTH; +} -float LIS331DLH::readAccelerationAY() { return readAccelerationGY() * G; } +float LIS331DLH::readAccelerationAY() { + return readAccelerationGY() * GRAVITY_EARTH; +} -float LIS331DLH::readAccelerationAZ() { return readAccelerationGZ() * G; } +float LIS331DLH::readAccelerationAZ() { + return readAccelerationGZ() * GRAVITY_EARTH; +} void LIS331DLH::readAccelerationGXYZ(float& ax, float& ay, float& az) { int16_t x, y, z; readXYZ(x, y, z); - ax = x * _scale; - ay = y * _scale; - az = z * _scale; + ax = x * _scalingFactor; + ay = y * _scalingFactor; + az = z * _scalingFactor; } void LIS331DLH::readAccelerationAXYZ(float& ax, float& ay, float& az) { readAccelerationGXYZ(ax, ay, az); - ax *= G; - ay *= G; - az *= G; + ax *= GRAVITY_EARTH; + ay *= GRAVITY_EARTH; + az *= GRAVITY_EARTH; } diff --git a/src/Accelerometer.h b/src/Accelerometer.h index e92a6c3..8251339 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -3,22 +3,24 @@ #include "BaseIMU.h" -#define RANGE_2G 2 -#define RANGE_4G 4 -#define RANGE_8G 8 +constexpr auto LIS331DLH_CTRL_REG4_FS0 = 0x10; +constexpr auto LIS331DLH_CTRL_REG4_FS1 = 0x20; -#define ADR_FS_2 0x00 -#define ADR_FS_4 0x10 -#define ADR_FS_8 0x30 +constexpr auto GRAVITY_EARTH = 9.8; -#define G 9.8 +enum class AccelerometerRange { RANGE_2G = 2, RANGE_4G = 4, RANGE_8G = 8 }; + +constexpr float SENS_2G = 1; +constexpr float SENS_4G = 2; +constexpr float SENS_8G = 3.9; class LIS331DLH : public BaseIMU { public: LIS331DLH(uint8_t slaveAddress); - void begin(TwoWire &wire = Wire); + enum class ScaleRange; + void begin(TwoWire& wire = Wire); void sleep(bool state); - void setRange(uint8_t range); + void setRange(AccelerometerRange range); float readAccelerationGX(); float readAccelerationGY(); float readAccelerationGZ(); @@ -27,6 +29,9 @@ class LIS331DLH : public BaseIMU { float readAccelerationAZ(); void readAccelerationGXYZ(float& ax, float& ay, float& az); void readAccelerationAXYZ(float& ax, float& ay, float& az); + +private: + float _scalingFactor; }; #endif // __ACCELEROMETER_H__ diff --git a/src/Barometer.cpp b/src/Barometer.cpp index 85466f4..2a07167 100644 --- a/src/Barometer.cpp +++ b/src/Barometer.cpp @@ -9,33 +9,33 @@ void LPS331::begin(TwoWire& wire) { _wire->begin(); _ctrlReg1 |= (1 << 7); _ctrlReg1 |= (1 << 6); - _ctrlReg1 |= (1 << 5); _writeByte(CTRL_REG1, _ctrlReg1); } -float LPS331::readPressurePascals() { - return (float)_readPressureRaw() / 40.96; +float LPS331::readPressureMillibars() { + return static_cast(_readPressureRaw()) / 4096; } -float LPS331::readPressureMillibars() { - return (float)_readPressureRaw() / 4096; +float LPS331::readPressurePascals() { + return readPressureMillibars() * LPS331_MILLIBARS_TO_PASCALS; } float LPS331::readPressureMillimetersHg() { - return (float)(_readPressureRaw()) * 0.75006375541921 / 4096.0; + return readPressureMillibars() * LPS331_MILLIBARS_TO_MILLIMETERSHG; } +// Temperature output data from datasheet float LPS331::readTemperatureC() { - return 42.5 + (float)_readTemperatureRaw() / 480; + return 42.5 + static_cast(_readTemperatureRaw()) / 480; } +// Convert Celsius scale to Kelvin float LPS331::readTemperatureK() { - return readTemperatureC() + LPS331_CELSIUS_TO_KELVIN_OFFSET; + return readTemperatureC() + LPS331_CELSIUS_TO_KELVIN; } -float LPS331::readTemperatureF() { - return 108.5 + (float)_readTemperatureRaw() / 480 * 1.8; -} +// Convert Celsius scale to Fahrenheit +float LPS331::readTemperatureF() { return readTemperatureC() * 1.8 + 32; } uint32_t LPS331::_readPressureRaw() { uint8_t data[3]; @@ -44,9 +44,8 @@ uint32_t LPS331::_readPressureRaw() { } int16_t LPS331::_readTemperatureRaw() { - uint8_t data[2]; - _readBytes(0x80 | LPS331_TEMP_OUT_L, data, 2); - return (int16_t)data[1] << 8 | data[0]; + return ((int16_t)_readByte(LPS331_TEMP_OUT_H) << 8) + | _readByte(LPS331_TEMP_OUT_L); } float LPS331::readAltitude() { diff --git a/src/Barometer.h b/src/Barometer.h index 0b62d98..59dc05a 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -3,29 +3,15 @@ #include "BaseIMU.h" -#define LPS331_REF_P_XL 0x08 -#define LPS331_REF_P_L 0x09 -#define LPS331_REF_P_H 0x0A -#define LPS331_WHO_AM_I 0x0F -#define LPS331_RES_CONF 0x10 -#define LPS331_CTRL_REG1 0x20 -#define LPS331_CTRL_REG2 0x21 -#define LPS331_CTRL_REG3 0x22 -#define LPS331_INTERRUPT_CFG 0x23 -#define LPS331_INT_SOURCE 0x24 -#define LPS331_THS_P_L 0x25 -#define LPS331_THS_P_H 0x26 -#define LPS331_STATUS_REG 0x27 -#define LPS331_PRESS_OUT_XL 0x28 -#define LPS331_PRESS_OUT_L 0x29 -#define LPS331_PRESS_OUT_H 0x2A -#define LPS331_TEMP_OUT_L 0x2B -#define LPS331_TEMP_OUT_H 0x2C -#define LPS331_AMP_CTRL 0x30 -#define LPS331_DELTA_PRESS_XL 0x3C -#define LPS331_DELTA_PRESS_L 0x3D -#define LPS331_DELTA_PRESS_H 0x3E -#define LPS331_CELSIUS_TO_KELVIN_OFFSET 273.15 +constexpr auto LPS331_PRESS_OUT_XL = 0x28; +constexpr auto LPS331_PRESS_OUT_L = 0x29; +constexpr auto LPS331_PRESS_OUT_H = 0x2A; +constexpr auto LPS331_TEMP_OUT_L = 0x2B; +constexpr auto LPS331_TEMP_OUT_H = 0x2C; + +constexpr auto LPS331_CELSIUS_TO_KELVIN = 273.15; +constexpr auto LPS331_MILLIBARS_TO_PASCALS = 100; +constexpr auto LPS331_MILLIBARS_TO_MILLIMETERSHG = 0.75; class LPS331 : public BaseIMU { public: diff --git a/src/BaseIMU.cpp b/src/BaseIMU.cpp index c232462..b28c0eb 100644 --- a/src/BaseIMU.cpp +++ b/src/BaseIMU.cpp @@ -1,23 +1,23 @@ #include "BaseIMU.h" int16_t BaseIMU::readX() { - return ((int16_t)_readByte(OUT_X + 1) << 8) | _readByte(OUT_X); + return ((int16_t)_readByte(OUT_X_H) << 8) | _readByte(OUT_X_L); } int16_t BaseIMU::readY() { - return ((int16_t)_readByte(OUT_Y + 1) << 8) | _readByte(OUT_Y); + return ((int16_t)_readByte(OUT_Y_H) << 8) | _readByte(OUT_Y_L); } int16_t BaseIMU::readZ() { - return ((int16_t)_readByte(OUT_Z + 1) << 8) | _readByte(OUT_Z); + return ((int16_t)_readByte(OUT_Z_H) << 8) | _readByte(OUT_Z_L); } -void BaseIMU::readXYZ(int16_t &x, int16_t &y, int16_t &z) { +void BaseIMU::readXYZ(int16_t& x, int16_t& y, int16_t& z) { uint8_t data[6]; - _readBytes(0x80 | OUT_X, data, 6); - x = ((int16_t)data[1] << 8 ) | data[0]; - y = ((int16_t)data[3] << 8 ) | data[2]; - z = ((int16_t)data[5] << 8 ) | data[4]; + _readBytes(0x80 | OUT_X_L, data, 6); + x = ((int16_t)data[1] << 8) | data[0]; + y = ((int16_t)data[3] << 8) | data[2]; + z = ((int16_t)data[5] << 8) | data[4]; } uint8_t BaseIMU::_readByte(uint8_t regAddress) { diff --git a/src/BaseIMU.h b/src/BaseIMU.h index cedde84..eb7531b 100644 --- a/src/BaseIMU.h +++ b/src/BaseIMU.h @@ -4,15 +4,18 @@ #include #include -#define CTRL_REG1 0x20 -#define CTRL_REG2 0x21 -#define CTRL_REG3 0x22 -#define CTRL_REG4 0x23 -#define CTRL_REG5 0x24 +constexpr auto CTRL_REG1 = 0x20; +constexpr auto CTRL_REG2 = 0x21; +constexpr auto CTRL_REG3 = 0x22; +constexpr auto CTRL_REG4 = 0x23; +constexpr auto CTRL_REG5 = 0x24; -#define OUT_X 0x28 -#define OUT_Y 0x2A -#define OUT_Z 0x2C +constexpr auto OUT_X_L = 0x28; +constexpr auto OUT_X_H = 0x29; +constexpr auto OUT_Y_L = 0x2A; +constexpr auto OUT_Y_H = 0x2B; +constexpr auto OUT_Z_L = 0x2C; +constexpr auto OUT_Z_H = 0x2D; class BaseIMU { public: diff --git a/src/Compass.cpp b/src/Compass.cpp index 5c52c88..5b37878 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -7,33 +7,33 @@ void LIS3MDL::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); _writeByte(CTRL_REG3, _ctrlReg3); - setRange(RANGE_4_GAUSS); + setRange(CompassRange::RANGE_16GAUSS); } -void LIS3MDL::setRange(uint8_t range) { +void LIS3MDL::setRange(CompassRange range) { switch (range) { - case RANGE_4_GAUSS: { - _ctrlReg2 = ADR_FS_4; - _scale = SENS_FS_4; + case CompassRange::RANGE_4GAUSS: { + _ctrlReg2 = 0; + _scalingFactor = SENS_4GAUSS; break; } - case RANGE_8_GAUSS: { - _ctrlReg2 = ADR_FS_8; - _scale = SENS_FS_8; + case CompassRange::RANGE_8GAUSS: { + _ctrlReg2 = LIS3MDL_CTRL_REG2_FS0; + _scalingFactor = SENS_8GAUSS; break; } - case RANGE_12_GAUSS: { - _ctrlReg2 = ADR_FS_12; - _scale = SENS_FS_12; + case CompassRange::RANGE_12GAUSS: { + _ctrlReg2 = LIS3MDL_CTRL_REG2_FS1; + _scalingFactor = SENS_12GAUSS; break; } - case RANGE_16_GAUSS: { - _ctrlReg2 = ADR_FS_16; - _scale = SENS_FS_16; + case CompassRange::RANGE_16GAUSS: { + _ctrlReg2 = LIS3MDL_CTRL_REG2_FS0 | LIS3MDL_CTRL_REG2_FS1; + _scalingFactor = SENS_16GAUSS; break; } default: { - _scale = SENS_FS_4; + _scalingFactor = SENS_4GAUSS; } break; } _writeByte(CTRL_REG2, _ctrlReg2); @@ -48,31 +48,32 @@ void LIS3MDL::sleep(bool state) { _writeByte(CTRL_REG3, _ctrlReg3); } -float LIS3MDL::readMagneticGaussX() { return readX() / _scale; } +float LIS3MDL::readMagneticGaussX() { return readX() / _scalingFactor; } -float LIS3MDL::readMagneticGaussY() { return readY() / _scale; } +float LIS3MDL::readMagneticGaussY() { return readY() / _scalingFactor; } -float LIS3MDL::readMagneticGaussZ() { return readZ() / _scale; } +float LIS3MDL::readMagneticGaussZ() { return readZ() / _scalingFactor; } void LIS3MDL::readMagneticGaussXYZ(float& mx, float& my, float& mz) { int16_t x, y, z; readXYZ(x, y, z); - mx = x / _scale; - my = y / _scale; - mz = z / _scale; + mx = x / _scalingFactor; + my = y / _scalingFactor; + mz = z / _scalingFactor; } void LIS3MDL::readCalibrateMagneticGaussXYZ(float& mx, float& my, float& mz) { int16_t x, y, z; readXYZ(x, y, z); - _calibrate((float&)x, (float&)y, (float&)z); - mx = x / _scale; - my = y / _scale; - mz = z / _scale; + mx = x, my = y, mz = z; + _calibrate(mx, my, mz); + mx = x / _scalingFactor; + my = y / _scalingFactor; + mz = z / _scalingFactor; } void LIS3MDL::setCalibrateMatrix(const float calibrationMatrix[3][3], - const float calibrationBias[3]) { + const float calibrationBias[3]) { memcpy(_calibrationBias, calibrationBias, 3 * sizeof(float)); memcpy(_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof(float)); } @@ -86,7 +87,8 @@ void LIS3MDL::_calibrate(float& x, float& y, float& z) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - calibrationValues[i] += _calibrationMatrix[i][j] * nonCalibrationValues[j]; + calibrationValues[i] + += _calibrationMatrix[i][j] * nonCalibrationValues[j]; } } @@ -96,11 +98,11 @@ void LIS3MDL::_calibrate(float& x, float& y, float& z) { } float LIS3MDL::readAzimut() { - float x = readX(); - float y = readY(); - float z = readZ(); - _calibrate(x, y, z); - float heading = atan2(x, y); + int16_t x, y, z; + readXYZ(x, y, z); + float mx = x, my = y, mz = z; + _calibrate(mx, my, mz); + float heading = atan2(mx, my); if (heading < 0) heading += TWO_PI; diff --git a/src/Compass.h b/src/Compass.h index e17552c..c620591 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -3,40 +3,41 @@ #include "BaseIMU.h" -#define RANGE_4_GAUSS 0 -#define RANGE_8_GAUSS 1 -#define RANGE_12_GAUSS 2 -#define RANGE_16_GAUSS 3 +constexpr auto LIS3MDL_CTRL_REG2_FS0 = 0x20; +constexpr auto LIS3MDL_CTRL_REG2_FS1 = 0x40; -#define ADR_FS_4 0x00 -#define ADR_FS_8 0x20 -#define ADR_FS_12 0x40 -#define ADR_FS_16 0x60 +enum class CompassRange { + RANGE_4GAUSS = 1, + RANGE_8GAUSS = 2, + RANGE_12GAUSS = 3, + RANGE_16GAUSS = 4 +}; -#define SENS_FS_4 6842 -#define SENS_FS_8 3421 -#define SENS_FS_12 2281 -#define SENS_FS_16 1711 +constexpr float SENS_4GAUSS = 6842; +constexpr float SENS_8GAUSS = 3421; +constexpr float SENS_12GAUSS = 2281; +constexpr float SENS_16GAUSS = 1711; class LIS3MDL : public BaseIMU { public: LIS3MDL(uint8_t slaveAddress); void begin(TwoWire& wire = Wire); void sleep(bool state); - void setRange(uint8_t range); + void setRange(CompassRange range); float readMagneticGaussX(); float readMagneticGaussY(); float readMagneticGaussZ(); - void readMagneticGaussXYZ(float& x, float& y, float& z); - void readCalibrateMagneticGaussXYZ(float& x, float& y, float& z); + void readCalibrateMagneticGaussXYZ(float& mx, float& my, float& mz); + void readMagneticGaussXYZ(float& mx, float& my, float& mz); void setCalibrateMatrix(const float calibrationMatrix[3][3], const float bias[3]); float readAzimut(); private: - void _calibrate(float& x, float& y, float& z); + void _calibrate(float& mx, float& my, float& mz); float _calibrationMatrix[3][3]; float _calibrationBias[3]; + float _scalingFactor; }; #endif // __COMPASS_H__ diff --git a/src/GOST4401_81.h b/src/GOST4401_81.h index 15640d1..c20d896 100644 --- a/src/GOST4401_81.h +++ b/src/GOST4401_81.h @@ -9,8 +9,8 @@ * by Oleg Kochetov */ -#ifndef __ATMOSPHERE_GOST4401_H__ -#define __ATMOSPHERE_GOST4401_H__ +#ifndef __ATMOSPHERE_GOST4401_81_H__ +#define __ATMOSPHERE_GOST4401_81_H__ #include @@ -47,4 +47,4 @@ static const GOST4401_RECORD ag_table[] = { { 47000, 270.65, 0.0, 110.9056 }, { 51000, 270.65, -0.0028, 6.69384 } }; -#endif // __ATMOSPHERE_GOST4401_H__ \ No newline at end of file +#endif // __ATMOSPHERE_GOST4401_81_H__ \ No newline at end of file diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp index 9895fdb..ffcc94e 100644 --- a/src/Gyroscope.cpp +++ b/src/Gyroscope.cpp @@ -3,38 +3,38 @@ L3G4200D::L3G4200D(uint8_t slaveAddress) : BaseIMU(slaveAddress) { } -void L3G4200D::setRange(uint8_t range) { +void L3G4200D::begin(TwoWire& wire) { + _wire = &wire; + _wire->begin(); + _ctrlReg1 |= (1 << 0) | (1 << 2) | (1 << 3) | (1 << 4); + _writeByte(CTRL_REG1, _ctrlReg1); + setRange(GyroscopeRange::RANGE_2000DPS); +} + +void L3G4200D::setRange(GyroscopeRange range) { switch (range) { - case RANGE_250DPS: { - _ctrlReg4 |= ADR_FS_250; - _scale = SENS_FS_250; + case GyroscopeRange::RANGE_250DPS: { + _ctrlReg4 = 0; + _scalingFactor = SENS_250DPS; break; } - case RANGE_500DPS: { - _ctrlReg4 |= ADR_FS_500; - _scale = SENS_FS_500; + case GyroscopeRange::RANGE_500DPS: { + _ctrlReg4 = L3G4200D_CTRL_REG4_FS0; + _scalingFactor = SENS_500DPS; break; } - case RANGE_2000DPS: { - _ctrlReg4 |= ADR_FS_2000; - _scale = SENS_FS_2000; + case GyroscopeRange::RANGE_2000DPS: { + _ctrlReg4 = L3G4200D_CTRL_REG4_FS0 | L3G4200D_CTRL_REG4_FS1; + _scalingFactor = SENS_2000DPS; break; } default: { - _scale = SENS_FS_250; + _scalingFactor = SENS_250DPS; } break; } _writeByte(CTRL_REG4, _ctrlReg4); } -void L3G4200D::begin(TwoWire& wire) { - _wire = &wire; - _wire->begin(); - _ctrlReg1 |= (1 << 0) | (1 << 2) | (1 << 3) | (1 << 4); - _writeByte(CTRL_REG1, _ctrlReg1); - setRange(RANGE_250DPS); -} - void L3G4200D::sleep(bool state) { if (state) { _ctrlReg1 &= ~(1 << 3); @@ -44,11 +44,11 @@ void L3G4200D::sleep(bool state) { _writeByte(CTRL_REG1, _ctrlReg1); } -float L3G4200D::readRotationDegX() { return readX() * _scale; } +float L3G4200D::readRotationDegX() { return readX() * _scalingFactor; } -float L3G4200D::readRotationDegY() { return readY() * _scale; } +float L3G4200D::readRotationDegY() { return readY() * _scalingFactor; } -float L3G4200D::readRotationDegZ() { return readZ() * _scale; } +float L3G4200D::readRotationDegZ() { return readZ() * _scalingFactor; } float L3G4200D::readRotationRadX() { return readRotationDegX() * DEG_TO_RAD; } diff --git a/src/Gyroscope.h b/src/Gyroscope.h index 102bdcb..f4d1946 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -3,24 +3,25 @@ #include "BaseIMU.h" -#define RANGE_250DPS 0 -#define RANGE_500DPS 1 -#define RANGE_2000DPS 2 +constexpr auto L3G4200D_CTRL_REG4_FS0 = 0x10; +constexpr auto L3G4200D_CTRL_REG4_FS1 = 0x20; -#define ADR_FS_250 0x00 -#define ADR_FS_500 0x10 -#define ADR_FS_2000 0x20 +enum class GyroscopeRange { + RANGE_250DPS = 1, + RANGE_500DPS = 2, + RANGE_2000DPS = 3 +}; -#define SENS_FS_250 0.00875 -#define SENS_FS_500 0.0175 -#define SENS_FS_2000 0.07 +constexpr float SENS_250DPS = 0.00875; +constexpr float SENS_500DPS = 0.0175; +constexpr float SENS_2000DPS = 0.07; class L3G4200D : public BaseIMU { public: L3G4200D(uint8_t slaveAddress); - void begin(TwoWire &wire = Wire); + void begin(TwoWire& wire = Wire); void sleep(bool state); - void setRange(uint8_t range); + void setRange(GyroscopeRange range); float readRotationDegX(); float readRotationDegY(); float readRotationDegZ(); @@ -29,6 +30,9 @@ class L3G4200D : public BaseIMU { float readRotationRadZ(); void readRotationDegXYZ(float& gx, float& gy, float& gz); void readRotationRadXYZ(float& gx, float& gy, float& gz); + +private: + float _scalingFactor; }; #endif // __GYROSCOPE_H__ From 68f8a4bc693c3fa55df2ffaedc3aa83e69f57a7e Mon Sep 17 00:00:00 2001 From: igor605ds Date: Mon, 26 Oct 2020 16:52:08 +0300 Subject: [PATCH 10/20] rename mems-chips class --- src/Accelerometer.cpp | 24 ++++++++++++------------ src/Accelerometer.h | 4 ++-- src/Barometer.cpp | 22 +++++++++++----------- src/Barometer.h | 4 ++-- src/Compass.cpp | 24 ++++++++++++------------ src/Compass.h | 4 ++-- src/Gyroscope.cpp | 24 ++++++++++++------------ src/Gyroscope.h | 4 ++-- src/TroykaIMU.h | 24 ------------------------ 9 files changed, 55 insertions(+), 79 deletions(-) diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index 5bbd41b..8fd50c1 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -1,9 +1,9 @@ #include "Accelerometer.h" -LIS331DLH::LIS331DLH(uint8_t slaveAddress) +Accelerometer::Accelerometer(uint8_t slaveAddress) : BaseIMU(slaveAddress) { } -void LIS331DLH::begin(TwoWire& wire) { +void Accelerometer::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); _ctrlReg1 |= (1 << 0); @@ -14,7 +14,7 @@ void LIS331DLH::begin(TwoWire& wire) { setRange(AccelerometerRange::RANGE_2G); } -void LIS331DLH::setRange(AccelerometerRange range) { +void Accelerometer::setRange(AccelerometerRange range) { switch (range) { case AccelerometerRange::RANGE_2G: { _ctrlReg4 = 0; @@ -38,7 +38,7 @@ void LIS331DLH::setRange(AccelerometerRange range) { _writeByte(CTRL_REG4, _ctrlReg4); } -void LIS331DLH::sleep(bool state) { +void Accelerometer::sleep(bool state) { if (state) _ctrlReg1 &= ~(1 << 5); else @@ -47,25 +47,25 @@ void LIS331DLH::sleep(bool state) { _writeByte(CTRL_REG1, _ctrlReg1); } -float LIS331DLH::readAccelerationGX() { return readX() * _scalingFactor; } +float Accelerometer::readAccelerationGX() { return readX() * _scalingFactor; } -float LIS331DLH::readAccelerationGY() { return readY() * _scalingFactor; } +float Accelerometer::readAccelerationGY() { return readY() * _scalingFactor; } -float LIS331DLH::readAccelerationGZ() { return readZ() * _scalingFactor; } +float Accelerometer::readAccelerationGZ() { return readZ() * _scalingFactor; } -float LIS331DLH::readAccelerationAX() { +float Accelerometer::readAccelerationAX() { return readAccelerationGX() * GRAVITY_EARTH; } -float LIS331DLH::readAccelerationAY() { +float Accelerometer::readAccelerationAY() { return readAccelerationGY() * GRAVITY_EARTH; } -float LIS331DLH::readAccelerationAZ() { +float Accelerometer::readAccelerationAZ() { return readAccelerationGZ() * GRAVITY_EARTH; } -void LIS331DLH::readAccelerationGXYZ(float& ax, float& ay, float& az) { +void Accelerometer::readAccelerationGXYZ(float& ax, float& ay, float& az) { int16_t x, y, z; readXYZ(x, y, z); ax = x * _scalingFactor; @@ -73,7 +73,7 @@ void LIS331DLH::readAccelerationGXYZ(float& ax, float& ay, float& az) { az = z * _scalingFactor; } -void LIS331DLH::readAccelerationAXYZ(float& ax, float& ay, float& az) { +void Accelerometer::readAccelerationAXYZ(float& ax, float& ay, float& az) { readAccelerationGXYZ(ax, ay, az); ax *= GRAVITY_EARTH; ay *= GRAVITY_EARTH; diff --git a/src/Accelerometer.h b/src/Accelerometer.h index 8251339..c4a2122 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -14,9 +14,9 @@ constexpr float SENS_2G = 1; constexpr float SENS_4G = 2; constexpr float SENS_8G = 3.9; -class LIS331DLH : public BaseIMU { +class Accelerometer : public BaseIMU { public: - LIS331DLH(uint8_t slaveAddress); + Accelerometer(uint8_t slaveAddress = 0x18); enum class ScaleRange; void begin(TwoWire& wire = Wire); void sleep(bool state); diff --git a/src/Barometer.cpp b/src/Barometer.cpp index 2a07167..1ceb193 100644 --- a/src/Barometer.cpp +++ b/src/Barometer.cpp @@ -1,10 +1,10 @@ #include #include -LPS331::LPS331(uint8_t slaveAddress) +Barometer::Barometer(uint8_t slaveAddress) : BaseIMU(slaveAddress) { } -void LPS331::begin(TwoWire& wire) { +void Barometer::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); _ctrlReg1 |= (1 << 7); @@ -12,43 +12,43 @@ void LPS331::begin(TwoWire& wire) { _writeByte(CTRL_REG1, _ctrlReg1); } -float LPS331::readPressureMillibars() { +float Barometer::readPressureMillibars() { return static_cast(_readPressureRaw()) / 4096; } -float LPS331::readPressurePascals() { +float Barometer::readPressurePascals() { return readPressureMillibars() * LPS331_MILLIBARS_TO_PASCALS; } -float LPS331::readPressureMillimetersHg() { +float Barometer::readPressureMillimetersHg() { return readPressureMillibars() * LPS331_MILLIBARS_TO_MILLIMETERSHG; } // Temperature output data from datasheet -float LPS331::readTemperatureC() { +float Barometer::readTemperatureC() { return 42.5 + static_cast(_readTemperatureRaw()) / 480; } // Convert Celsius scale to Kelvin -float LPS331::readTemperatureK() { +float Barometer::readTemperatureK() { return readTemperatureC() + LPS331_CELSIUS_TO_KELVIN; } // Convert Celsius scale to Fahrenheit -float LPS331::readTemperatureF() { return readTemperatureC() * 1.8 + 32; } +float Barometer::readTemperatureF() { return readTemperatureC() * 1.8 + 32; } -uint32_t LPS331::_readPressureRaw() { +uint32_t Barometer::_readPressureRaw() { uint8_t data[3]; _readBytes(0x80 | LPS331_PRESS_OUT_XL, data, 3); return (uint32_t)data[2] << 16 | (uint16_t)data[1] << 8 | data[0]; } -int16_t LPS331::_readTemperatureRaw() { +int16_t Barometer::_readTemperatureRaw() { return ((int16_t)_readByte(LPS331_TEMP_OUT_H) << 8) | _readByte(LPS331_TEMP_OUT_L); } -float LPS331::readAltitude() { +float Barometer::readAltitude() { float pressurePascals = readPressurePascals(); return GOST4401_getAltitude(pressurePascals); } diff --git a/src/Barometer.h b/src/Barometer.h index 59dc05a..6d7207a 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -13,9 +13,9 @@ constexpr auto LPS331_CELSIUS_TO_KELVIN = 273.15; constexpr auto LPS331_MILLIBARS_TO_PASCALS = 100; constexpr auto LPS331_MILLIBARS_TO_MILLIMETERSHG = 0.75; -class LPS331 : public BaseIMU { +class Barometer : public BaseIMU { public: - LPS331(uint8_t slaveAddress); + Barometer(uint8_t slaveAddress = 0x5C); void begin(TwoWire& wire = Wire); float readPressurePascals(); float readPressureMillibars(); diff --git a/src/Compass.cpp b/src/Compass.cpp index 5b37878..4a45b13 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -1,16 +1,16 @@ #include "Compass.h" -LIS3MDL::LIS3MDL(uint8_t slaveAddress) +Compass::Compass(uint8_t slaveAddress) : BaseIMU(slaveAddress) { } -void LIS3MDL::begin(TwoWire& wire) { +void Compass::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); _writeByte(CTRL_REG3, _ctrlReg3); setRange(CompassRange::RANGE_16GAUSS); } -void LIS3MDL::setRange(CompassRange range) { +void Compass::setRange(CompassRange range) { switch (range) { case CompassRange::RANGE_4GAUSS: { _ctrlReg2 = 0; @@ -39,7 +39,7 @@ void LIS3MDL::setRange(CompassRange range) { _writeByte(CTRL_REG2, _ctrlReg2); } -void LIS3MDL::sleep(bool state) { +void Compass::sleep(bool state) { if (state) _ctrlReg3 |= (3 << 0); else @@ -48,13 +48,13 @@ void LIS3MDL::sleep(bool state) { _writeByte(CTRL_REG3, _ctrlReg3); } -float LIS3MDL::readMagneticGaussX() { return readX() / _scalingFactor; } +float Compass::readMagneticGaussX() { return readX() / _scalingFactor; } -float LIS3MDL::readMagneticGaussY() { return readY() / _scalingFactor; } +float Compass::readMagneticGaussY() { return readY() / _scalingFactor; } -float LIS3MDL::readMagneticGaussZ() { return readZ() / _scalingFactor; } +float Compass::readMagneticGaussZ() { return readZ() / _scalingFactor; } -void LIS3MDL::readMagneticGaussXYZ(float& mx, float& my, float& mz) { +void Compass::readMagneticGaussXYZ(float& mx, float& my, float& mz) { int16_t x, y, z; readXYZ(x, y, z); mx = x / _scalingFactor; @@ -62,7 +62,7 @@ void LIS3MDL::readMagneticGaussXYZ(float& mx, float& my, float& mz) { mz = z / _scalingFactor; } -void LIS3MDL::readCalibrateMagneticGaussXYZ(float& mx, float& my, float& mz) { +void Compass::readCalibrateMagneticGaussXYZ(float& mx, float& my, float& mz) { int16_t x, y, z; readXYZ(x, y, z); mx = x, my = y, mz = z; @@ -72,13 +72,13 @@ void LIS3MDL::readCalibrateMagneticGaussXYZ(float& mx, float& my, float& mz) { mz = z / _scalingFactor; } -void LIS3MDL::setCalibrateMatrix(const float calibrationMatrix[3][3], +void Compass::setCalibrateMatrix(const float calibrationMatrix[3][3], const float calibrationBias[3]) { memcpy(_calibrationBias, calibrationBias, 3 * sizeof(float)); memcpy(_calibrationMatrix, calibrationMatrix, 3 * 3 * sizeof(float)); } -void LIS3MDL::_calibrate(float& x, float& y, float& z) { +void Compass::_calibrate(float& x, float& y, float& z) { float calibrationValues[3] = { 0, 0, 0 }; float nonCalibrationValues[3] = { 0, 0, 0 }; nonCalibrationValues[0] = x - _calibrationBias[0]; @@ -97,7 +97,7 @@ void LIS3MDL::_calibrate(float& x, float& y, float& z) { z = calibrationValues[2]; } -float LIS3MDL::readAzimut() { +float Compass::readAzimut() { int16_t x, y, z; readXYZ(x, y, z); float mx = x, my = y, mz = z; diff --git a/src/Compass.h b/src/Compass.h index c620591..c42becf 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -18,9 +18,9 @@ constexpr float SENS_8GAUSS = 3421; constexpr float SENS_12GAUSS = 2281; constexpr float SENS_16GAUSS = 1711; -class LIS3MDL : public BaseIMU { +class Compass : public BaseIMU { public: - LIS3MDL(uint8_t slaveAddress); + Compass(uint8_t slaveAddress = 0x1C); void begin(TwoWire& wire = Wire); void sleep(bool state); void setRange(CompassRange range); diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp index ffcc94e..cd68c1a 100644 --- a/src/Gyroscope.cpp +++ b/src/Gyroscope.cpp @@ -1,9 +1,9 @@ #include "Gyroscope.h" -L3G4200D::L3G4200D(uint8_t slaveAddress) +Gyroscope::Gyroscope(uint8_t slaveAddress) : BaseIMU(slaveAddress) { } -void L3G4200D::begin(TwoWire& wire) { +void Gyroscope::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); _ctrlReg1 |= (1 << 0) | (1 << 2) | (1 << 3) | (1 << 4); @@ -11,7 +11,7 @@ void L3G4200D::begin(TwoWire& wire) { setRange(GyroscopeRange::RANGE_2000DPS); } -void L3G4200D::setRange(GyroscopeRange range) { +void Gyroscope::setRange(GyroscopeRange range) { switch (range) { case GyroscopeRange::RANGE_250DPS: { _ctrlReg4 = 0; @@ -35,7 +35,7 @@ void L3G4200D::setRange(GyroscopeRange range) { _writeByte(CTRL_REG4, _ctrlReg4); } -void L3G4200D::sleep(bool state) { +void Gyroscope::sleep(bool state) { if (state) { _ctrlReg1 &= ~(1 << 3); } else { @@ -44,19 +44,19 @@ void L3G4200D::sleep(bool state) { _writeByte(CTRL_REG1, _ctrlReg1); } -float L3G4200D::readRotationDegX() { return readX() * _scalingFactor; } +float Gyroscope::readRotationDegX() { return readX() * _scalingFactor; } -float L3G4200D::readRotationDegY() { return readY() * _scalingFactor; } +float Gyroscope::readRotationDegY() { return readY() * _scalingFactor; } -float L3G4200D::readRotationDegZ() { return readZ() * _scalingFactor; } +float Gyroscope::readRotationDegZ() { return readZ() * _scalingFactor; } -float L3G4200D::readRotationRadX() { return readRotationDegX() * DEG_TO_RAD; } +float Gyroscope::readRotationRadX() { return readRotationDegX() * DEG_TO_RAD; } -float L3G4200D::readRotationRadY() { return readRotationDegY() * DEG_TO_RAD; } +float Gyroscope::readRotationRadY() { return readRotationDegY() * DEG_TO_RAD; } -float L3G4200D::readRotationRadZ() { return readRotationDegZ() * DEG_TO_RAD; } +float Gyroscope::readRotationRadZ() { return readRotationDegZ() * DEG_TO_RAD; } -void L3G4200D::readRotationDegXYZ(float& gx, float& gy, float& gz) { +void Gyroscope::readRotationDegXYZ(float& gx, float& gy, float& gz) { int16_t x, y, z; readXYZ(x, y, z); gx = x * _scale; @@ -64,7 +64,7 @@ void L3G4200D::readRotationDegXYZ(float& gx, float& gy, float& gz) { gz = z * _scale; } -void L3G4200D::readRotationRadXYZ(float& gx, float& gy, float& gz) { +void Gyroscope::readRotationRadXYZ(float& gx, float& gy, float& gz) { readRotationDegXYZ(gx, gy, gz); gx *= DEG_TO_RAD; gy *= DEG_TO_RAD; diff --git a/src/Gyroscope.h b/src/Gyroscope.h index f4d1946..f919778 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -16,9 +16,9 @@ constexpr float SENS_250DPS = 0.00875; constexpr float SENS_500DPS = 0.0175; constexpr float SENS_2000DPS = 0.07; -class L3G4200D : public BaseIMU { +class Gyroscope : public BaseIMU { public: - L3G4200D(uint8_t slaveAddress); + Gyroscope(uint8_t slaveAddress = 0x68); void begin(TwoWire& wire = Wire); void sleep(bool state); void setRange(GyroscopeRange range); diff --git a/src/TroykaIMU.h b/src/TroykaIMU.h index 546b1db..94f444f 100644 --- a/src/TroykaIMU.h +++ b/src/TroykaIMU.h @@ -7,28 +7,4 @@ #include "Gyroscope.h" #include "MadgwickAHRS.h" -class Accelerometer : public LIS331DLH { -public: - Accelerometer(uint8_t slaveAddress = 0x18) - : LIS331DLH(slaveAddress) { } -}; - -class Gyroscope : public L3G4200D { -public: - Gyroscope(uint8_t slaveAddress = 0x68) - : L3G4200D(slaveAddress) { } -}; - -class Compass : public LIS3MDL { -public: - Compass(uint8_t slaveAddress = 0x1C) - : LIS3MDL(slaveAddress) { } -}; - -class Barometer : public LPS331 { -public: - Barometer(uint8_t slaveAddress = 0x5C) - : LPS331(slaveAddress) { } -}; - #endif // __TROYKA_IMU_H__ \ No newline at end of file From a17024189c4bfacea27fad48eded01d1d503ecc3 Mon Sep 17 00:00:00 2001 From: igor605ds Date: Mon, 26 Oct 2020 20:29:45 +0300 Subject: [PATCH 11/20] add support LPS25HB and assigned names to registers --- src/Accelerometer.cpp | 18 +++++++++--------- src/Accelerometer.h | 24 ++++++++++++++++-------- src/Barometer.cpp | 23 ++++++++++++++--------- src/Barometer.h | 28 ++++++++++++++++++++-------- src/BaseIMU.cpp | 12 ++++++++---- src/BaseIMU.h | 33 ++++++++++++++++++--------------- src/Compass.cpp | 10 +++++----- src/Compass.h | 20 +++++++++++++------- src/Gyroscope.cpp | 21 ++++++++++++--------- src/Gyroscope.h | 20 ++++++++++++++------ 10 files changed, 129 insertions(+), 80 deletions(-) diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index 8fd50c1..e06214f 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -6,14 +6,14 @@ Accelerometer::Accelerometer(uint8_t slaveAddress) void Accelerometer::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - _ctrlReg1 |= (1 << 0); - _ctrlReg1 |= (1 << 1); - _ctrlReg1 |= (1 << 2); - _ctrlReg1 |= (1 << 5); - _writeByte(CTRL_REG1, _ctrlReg1); + _ctrlReg1 |= LIS331DLH_CTRL_REG1_X_EN | LIS331DLH_CTRL_REG1_Y_EN + | LIS331DLH_CTRL_REG1_Z_EN; + _ctrlReg1 |= LIS331DLH_CTRL_REG1_PM0; + _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); setRange(AccelerometerRange::RANGE_2G); } +// Set range scale output data from datasheet void Accelerometer::setRange(AccelerometerRange range) { switch (range) { case AccelerometerRange::RANGE_2G: { @@ -35,16 +35,16 @@ void Accelerometer::setRange(AccelerometerRange range) { _scalingFactor = SENS_2G * 4 / pow(2, 16); } break; } - _writeByte(CTRL_REG4, _ctrlReg4); + _writeByte(BASE_IMU_CTRL_REG4, _ctrlReg4); } void Accelerometer::sleep(bool state) { if (state) - _ctrlReg1 &= ~(1 << 5); + _ctrlReg1 &= ~LIS331DLH_CTRL_REG1_PM0; else - _ctrlReg1 |= (1 << 5); + _ctrlReg1 |= LIS331DLH_CTRL_REG1_PM0; - _writeByte(CTRL_REG1, _ctrlReg1); + _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); } float Accelerometer::readAccelerationGX() { return readX() * _scalingFactor; } diff --git a/src/Accelerometer.h b/src/Accelerometer.h index c4a2122..c8b24b9 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -3,21 +3,29 @@ #include "BaseIMU.h" -constexpr auto LIS331DLH_CTRL_REG4_FS0 = 0x10; -constexpr auto LIS331DLH_CTRL_REG4_FS1 = 0x20; - -constexpr auto GRAVITY_EARTH = 9.8; - -enum class AccelerometerRange { RANGE_2G = 2, RANGE_4G = 4, RANGE_8G = 8 }; - +// Registers addrress +constexpr uint8_t LIS331DLH_CTRL_REG4_FS0 = 0x10; +constexpr uint8_t LIS331DLH_CTRL_REG4_FS1 = 0x20; + +// Registers value bits +constexpr uint8_t LIS331DLH_CTRL_REG1_X_EN = 0x01; +constexpr uint8_t LIS331DLH_CTRL_REG1_Y_EN = 0x02; +constexpr uint8_t LIS331DLH_CTRL_REG1_Z_EN = 0x04; +constexpr uint8_t LIS331DLH_CTRL_REG1_PM0 = 0x20; +constexpr uint8_t LIS331DLH_CTRL_REG1_PM1 = 0x40; +constexpr uint8_t LIS331DLH_CTRL_REG1_PM2 = 0x80; + +// Constans +constexpr float GRAVITY_EARTH = 9.8; constexpr float SENS_2G = 1; constexpr float SENS_4G = 2; constexpr float SENS_8G = 3.9; +enum class AccelerometerRange { RANGE_2G = 1, RANGE_4G = 2, RANGE_8G = 3 }; + class Accelerometer : public BaseIMU { public: Accelerometer(uint8_t slaveAddress = 0x18); - enum class ScaleRange; void begin(TwoWire& wire = Wire); void sleep(bool state); void setRange(AccelerometerRange range); diff --git a/src/Barometer.cpp b/src/Barometer.cpp index 1ceb193..9fac8a3 100644 --- a/src/Barometer.cpp +++ b/src/Barometer.cpp @@ -7,9 +7,14 @@ Barometer::Barometer(uint8_t slaveAddress) void Barometer::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - _ctrlReg1 |= (1 << 7); - _ctrlReg1 |= (1 << 6); - _writeByte(CTRL_REG1, _ctrlReg1); + _deviceID = readDeviceID(); + if (_deviceID == LPS331_WHO_AM_I) { + _ctrlReg1 |= LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR0; + } else if (_deviceID == LPS25HB_WHO_AM_I) { + _ctrlReg1 |= LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR0; + } + _ctrlReg1 |= LPS_CTRL_REG1_PD; + _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); } float Barometer::readPressureMillibars() { @@ -17,11 +22,11 @@ float Barometer::readPressureMillibars() { } float Barometer::readPressurePascals() { - return readPressureMillibars() * LPS331_MILLIBARS_TO_PASCALS; + return readPressureMillibars() * MILLIBARS_TO_PASCALS; } float Barometer::readPressureMillimetersHg() { - return readPressureMillibars() * LPS331_MILLIBARS_TO_MILLIMETERSHG; + return readPressureMillibars() * MILLIBARS_TO_MILLIMETERSHG; } // Temperature output data from datasheet @@ -31,7 +36,7 @@ float Barometer::readTemperatureC() { // Convert Celsius scale to Kelvin float Barometer::readTemperatureK() { - return readTemperatureC() + LPS331_CELSIUS_TO_KELVIN; + return readTemperatureC() + CELSIUS_TO_KELVIN; } // Convert Celsius scale to Fahrenheit @@ -39,13 +44,13 @@ float Barometer::readTemperatureF() { return readTemperatureC() * 1.8 + 32; } uint32_t Barometer::_readPressureRaw() { uint8_t data[3]; - _readBytes(0x80 | LPS331_PRESS_OUT_XL, data, 3); + _readBytes(0x80 | LPS_PRESS_OUT_XL, data, 3); return (uint32_t)data[2] << 16 | (uint16_t)data[1] << 8 | data[0]; } int16_t Barometer::_readTemperatureRaw() { - return ((int16_t)_readByte(LPS331_TEMP_OUT_H) << 8) - | _readByte(LPS331_TEMP_OUT_L); + return ((int16_t)_readByte(LPS_TEMP_OUT_H) << 8) + | _readByte(LPS_TEMP_OUT_L); } float Barometer::readAltitude() { diff --git a/src/Barometer.h b/src/Barometer.h index 6d7207a..e66e174 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -3,15 +3,26 @@ #include "BaseIMU.h" -constexpr auto LPS331_PRESS_OUT_XL = 0x28; -constexpr auto LPS331_PRESS_OUT_L = 0x29; -constexpr auto LPS331_PRESS_OUT_H = 0x2A; -constexpr auto LPS331_TEMP_OUT_L = 0x2B; -constexpr auto LPS331_TEMP_OUT_H = 0x2C; +// Registers address LPS25HB and LPS331 +constexpr uint8_t LPS_CTRL_REG1_ODR0 = 0x10; +constexpr uint8_t LPS_CTRL_REG1_ODR1 = 0x20; +constexpr uint8_t LPS_CTRL_REG1_ODR2 = 0x40; +constexpr uint8_t LPS_CTRL_REG1_PD = 0x80; -constexpr auto LPS331_CELSIUS_TO_KELVIN = 273.15; -constexpr auto LPS331_MILLIBARS_TO_PASCALS = 100; -constexpr auto LPS331_MILLIBARS_TO_MILLIMETERSHG = 0.75; +constexpr uint8_t LPS_PRESS_OUT_XL = 0x28; +constexpr uint8_t LPS_PRESS_OUT_L = 0x29; +constexpr uint8_t LPS_PRESS_OUT_H = 0x2A; +constexpr uint8_t LPS_TEMP_OUT_L = 0x2B; +constexpr uint8_t LPS_TEMP_OUT_H = 0x2C; + +// Registers value byte +constexpr uint8_t LPS331_WHO_AM_I = 0xBB; +constexpr uint8_t LPS25HB_WHO_AM_I = 0xBD; + +// Constans +constexpr float CELSIUS_TO_KELVIN = 273.15; +constexpr float MILLIBARS_TO_PASCALS = 100; +constexpr float MILLIBARS_TO_MILLIMETERSHG = 0.75; class Barometer : public BaseIMU { public: @@ -28,6 +39,7 @@ class Barometer : public BaseIMU { private: uint32_t _readPressureRaw(); int16_t _readTemperatureRaw(); + uint8_t _deviceID; }; #endif // __BAROMETER_H__ diff --git a/src/BaseIMU.cpp b/src/BaseIMU.cpp index b28c0eb..0732a89 100644 --- a/src/BaseIMU.cpp +++ b/src/BaseIMU.cpp @@ -1,20 +1,24 @@ #include "BaseIMU.h" +uint8_t BaseIMU::readDeviceID(){ + return _readByte(BASE_IMU_WHO_AM_I); +} + int16_t BaseIMU::readX() { - return ((int16_t)_readByte(OUT_X_H) << 8) | _readByte(OUT_X_L); + return ((int16_t)_readByte(BASE_IMU_OUT_X_H) << 8) | _readByte(BASE_IMU_OUT_X_L); } int16_t BaseIMU::readY() { - return ((int16_t)_readByte(OUT_Y_H) << 8) | _readByte(OUT_Y_L); + return ((int16_t)_readByte(BASE_IMU_OUT_Y_H) << 8) | _readByte(BASE_IMU_OUT_Y_L); } int16_t BaseIMU::readZ() { - return ((int16_t)_readByte(OUT_Z_H) << 8) | _readByte(OUT_Z_L); + return ((int16_t)_readByte(BASE_IMU_OUT_Z_H) << 8) | _readByte(BASE_IMU_OUT_Z_L); } void BaseIMU::readXYZ(int16_t& x, int16_t& y, int16_t& z) { uint8_t data[6]; - _readBytes(0x80 | OUT_X_L, data, 6); + _readBytes(0x80 | BASE_IMU_OUT_X_L, data, 6); x = ((int16_t)data[1] << 8) | data[0]; y = ((int16_t)data[3] << 8) | data[2]; z = ((int16_t)data[5] << 8) | data[4]; diff --git a/src/BaseIMU.h b/src/BaseIMU.h index eb7531b..72cf29d 100644 --- a/src/BaseIMU.h +++ b/src/BaseIMU.h @@ -1,27 +1,31 @@ -#ifndef __BaseIMU_H__ -#define __BaseIMU_H__ +#ifndef __BASE_IMU_H__ +#define __BASE_IMU_H__ #include #include -constexpr auto CTRL_REG1 = 0x20; -constexpr auto CTRL_REG2 = 0x21; -constexpr auto CTRL_REG3 = 0x22; -constexpr auto CTRL_REG4 = 0x23; -constexpr auto CTRL_REG5 = 0x24; +// Registers addrress +constexpr uint8_t BASE_IMU_WHO_AM_I = 0x0F; +constexpr uint8_t BASE_IMU_CTRL_REG1 = 0x20; +constexpr uint8_t BASE_IMU_CTRL_REG2 = 0x21; +constexpr uint8_t BASE_IMU_CTRL_REG3 = 0x22; +constexpr uint8_t BASE_IMU_CTRL_REG4 = 0x23; +constexpr uint8_t BASE_IMU_CTRL_REG5 = 0x24; -constexpr auto OUT_X_L = 0x28; -constexpr auto OUT_X_H = 0x29; -constexpr auto OUT_Y_L = 0x2A; -constexpr auto OUT_Y_H = 0x2B; -constexpr auto OUT_Z_L = 0x2C; -constexpr auto OUT_Z_H = 0x2D; +// Registers value bits +constexpr uint8_t BASE_IMU_OUT_X_L = 0x28; +constexpr uint8_t BASE_IMU_OUT_X_H = 0x29; +constexpr uint8_t BASE_IMU_OUT_Y_L = 0x2A; +constexpr uint8_t BASE_IMU_OUT_Y_H = 0x2B; +constexpr uint8_t BASE_IMU_OUT_Z_L = 0x2C; +constexpr uint8_t BASE_IMU_OUT_Z_H = 0x2D; class BaseIMU { public: BaseIMU(uint8_t slaveAddress) : _slaveAddress(slaveAddress) { } void begin(TwoWire& wire = Wire); + uint8_t readDeviceID(); int16_t readX(); int16_t readY(); int16_t readZ(); @@ -36,11 +40,10 @@ class BaseIMU { uint8_t _ctrlReg3; uint8_t _ctrlReg4; uint8_t _ctrlReg5; - float _scale; TwoWire* _wire; private: uint8_t _slaveAddress; }; -#endif // __BaseIMU_H__ +#endif // __BASE_IMU_H__ diff --git a/src/Compass.cpp b/src/Compass.cpp index 4a45b13..e12a1c9 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -6,7 +6,7 @@ Compass::Compass(uint8_t slaveAddress) void Compass::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - _writeByte(CTRL_REG3, _ctrlReg3); + _writeByte(BASE_IMU_CTRL_REG3, _ctrlReg3); setRange(CompassRange::RANGE_16GAUSS); } @@ -36,16 +36,16 @@ void Compass::setRange(CompassRange range) { _scalingFactor = SENS_4GAUSS; } break; } - _writeByte(CTRL_REG2, _ctrlReg2); + _writeByte(BASE_IMU_CTRL_REG2, _ctrlReg2); } void Compass::sleep(bool state) { if (state) - _ctrlReg3 |= (3 << 0); + _ctrlReg3 |= LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1; else - _ctrlReg3 &= ~(3 << 0); + _ctrlReg3 &= ~(LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1); - _writeByte(CTRL_REG3, _ctrlReg3); + _writeByte(BASE_IMU_CTRL_REG3, _ctrlReg3); } float Compass::readMagneticGaussX() { return readX() / _scalingFactor; } diff --git a/src/Compass.h b/src/Compass.h index c42becf..abda0da 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -3,8 +3,19 @@ #include "BaseIMU.h" -constexpr auto LIS3MDL_CTRL_REG2_FS0 = 0x20; -constexpr auto LIS3MDL_CTRL_REG2_FS1 = 0x40; +// Registers addrress +constexpr uint8_t LIS3MDL_CTRL_REG2_FS0 = 0x20; +constexpr uint8_t LIS3MDL_CTRL_REG2_FS1 = 0x40; + +// Registers value bits +constexpr uint8_t LIS3MDL_CTRL_REG3_MD0 = 0x00; +constexpr uint8_t LIS3MDL_CTRL_REG3_MD1 = 0x02; + +// Constans +constexpr float SENS_4GAUSS = 6842; +constexpr float SENS_8GAUSS = 3421; +constexpr float SENS_12GAUSS = 2281; +constexpr float SENS_16GAUSS = 1711; enum class CompassRange { RANGE_4GAUSS = 1, @@ -13,11 +24,6 @@ enum class CompassRange { RANGE_16GAUSS = 4 }; -constexpr float SENS_4GAUSS = 6842; -constexpr float SENS_8GAUSS = 3421; -constexpr float SENS_12GAUSS = 2281; -constexpr float SENS_16GAUSS = 1711; - class Compass : public BaseIMU { public: Compass(uint8_t slaveAddress = 0x1C); diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp index cd68c1a..39873b6 100644 --- a/src/Gyroscope.cpp +++ b/src/Gyroscope.cpp @@ -6,11 +6,14 @@ Gyroscope::Gyroscope(uint8_t slaveAddress) void Gyroscope::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - _ctrlReg1 |= (1 << 0) | (1 << 2) | (1 << 3) | (1 << 4); - _writeByte(CTRL_REG1, _ctrlReg1); + _ctrlReg1 |= L3G4200D_CTRL_REG1_X_EN | L3G4200D_CTRL_REG1_Y_EN + | L3G4200D_CTRL_REG1_Z_EN; + _ctrlReg1 |= L3G4200D_CTRL_REG1_PD; + _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); setRange(GyroscopeRange::RANGE_2000DPS); } +// Set range scale output data from datasheet void Gyroscope::setRange(GyroscopeRange range) { switch (range) { case GyroscopeRange::RANGE_250DPS: { @@ -32,16 +35,16 @@ void Gyroscope::setRange(GyroscopeRange range) { _scalingFactor = SENS_250DPS; } break; } - _writeByte(CTRL_REG4, _ctrlReg4); + _writeByte(BASE_IMU_CTRL_REG4, _ctrlReg4); } void Gyroscope::sleep(bool state) { if (state) { - _ctrlReg1 &= ~(1 << 3); + _ctrlReg1 &= ~L3G4200D_CTRL_REG1_PD; } else { - _ctrlReg1 |= (1 << 3); + _ctrlReg1 |= L3G4200D_CTRL_REG1_PD; } - _writeByte(CTRL_REG1, _ctrlReg1); + _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); } float Gyroscope::readRotationDegX() { return readX() * _scalingFactor; } @@ -59,9 +62,9 @@ float Gyroscope::readRotationRadZ() { return readRotationDegZ() * DEG_TO_RAD; } void Gyroscope::readRotationDegXYZ(float& gx, float& gy, float& gz) { int16_t x, y, z; readXYZ(x, y, z); - gx = x * _scale; - gy = y * _scale; - gz = z * _scale; + gx = x * _scalingFactor; + gy = y * _scalingFactor; + gz = z * _scalingFactor; } void Gyroscope::readRotationRadXYZ(float& gx, float& gy, float& gz) { diff --git a/src/Gyroscope.h b/src/Gyroscope.h index f919778..3c9b9b1 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -3,8 +3,20 @@ #include "BaseIMU.h" -constexpr auto L3G4200D_CTRL_REG4_FS0 = 0x10; -constexpr auto L3G4200D_CTRL_REG4_FS1 = 0x20; +// Registers address +constexpr uint8_t L3G4200D_CTRL_REG4_FS0 = 0x10; +constexpr uint8_t L3G4200D_CTRL_REG4_FS1 = 0x20; + +// Registers value bits +constexpr uint8_t L3G4200D_CTRL_REG1_X_EN = 0x01; +constexpr uint8_t L3G4200D_CTRL_REG1_Y_EN = 0x02; +constexpr uint8_t L3G4200D_CTRL_REG1_Z_EN = 0x04; +constexpr uint8_t L3G4200D_CTRL_REG1_PD = 0x08; + +// Constans +constexpr float SENS_250DPS = 0.00875; +constexpr float SENS_500DPS = 0.0175; +constexpr float SENS_2000DPS = 0.07; enum class GyroscopeRange { RANGE_250DPS = 1, @@ -12,10 +24,6 @@ enum class GyroscopeRange { RANGE_2000DPS = 3 }; -constexpr float SENS_250DPS = 0.00875; -constexpr float SENS_500DPS = 0.0175; -constexpr float SENS_2000DPS = 0.07; - class Gyroscope : public BaseIMU { public: Gyroscope(uint8_t slaveAddress = 0x68); From 89f543c1326485be9926265083a5dcd39e2b13c0 Mon Sep 17 00:00:00 2001 From: igor605ds Date: Mon, 26 Oct 2020 21:07:27 +0300 Subject: [PATCH 12/20] define I2C addresses for modules --- src/Accelerometer.h | 6 +++++- src/Barometer.h | 6 +++++- src/BaseIMU.h | 1 - src/Compass.h | 6 +++++- src/Gyroscope.h | 6 +++++- 5 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/Accelerometer.h b/src/Accelerometer.h index c8b24b9..ceadf41 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -3,6 +3,10 @@ #include "BaseIMU.h" +// I²C-address device +constexpr uint8_t LIS331DLH_SLAVE_ADDRESS = 0x18; +constexpr uint8_t LIS331DLH_SLAVE_ADDRESS_ALT = 0x19; + // Registers addrress constexpr uint8_t LIS331DLH_CTRL_REG4_FS0 = 0x10; constexpr uint8_t LIS331DLH_CTRL_REG4_FS1 = 0x20; @@ -25,7 +29,7 @@ enum class AccelerometerRange { RANGE_2G = 1, RANGE_4G = 2, RANGE_8G = 3 }; class Accelerometer : public BaseIMU { public: - Accelerometer(uint8_t slaveAddress = 0x18); + Accelerometer(uint8_t slaveAddress = LIS331DLH_SLAVE_ADDRESS); void begin(TwoWire& wire = Wire); void sleep(bool state); void setRange(AccelerometerRange range); diff --git a/src/Barometer.h b/src/Barometer.h index e66e174..d3241de 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -3,6 +3,10 @@ #include "BaseIMU.h" +// I²C-address devices LPS25HB and LPS331 +constexpr uint8_t LPS_SLAVE_ADDRESS = 0x1C; +constexpr uint8_t LPS_SLAVE_ADDRESS_ALT = 0x1D; + // Registers address LPS25HB and LPS331 constexpr uint8_t LPS_CTRL_REG1_ODR0 = 0x10; constexpr uint8_t LPS_CTRL_REG1_ODR1 = 0x20; @@ -26,7 +30,7 @@ constexpr float MILLIBARS_TO_MILLIMETERSHG = 0.75; class Barometer : public BaseIMU { public: - Barometer(uint8_t slaveAddress = 0x5C); + Barometer(uint8_t slaveAddress = LPS_SLAVE_ADDRESS); void begin(TwoWire& wire = Wire); float readPressurePascals(); float readPressureMillibars(); diff --git a/src/BaseIMU.h b/src/BaseIMU.h index 72cf29d..82663a2 100644 --- a/src/BaseIMU.h +++ b/src/BaseIMU.h @@ -12,7 +12,6 @@ constexpr uint8_t BASE_IMU_CTRL_REG3 = 0x22; constexpr uint8_t BASE_IMU_CTRL_REG4 = 0x23; constexpr uint8_t BASE_IMU_CTRL_REG5 = 0x24; -// Registers value bits constexpr uint8_t BASE_IMU_OUT_X_L = 0x28; constexpr uint8_t BASE_IMU_OUT_X_H = 0x29; constexpr uint8_t BASE_IMU_OUT_Y_L = 0x2A; diff --git a/src/Compass.h b/src/Compass.h index abda0da..8a18c28 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -3,6 +3,10 @@ #include "BaseIMU.h" +// I²C-address device +constexpr uint8_t LIS3MDL_SLAVE_ADDRESS = 0x1C; +constexpr uint8_t LIS3MDL_SLAVE_ADDRESS_ALT = 0x1E; + // Registers addrress constexpr uint8_t LIS3MDL_CTRL_REG2_FS0 = 0x20; constexpr uint8_t LIS3MDL_CTRL_REG2_FS1 = 0x40; @@ -26,7 +30,7 @@ enum class CompassRange { class Compass : public BaseIMU { public: - Compass(uint8_t slaveAddress = 0x1C); + Compass(uint8_t slaveAddress = LIS3MDL_SLAVE_ADDRESS); void begin(TwoWire& wire = Wire); void sleep(bool state); void setRange(CompassRange range); diff --git a/src/Gyroscope.h b/src/Gyroscope.h index 3c9b9b1..090fcef 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -3,6 +3,10 @@ #include "BaseIMU.h" +// I²C-address device +constexpr uint8_t L3G4200D_SLAVE_ADDRESS = 0x68; +constexpr uint8_t L3G4200D_SLAVE_ADDRESS_ALT = 0x69; + // Registers address constexpr uint8_t L3G4200D_CTRL_REG4_FS0 = 0x10; constexpr uint8_t L3G4200D_CTRL_REG4_FS1 = 0x20; @@ -26,7 +30,7 @@ enum class GyroscopeRange { class Gyroscope : public BaseIMU { public: - Gyroscope(uint8_t slaveAddress = 0x68); + Gyroscope(uint8_t slaveAddress = L3G4200D_SLAVE_ADDRESS); void begin(TwoWire& wire = Wire); void sleep(bool state); void setRange(GyroscopeRange range); From b4c7483d8c71d7d1d07adcd1bf8e5a7433f7969d Mon Sep 17 00:00:00 2001 From: igor605ds Date: Tue, 27 Oct 2020 03:48:00 +0300 Subject: [PATCH 13/20] fix filter MadgwickAHRS --- .../Compass/CompassAzimuth/CompassAzimuth.ino | 8 +- examples/IMU/Madgwick6DOF/Madgwick6DOF.ino | 9 +- examples/IMU/Madgwick9DOF/Madgwick9DOF.ino | 18 +-- .../Madgwick6DOFVisualization.ino | 65 ++++++++ .../Madgwick9DOFVisualization.ino | 23 ++- .../MadgwickVisualizationDraw.pde} | 0 src/MadgwickAHRS.cpp | 150 +++++++++--------- src/MadgwickAHRS.h | 31 ++-- 8 files changed, 181 insertions(+), 123 deletions(-) create mode 100644 examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino rename examples/IMU/{Madgwick9DOFVisualization => MadgwickVisualization}/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino (82%) rename examples/IMU/{Madgwick9DOFVisualization/Processing/Madgwick9DOFVisualizationDraw/Madgwick9DOFVisualizationDraw.pde => MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde} (100%) diff --git a/examples/Compass/CompassAzimuth/CompassAzimuth.ino b/examples/Compass/CompassAzimuth/CompassAzimuth.ino index f04ea7a..8e2a669 100644 --- a/examples/Compass/CompassAzimuth/CompassAzimuth.ino +++ b/examples/Compass/CompassAzimuth/CompassAzimuth.ino @@ -6,11 +6,11 @@ Compass compass; // Калибровочные значения, полученные в калибровочной матрице // из примера compassCalibration -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; +const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; +const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, + { 0.049, 1.942, -0.235 }, + { -0.003, 0.008, 1.944} }; void setup() { // Открываем последовательный порт diff --git a/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino index 2009878..fdfe6ba 100644 --- a/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino +++ b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino @@ -1,9 +1,6 @@ // Библиотека для работы с модулями IMU #include -// Множитель фильтра -constexpr float BETA = 0.22; - // Создаём объект для фильтра Madgwick Madgwick filter; // Создаём объект для работы с гироскопом @@ -29,6 +26,8 @@ void setup() { gyroscope.begin(); // Инициализируем акселерометр accelerometer.begin(); + // Инициализируем фильтр + filter.begin(); // Выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } @@ -40,8 +39,8 @@ void loop() { accelerometer.readAccelerationGXYZ(ax, ay, az); // Считываем данные с гироскопа в радианах в секунду gyroscope.readRotationRadXYZ(gx, gy, gz); - // Устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); + // Устанавливаем частоту фильтра + filter.setFrequency(fps); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az); diff --git a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino index dfaa032..b2f63af 100644 --- a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino +++ b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino @@ -1,9 +1,6 @@ // Библиотека для работы с модулями IMU #include -// Множитель фильтра -constexpr float BETA = 0.22; - // Создаём объект для фильтра Madgwick Madgwick filter; // Создаём объект для работы с гироскопом @@ -24,11 +21,11 @@ float fps = 100; // Калибровочные значения, полученные в калибровочной матрице // из примера compassCalibration -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; +const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; +const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, + { 0.049, 1.942, -0.235 }, + { -0.003, 0.008, 1.944} }; void setup() { // Открываем последовательный порт @@ -41,6 +38,8 @@ void setup() { accelerometer.begin(); // Инициализируем компас compass.begin(); + // Инициализируем фильтр + filter.begin(); // Устанавливаем калибровочные данные compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); @@ -58,9 +57,8 @@ void loop() { gyroscope.readRotationRadXYZ(gx, gy, gz); // Считываем данные с компаса в Гауссах compass.readCalibrateMagneticGaussXYZ(mx, my, mz); - - // Устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); + // Устанавливаем частоту фильтра + filter.setFrequency(fps); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino new file mode 100644 index 0000000..cccbaba --- /dev/null +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino @@ -0,0 +1,65 @@ +// Библиотека для работы с модулями IMU +#include + +// Создаём объект для фильтра Madgwick +Madgwick filter; +// Создаём объект для работы с гироскопом +Gyroscope gyroscope; +// Создаём объект для работы с акселерометром +Accelerometer accelerometer; + +// Переменные для данных с гироска и акселерометра и компаса +float gx, gy, gz, ax, ay, az; + +// Переменные для хранения самолётных углов ориентации +float yaw, pitch, roll; + +// Переменная для хранения частоты выборок фильтра +float fps = 100; + +void setup() { + // Открываем последовательный порт + Serial.begin(9600); + // Инициализируем гироскоп + gyroscope.begin(); + // Инициализируем акселерометр + accelerometer.begin(); + // Инициализируем фильтр + filter.begin(); +} + +void loop() { + // Запоминаем текущее время + unsigned long startMillis = millis(); + + // Считываем данные с акселерометра в единицах G + accelerometer.readAccelerationGXYZ(ax, ay, az); + // Считываем данные с гироскопа в радианах в секунду + gyroscope.readRotationRadXYZ(gx, gy, gz); + + // Устанавливаем частоту фильтра + filter.setFrequency(fps); + // Обновляем входные данные в фильтр + filter.update(gx, gy, gz, ax, ay, az); + + if (Serial.available() > 0) { + int val = Serial.read(); + // Если пришёл символ 's' + if (val == 's') { + float q0, q1, q2, q3; + filter.readQuaternions(q0, q1, q2, q3); + // Выводим кватернионы в serial-порт + Serial.print(q0); + Serial.print(","); + Serial.print(q1); + Serial.print(","); + Serial.print(q2); + Serial.print(","); + Serial.println(q3); + } + } + // Вычисляем затраченное время на обработку данных + unsigned long deltaMillis = millis() - startMillis; + // Вычисляем частоту обработки фильтра + fps = 1000 / deltaMillis; +} diff --git a/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino similarity index 82% rename from examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino rename to examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino index 869c198..c63b521 100644 --- a/examples/IMU/Madgwick9DOFVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino @@ -1,12 +1,8 @@ // Библиотека для работы с модулями IMU #include -// Множитель фильтра -constexpr float BETA = 0.22; - // Создаём объект для фильтра Madgwick Madgwick filter; - // Создаём объект для работы с гироскопом Gyroscope gyroscope; // Создаём объект для работы с акселерометром @@ -20,16 +16,16 @@ float gx, gy, gz, ax, ay, az, mx, my, mz; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; -// Переменная для хранения частоты выборок фильтра +// Переменная для хранения частоты фильтра float fps = 100; // Калибровочные значения, полученные в калибровочной матрице // из примера compassCalibration -const float compassCalibrationBias[3] = { 2269.685, -3415.288, 4698.337 }; +const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; -const float compassCalibrationMatrix[3][3] = { { 2.464, 0.133, 0.009 }, - { 0.09, 3.081, 0.016 }, - { -0.003, -0.225, 2.922 } }; +const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, + { 0.049, 1.942, -0.235 }, + { -0.003, 0.008, 1.944 } }; void setup() { // Открываем последовательный порт @@ -40,6 +36,8 @@ void setup() { accelerometer.begin(); // Инициализируем компас compass.begin(); + // Инициализируем фильтр + filter.begin(); // Устанавливаем калибровочные данные compass.setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); @@ -55,9 +53,8 @@ void loop() { gyroscope.readRotationRadXYZ(gx, gy, gz); // Считываем данные с компаса в Гауссах compass.readCalibrateMagneticGaussXYZ(mx, my, mz); - - // Устанавливаем коэффициенты фильтра - filter.setKoeff(fps, BETA); + // Устанавливаем частоту фильтра + filter.setFrequency(fps); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); @@ -66,7 +63,7 @@ void loop() { // Если пришёл символ 's' if (val == 's') { float q0, q1, q2, q3; - filter.readQuaternions(&q0, &q1, &q2, &q3); + filter.readQuaternions(q0, q1, q2, q3); // Выводим кватернионы в serial-порт Serial.print(q0); Serial.print(","); diff --git a/examples/IMU/Madgwick9DOFVisualization/Processing/Madgwick9DOFVisualizationDraw/Madgwick9DOFVisualizationDraw.pde b/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde similarity index 100% rename from examples/IMU/Madgwick9DOFVisualization/Processing/Madgwick9DOFVisualizationDraw/Madgwick9DOFVisualizationDraw.pde rename to examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index 4e16dba..13f2f73 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -4,62 +4,65 @@ Madgwick::Madgwick() { } -void Madgwick::setKoeff(float sampleFreq, float beta) { - _beta = beta; - _sampleFreq = sampleFreq; +void Madgwick::begin() { + reset(); + setSettings(); } + void Madgwick::reset() { - _q0 = 1.0; - _q1 = 0; - _q2 = 0; - _q3 = 0; + _q0 = 1.0f; + _q1 = 0.0f; + _q2 = 0.0f; + _q3 = 0.0f; } -void Madgwick::readQuaternions(float* q0, float* q1, float* q2, float* q3) { - *q0 = _q0; - *q1 = _q1; - *q2 = _q2; - *q3 = _q3; +void Madgwick::setSettings(float beta, float zeta) { + _beta = beta; + _zeta = zeta; } -//--------------------------------------------------------------------------------------------------- -// AHRS algorithm update +void Madgwick::setFrequency(float frequency) { + _frequency = frequency; +} +void Madgwick::readQuaternions(float& q0, float& q1, float& q2, float& q3) { + q0 = _q0; + q1 = _q1; + q2 = _q2; + q3 = _q3; +} + +// IMU algorithm update with magnetometer void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float recipNorm; float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; + float qDot0, qDot1, qDot2, qDot3; float hx, hy; float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in - // magnetometer normalisation) - if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - update(gx, gy, gz, ax, ay, az); - return; - } - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); - qDot2 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); - qDot3 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); - qDot4 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); + qDot0 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); + qDot1 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); + qDot2 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); + qDot3 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); - // Compute feedback only if accelerometer measurement valid (avoids NaN in - // accelerometer normalisation) + // Compute feedback only if accelerometer measurement valid + // avoids NaN in accelerometer normalisation if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); + recipNorm = sqrtf(ax * ax + ay * ay + az * az); + recipNorm = 1 / recipNorm; ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalise magnetometer measurement - recipNorm = invSqrt(mx * mx + my * my + mz * mz); + recipNorm = sqrtf(mx * mx + my * my + mz * mz); + recipNorm = 1 / recipNorm; mx *= recipNorm; my *= recipNorm; mz *= recipNorm; @@ -132,57 +135,57 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * _q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 - + s3 * s3); // normalise step magnitude + // Normalise step magnitude + recipNorm = sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + recipNorm = 1 / recipNorm; s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; // Apply feedback step - qDot1 -= _beta * s0; - qDot2 -= _beta * s1; - qDot3 -= _beta * s2; - qDot4 -= _beta * s3; + qDot0 -= _beta * s0; + qDot1 -= _beta * s1; + qDot2 -= _beta * s2; + qDot3 -= _beta * s3; } // Integrate rate of change of quaternion to yield quaternion - _q0 += qDot1 * (1.0f / _sampleFreq); - _q1 += qDot2 * (1.0f / _sampleFreq); - _q2 += qDot3 * (1.0f / _sampleFreq); - _q3 += qDot4 * (1.0f / _sampleFreq); + _q0 += qDot0 * (1.0f / _frequency); + _q1 += qDot1 * (1.0f / _frequency); + _q2 += qDot2 * (1.0f / _frequency); + _q3 += qDot3 * (1.0f / _frequency); // Normalise quaternion - recipNorm = invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); + recipNorm = sqrtf(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); + recipNorm = 1 / recipNorm; _q0 *= recipNorm; _q1 *= recipNorm; _q2 *= recipNorm; _q3 *= recipNorm; } -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update - +// IMU algorithm update without magnetometer void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; + float qDot0, qDot1, qDot2, qDot3; float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); - qDot2 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); - qDot3 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); - qDot4 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); + qDot0 = 0.5f * (-_q1 * gx - _q2 * gy - _q3 * gz); + qDot1 = 0.5f * (_q0 * gx + _q2 * gz - _q3 * gy); + qDot2 = 0.5f * (_q0 * gy - _q1 * gz + _q3 * gx); + qDot3 = 0.5f * (_q0 * gz + _q1 * gy - _q2 * gx); - // Compute feedback only if accelerometer measurement valid (avoids NaN in - // accelerometer normalisation) + // Compute feedback only if accelerometer measurement valid + // avoids NaN in accelerometer normalisation if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); + recipNorm = sqrtf(ax * ax + ay * ay + az * az); + recipNorm = 1 / recipNorm; ax *= recipNorm; ay *= recipNorm; az *= recipNorm; @@ -209,48 +212,37 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, s2 = 4.0f * q0q0 * _q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; s3 = 4.0f * q1q1 * _q3 - _2q1 * ax + 4.0f * q2q2 * _q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 - + s3 * s3); // normalise step magnitude + + // Normalise step magnitude + recipNorm = sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + recipNorm = 1 / recipNorm; s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; // Apply feedback step - qDot1 -= _beta * s0; - qDot2 -= _beta * s1; - qDot3 -= _beta * s2; - qDot4 -= _beta * s3; + qDot0 -= _beta * s0; + qDot1 -= _beta * s1; + qDot2 -= _beta * s2; + qDot3 -= _beta * s3; } // Integrate rate of change of quaternion to yield quaternion - _q0 += qDot1 * (1.0f / _sampleFreq); - _q1 += qDot2 * (1.0f / _sampleFreq); - _q2 += qDot3 * (1.0f / _sampleFreq); - _q3 += qDot4 * (1.0f / _sampleFreq); + _q0 += qDot0 * (1.0f / _frequency); + _q1 += qDot1 * (1.0f / _frequency); + _q2 += qDot2 * (1.0f / _frequency); + _q3 += qDot3 * (1.0f / _frequency); // Normalise quaternion - recipNorm = invSqrt(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); + recipNorm = sqrtf(_q0 * _q0 + _q1 * _q1 + _q2 * _q2 + _q3 * _q3); + recipNorm = 1 / recipNorm; _q0 *= recipNorm; _q1 *= recipNorm; _q2 *= recipNorm; _q3 *= recipNorm; } -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root - -float Madgwick::invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i >> 1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - return y; -} - float Madgwick::getYawRad() { return atan2(2 * _q1 * _q2 - 2 * _q0 * _q3, 2 * _q0 * _q0 + 2 * _q1 * _q1 - 1); @@ -271,4 +263,4 @@ float Madgwick::getYawDeg() { return getYawRad() * RAD_TO_DEG; } float Madgwick::getPitchDeg() { return getPitchRad() * RAD_TO_DEG; } -float Madgwick::getRollDeg() { return getRollRad() * RAD_TO_DEG; } \ No newline at end of file +float Madgwick::getRollDeg() { return getRollRad() * RAD_TO_DEG; } diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 8ed49b2..842a5e4 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -3,16 +3,23 @@ #include -#define SAMPLE_FREQ 1000.0f // sample frequency in Hz -#define BETA_DEF 0.5f // 2 * proportional gain +// Gyroscope measurement error in rads/s (start at 40 deg/s) +constexpr float GYRO_MEAS_ERROR = M_PI * (40.0f / 180.0f); +// Gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) +constexpr float GYRO_MEAS_DRIFT = M_PI * (0.0f / 180.0f); +// Compute beta +constexpr float BETA_DEFAULT = sqrt(3.0f / 4.0f) * GYRO_MEAS_ERROR; +// Compute zeta +constexpr float ZETA_DEFAULT = sqrt(3.0f / 4.0f) * GYRO_MEAS_DRIFT; class Madgwick { - public: Madgwick(); - void readQuaternions(float* q0, float* q1, float* q2, float* q3); + void begin(); void reset(); - void setKoeff(float sampleFreq, float beta); + void setSettings(float beta = BETA_DEFAULT, float zeta = ZETA_DEFAULT); + void setFrequency(float frequency); + void readQuaternions(float& q0, float& q1, float& q2, float& q3); void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); void update(float gx, float gy, float gz, float ax, float ay, float az); @@ -24,13 +31,13 @@ class Madgwick { float getYawDeg(); private: - float invSqrt(float x); - volatile float _beta = BETA_DEF; // algorithm gain - volatile float _sampleFreq = SAMPLE_FREQ; - volatile float _q0 = 1.0f; - volatile float _q1 = 0.0f; - volatile float _q2 = 0.0f; - volatile float _q3 = 0.0f; + volatile float _beta; + volatile float _zeta; + volatile float _frequency; + volatile float _q0; + volatile float _q1; + volatile float _q2; + volatile float _q3; }; #endif // __MADGWICK_AHRS_H__ From 79b07b8928c295759cfb292267678f64b5c971a2 Mon Sep 17 00:00:00 2001 From: Igor Date: Tue, 27 Oct 2020 04:11:44 +0300 Subject: [PATCH 14/20] Create LICENSE --- LICENSE | 674 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 674 insertions(+) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..f288702 --- /dev/null +++ b/LICENSE @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. From 83799bde73a4f87d84755816c385c56b9a2ee956 Mon Sep 17 00:00:00 2001 From: igor605ds Date: Tue, 27 Oct 2020 04:18:57 +0300 Subject: [PATCH 15/20] delete volotile qualifiers in variables of Madgwick class --- src/MadgwickAHRS.h | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 842a5e4..0a19a84 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -31,13 +31,10 @@ class Madgwick { float getYawDeg(); private: - volatile float _beta; - volatile float _zeta; - volatile float _frequency; - volatile float _q0; - volatile float _q1; - volatile float _q2; - volatile float _q3; + float _beta; + float _zeta; + float _frequency; + float _q0, _q1, _q2, _q3; }; #endif // __MADGWICK_AHRS_H__ From 073519faf4823c8373f5b939dcded68f5d12e578 Mon Sep 17 00:00:00 2001 From: igor605ds Date: Tue, 27 Oct 2020 04:56:39 +0300 Subject: [PATCH 16/20] replace explicit type conversion C-style to static_cast --- src/Barometer.cpp | 8 ++++---- src/Barometer.h | 4 ++-- src/BaseIMU.cpp | 12 ++++++------ 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/Barometer.cpp b/src/Barometer.cpp index 9fac8a3..8adc69a 100644 --- a/src/Barometer.cpp +++ b/src/Barometer.cpp @@ -9,9 +9,9 @@ void Barometer::begin(TwoWire& wire) { _wire->begin(); _deviceID = readDeviceID(); if (_deviceID == LPS331_WHO_AM_I) { - _ctrlReg1 |= LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR0; + _ctrlReg1 |= LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR1 | LPS_CTRL_REG1_ODR2; } else if (_deviceID == LPS25HB_WHO_AM_I) { - _ctrlReg1 |= LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR0; + _ctrlReg1 |= LPS_CTRL_REG1_ODR2; } _ctrlReg1 |= LPS_CTRL_REG1_PD; _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); @@ -45,11 +45,11 @@ float Barometer::readTemperatureF() { return readTemperatureC() * 1.8 + 32; } uint32_t Barometer::_readPressureRaw() { uint8_t data[3]; _readBytes(0x80 | LPS_PRESS_OUT_XL, data, 3); - return (uint32_t)data[2] << 16 | (uint16_t)data[1] << 8 | data[0]; + return static_cast(data[2]) << 16 | static_cast(data[1]) << 8 | data[0]; } int16_t Barometer::_readTemperatureRaw() { - return ((int16_t)_readByte(LPS_TEMP_OUT_H) << 8) + return (static_cast(_readByte(LPS_TEMP_OUT_H)) << 8) | _readByte(LPS_TEMP_OUT_L); } diff --git a/src/Barometer.h b/src/Barometer.h index d3241de..2493e53 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -4,8 +4,8 @@ #include "BaseIMU.h" // I²C-address devices LPS25HB and LPS331 -constexpr uint8_t LPS_SLAVE_ADDRESS = 0x1C; -constexpr uint8_t LPS_SLAVE_ADDRESS_ALT = 0x1D; +constexpr uint8_t LPS_SLAVE_ADDRESS = 0x5C; +constexpr uint8_t LPS_SLAVE_ADDRESS_ALT = 0x5D; // Registers address LPS25HB and LPS331 constexpr uint8_t LPS_CTRL_REG1_ODR0 = 0x10; diff --git a/src/BaseIMU.cpp b/src/BaseIMU.cpp index 0732a89..c6bbc6d 100644 --- a/src/BaseIMU.cpp +++ b/src/BaseIMU.cpp @@ -5,23 +5,23 @@ uint8_t BaseIMU::readDeviceID(){ } int16_t BaseIMU::readX() { - return ((int16_t)_readByte(BASE_IMU_OUT_X_H) << 8) | _readByte(BASE_IMU_OUT_X_L); + return (static_cast(_readByte(BASE_IMU_OUT_X_H)) << 8) | _readByte(BASE_IMU_OUT_X_L); } int16_t BaseIMU::readY() { - return ((int16_t)_readByte(BASE_IMU_OUT_Y_H) << 8) | _readByte(BASE_IMU_OUT_Y_L); + return (static_cast(_readByte(BASE_IMU_OUT_Y_H)) << 8) | _readByte(BASE_IMU_OUT_Y_L); } int16_t BaseIMU::readZ() { - return ((int16_t)_readByte(BASE_IMU_OUT_Z_H) << 8) | _readByte(BASE_IMU_OUT_Z_L); + return (static_cast(_readByte(BASE_IMU_OUT_Z_H)) << 8) | _readByte(BASE_IMU_OUT_Z_L); } void BaseIMU::readXYZ(int16_t& x, int16_t& y, int16_t& z) { uint8_t data[6]; _readBytes(0x80 | BASE_IMU_OUT_X_L, data, 6); - x = ((int16_t)data[1] << 8) | data[0]; - y = ((int16_t)data[3] << 8) | data[2]; - z = ((int16_t)data[5] << 8) | data[4]; + x = (static_cast(data[1]) << 8) | data[0]; + y = (static_cast(data[3]) << 8) | data[2]; + z = (static_cast(data[5]) << 8) | data[4]; } uint8_t BaseIMU::_readByte(uint8_t regAddress) { From cc9bd8ce33ea7d58a040e26673d17a2da4e255c1 Mon Sep 17 00:00:00 2001 From: igor605ds Date: Tue, 27 Oct 2020 18:38:11 +0300 Subject: [PATCH 17/20] add compatibility with old functions and examples --- .../AccelerometerSimple.ino | 2 +- .../Compass/CompassAzimuth/CompassAzimuth.ino | 2 +- .../CompassCalibration/CompassCalibration.ino | 2 +- .../Compass/CompassSimple/CompassSimple.ino | 2 +- examples/IMU/IMUSimple/IMUSimple.ino | 2 +- examples/IMU/Madgwick9DOF/Madgwick9DOF.ino | 2 +- .../Madgwick6DOFVisualization.ino | 2 +- .../Madgwick9DOFVisualization.ino | 4 +-- .../MadgwickVisualizationDraw.pde | 4 +-- src/Accelerometer.h | 26 ++++++++++++++++++- src/Barometer.h | 2 +- src/Compass.h | 19 +++++++++++++- src/GOST4401_81.h | 2 +- src/Gyroscope.h | 24 ++++++++++++++++- src/MadgwickAHRS.cpp | 16 +++++++++++- src/MadgwickAHRS.h | 8 ++++-- src/TroykaIMU.h | 2 +- 17 files changed, 101 insertions(+), 20 deletions(-) diff --git a/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino b/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino index 23ecc31..131a4e0 100644 --- a/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino +++ b/examples/Accelerometer/AccelerometerSimple/AccelerometerSimple.ino @@ -27,4 +27,4 @@ void loop() { Serial.print("\t\t"); Serial.println(); delay(100); -} \ No newline at end of file +} diff --git a/examples/Compass/CompassAzimuth/CompassAzimuth.ino b/examples/Compass/CompassAzimuth/CompassAzimuth.ino index 8e2a669..98b5584 100644 --- a/examples/Compass/CompassAzimuth/CompassAzimuth.ino +++ b/examples/Compass/CompassAzimuth/CompassAzimuth.ino @@ -1,7 +1,7 @@ // Библиотека для работы с модулями IMU #include -// Создаём объект для работы с иагнитометром/компасом +// Создаём объект для работы с магнитометром/компасом Compass compass; // Калибровочные значения, полученные в калибровочной матрице diff --git a/examples/Compass/CompassCalibration/CompassCalibration.ino b/examples/Compass/CompassCalibration/CompassCalibration.ino index a524f13..aa4f287 100644 --- a/examples/Compass/CompassCalibration/CompassCalibration.ino +++ b/examples/Compass/CompassCalibration/CompassCalibration.ino @@ -1,7 +1,7 @@ // Библиотека для работы с модулями IMU #include -// Создаём объект для работы с иагнитометром/компасом +// Создаём объект для работы с магнитометром/компасом Compass compass; void setup() { diff --git a/examples/Compass/CompassSimple/CompassSimple.ino b/examples/Compass/CompassSimple/CompassSimple.ino index be77baa..4b8880b 100644 --- a/examples/Compass/CompassSimple/CompassSimple.ino +++ b/examples/Compass/CompassSimple/CompassSimple.ino @@ -1,7 +1,7 @@ // Библиотека для работы с модулями IMU #include -// Создаём объект для работы с иагнитометром/компасом +// Создаём объект для работы с магнитометром/компасом Compass compass; void setup() { diff --git a/examples/IMU/IMUSimple/IMUSimple.ino b/examples/IMU/IMUSimple/IMUSimple.ino index d6ba7fe..0792e36 100644 --- a/examples/IMU/IMUSimple/IMUSimple.ino +++ b/examples/IMU/IMUSimple/IMUSimple.ino @@ -5,7 +5,7 @@ Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; -// Создаём объект для работы с компасом +// Создаём объект для работы с магнитометром/компасом Compass compass; // Создаём объект для работы с барометром Barometer barometer; diff --git a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino index b2f63af..e4b31dc 100644 --- a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino +++ b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino @@ -7,7 +7,7 @@ Madgwick filter; Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; -// Создаём объект для работы с компасом +// Создаём объект для работы с магнитометром/компасом Compass compass; // Переменные для данных с гироска, акселерометра и компаса diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino index cccbaba..5493383 100644 --- a/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino @@ -47,7 +47,7 @@ void loop() { // Если пришёл символ 's' if (val == 's') { float q0, q1, q2, q3; - filter.readQuaternions(q0, q1, q2, q3); + filter.readQuaternion(q0, q1, q2, q3); // Выводим кватернионы в serial-порт Serial.print(q0); Serial.print(","); diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino index c63b521..8baf45d 100644 --- a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino @@ -7,7 +7,7 @@ Madgwick filter; Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; -// Создаём объект для работы с компасом +// Создаём объект для работы с магнитометром/компасом Compass compass; // Переменные для данных с гироска, акселерометра и компаса @@ -63,7 +63,7 @@ void loop() { // Если пришёл символ 's' if (val == 's') { float q0, q1, q2, q3; - filter.readQuaternions(q0, q1, q2, q3); + filter.readQuaternion(q0, q1, q2, q3); // Выводим кватернионы в serial-порт Serial.print(q0); Serial.print(","); diff --git a/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde b/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde index d89dcb1..339cacc 100644 --- a/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde +++ b/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde @@ -16,7 +16,7 @@ String message; float[] q = new float[4]; Quaternion quat = new Quaternion(1, 0, 0, 0); // New line character in ASCII -int newLine = 13; +final char newLine = '\n'; String [] massQ = new String [4]; float[] ypr = new float[3]; @@ -185,4 +185,4 @@ void printYawPitchRoll() { text(ypr[1], 220, 40, 10); text("Roll:", 290, 20, 10); text(ypr[2], 290, 40, 10); -} \ No newline at end of file +} diff --git a/src/Accelerometer.h b/src/Accelerometer.h index ceadf41..ba86901 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -19,8 +19,11 @@ constexpr uint8_t LIS331DLH_CTRL_REG1_PM0 = 0x20; constexpr uint8_t LIS331DLH_CTRL_REG1_PM1 = 0x40; constexpr uint8_t LIS331DLH_CTRL_REG1_PM2 = 0x80; -// Constans +// Constans, Gravity of Earth constexpr float GRAVITY_EARTH = 9.8; + +// Sensor sensitivity depending on selectable full scales +// Use the datasheet for details constexpr float SENS_2G = 1; constexpr float SENS_4G = 2; constexpr float SENS_8G = 3.9; @@ -41,6 +44,27 @@ class Accelerometer : public BaseIMU { float readAccelerationAZ(); void readAccelerationGXYZ(float& ax, float& ay, float& az); void readAccelerationAXYZ(float& ax, float& ay, float& az); + // DEPRECATED fuctions + // Use readAccelerationGX instead + float readGX() { return readAccelerationGX(); } + // Use readAccelerationGY instead + float readGY() { return readAccelerationGY(); } + // Use readAccelerationGZ instead + float readGZ() { return readAccelerationGZ(); } + // Use readAccelerationAX instead + float readAX() { return readAccelerationAX(); } + // Use readAccelerationAY instead + float readAY() { return readAccelerationAY(); } + // Use readAccelerationAZ instead + float readAZ() { return readAccelerationAZ(); } + // Use readAccelerationGXYZ instead + void readGXYZ(float* ax, float* ay, float* az) { + readAccelerationGXYZ(*ax, *ay, *az); + } + // Use readAccelerationAXYZ instead + void readAXYZ(float* ax, float* ay, float* az) { + readAccelerationAXYZ(*ax, *ay, *az); + } private: float _scalingFactor; diff --git a/src/Barometer.h b/src/Barometer.h index 2493e53..5300129 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -23,7 +23,7 @@ constexpr uint8_t LPS_TEMP_OUT_H = 0x2C; constexpr uint8_t LPS331_WHO_AM_I = 0xBB; constexpr uint8_t LPS25HB_WHO_AM_I = 0xBD; -// Constans +// Constans: conversion factors units of measurement constexpr float CELSIUS_TO_KELVIN = 273.15; constexpr float MILLIBARS_TO_PASCALS = 100; constexpr float MILLIBARS_TO_MILLIMETERSHG = 0.75; diff --git a/src/Compass.h b/src/Compass.h index 8a18c28..84fc367 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -15,7 +15,8 @@ constexpr uint8_t LIS3MDL_CTRL_REG2_FS1 = 0x40; constexpr uint8_t LIS3MDL_CTRL_REG3_MD0 = 0x00; constexpr uint8_t LIS3MDL_CTRL_REG3_MD1 = 0x02; -// Constans +// Constans: sensor sensitivity depending on selectable full scales +// Use the datasheet for details constexpr float SENS_4GAUSS = 6842; constexpr float SENS_8GAUSS = 3421; constexpr float SENS_12GAUSS = 2281; @@ -42,6 +43,22 @@ class Compass : public BaseIMU { void setCalibrateMatrix(const float calibrationMatrix[3][3], const float bias[3]); float readAzimut(); + // DEPRECATED fuctions + // Use readMagneticGaussX instead + float readGaussX() { return readMagneticGaussX(); } + // Use readMagneticGaussY instead + float readGaussY() { return readMagneticGaussY(); } + // Use readMagneticGaussZ instead + float readGaussZ() { return readMagneticGaussZ(); } + // Use readCalibrateMagneticGaussXYZ instead + void readCalibrateGaussXYZ(float* mx, float* my, float* mz) { + readCalibrateMagneticGaussXYZ(*mx, *my, *mz); + } + // Use setCalibrateMatrix instead + void calibrateMatrix(const float compassCalibrationMatrix[3][3], + const float compassCalibrationBias[3]) { + setCalibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); + } private: void _calibrate(float& mx, float& my, float& mz); diff --git a/src/GOST4401_81.h b/src/GOST4401_81.h index c20d896..da773ba 100644 --- a/src/GOST4401_81.h +++ b/src/GOST4401_81.h @@ -47,4 +47,4 @@ static const GOST4401_RECORD ag_table[] = { { 47000, 270.65, 0.0, 110.9056 }, { 51000, 270.65, -0.0028, 6.69384 } }; -#endif // __ATMOSPHERE_GOST4401_81_H__ \ No newline at end of file +#endif // __ATMOSPHERE_GOST4401_81_H__ diff --git a/src/Gyroscope.h b/src/Gyroscope.h index 090fcef..dd29a45 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -17,7 +17,8 @@ constexpr uint8_t L3G4200D_CTRL_REG1_Y_EN = 0x02; constexpr uint8_t L3G4200D_CTRL_REG1_Z_EN = 0x04; constexpr uint8_t L3G4200D_CTRL_REG1_PD = 0x08; -// Constans +// Constans: sensor sensitivity depending on selectable full scales +// Use the datasheet for details constexpr float SENS_250DPS = 0.00875; constexpr float SENS_500DPS = 0.0175; constexpr float SENS_2000DPS = 0.07; @@ -42,6 +43,27 @@ class Gyroscope : public BaseIMU { float readRotationRadZ(); void readRotationDegXYZ(float& gx, float& gy, float& gz); void readRotationRadXYZ(float& gx, float& gy, float& gz); + // DEPRECATED fuctions + // Use readRotationDegX instead + float readDegPerSecX() { return readRotationDegX(); } + // Use readRotationDegY instead + float readDegPerSecY() { return readRotationDegY(); } + // Use readRotationDegZ instead + float readDegPerSecZ() { return readRotationDegZ(); } + // Use readRotationRadX instead + float readRadPerSecX() { return readRotationRadX(); } + // Use readRotationRadY instead + float readRadPerSecY() { return readRotationRadY(); } + // Use readRotationRadZ instead + float readRadPerSecZ() { return readRotationRadZ(); } + // Use readRotationDegXYZ instead + void readDegPerSecXYZ(float* ax, float* ay, float* az) { + readRotationDegXYZ(*ax, *ay, *az); + } + // Use readRotationRadXYZ instead + void readRadPerSecXYZ(float* ax, float* ay, float* az) { + readRotationRadXYZ(*ax, *ay, *az); + } private: float _scalingFactor; diff --git a/src/MadgwickAHRS.cpp b/src/MadgwickAHRS.cpp index 13f2f73..1fc0a23 100644 --- a/src/MadgwickAHRS.cpp +++ b/src/MadgwickAHRS.cpp @@ -25,7 +25,7 @@ void Madgwick::setFrequency(float frequency) { _frequency = frequency; } -void Madgwick::readQuaternions(float& q0, float& q1, float& q2, float& q3) { +void Madgwick::readQuaternion(float& q0, float& q1, float& q2, float& q3) { q0 = _q0; q1 = _q1; q2 = _q2; @@ -264,3 +264,17 @@ float Madgwick::getYawDeg() { return getYawRad() * RAD_TO_DEG; } float Madgwick::getPitchDeg() { return getPitchRad() * RAD_TO_DEG; } float Madgwick::getRollDeg() { return getRollRad() * RAD_TO_DEG; } + +// DEPRECATED: use readQuaternion instead +void Madgwick::readQuaternions(float *q0, float *q1, float *q2, float *q3) { + *q0 = _q0; + *q1 = _q1; + *q2 = _q2; + *q3 = _q3; +} + +// DEPRECATED: use setSettings and setFrequency instead +void Madgwick::setKoeff(float frequency, float beta) { + _beta = beta; + _frequency = frequency; +} diff --git a/src/MadgwickAHRS.h b/src/MadgwickAHRS.h index 0a19a84..b9aea88 100644 --- a/src/MadgwickAHRS.h +++ b/src/MadgwickAHRS.h @@ -19,7 +19,7 @@ class Madgwick { void reset(); void setSettings(float beta = BETA_DEFAULT, float zeta = ZETA_DEFAULT); void setFrequency(float frequency); - void readQuaternions(float& q0, float& q1, float& q2, float& q3); + void readQuaternion(float& q0, float& q1, float& q2, float& q3); void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); void update(float gx, float gy, float gz, float ax, float ay, float az); @@ -29,7 +29,11 @@ class Madgwick { float getPitchDeg(); float getRollDeg(); float getYawDeg(); - + // DEPRECATED fuctions + // Use readQuaternion instead + void readQuaternions(float *q0, float *q1, float *q2, float *q3); + // Use setSettings and setFrequency instead + void setKoeff(float frequency, float beta); private: float _beta; float _zeta; diff --git a/src/TroykaIMU.h b/src/TroykaIMU.h index 94f444f..88c3957 100644 --- a/src/TroykaIMU.h +++ b/src/TroykaIMU.h @@ -7,4 +7,4 @@ #include "Gyroscope.h" #include "MadgwickAHRS.h" -#endif // __TROYKA_IMU_H__ \ No newline at end of file +#endif // __TROYKA_IMU_H__ From 24f1174c23abcee4fc976cabe21eb4a322d61b49 Mon Sep 17 00:00:00 2001 From: igor605ds Date: Thu, 29 Oct 2020 03:03:40 +0200 Subject: [PATCH 18/20] apply suggestions from code review --- .../Compass/CompassAzimuth/CompassAzimuth.ino | 5 ++-- .../CompassCalibration/CompassCalibration.ino | 5 ++++ examples/IMU/Madgwick9DOF/Madgwick9DOF.ino | 5 ++-- .../Madgwick9DOFVisualization.ino | 5 ++-- src/Accelerometer.cpp | 25 +++++++++++-------- src/Accelerometer.h | 2 +- src/Barometer.cpp | 9 ++++--- src/Barometer.h | 6 +++-- src/BaseIMU.cpp | 2 -- src/BaseIMU.h | 5 ---- src/Compass.cpp | 25 +++++++++++-------- src/Compass.h | 4 +-- src/Gyroscope.cpp | 25 +++++++++++-------- src/Gyroscope.h | 2 +- 14 files changed, 70 insertions(+), 55 deletions(-) diff --git a/examples/Compass/CompassAzimuth/CompassAzimuth.ino b/examples/Compass/CompassAzimuth/CompassAzimuth.ino index 98b5584..97a6d46 100644 --- a/examples/Compass/CompassAzimuth/CompassAzimuth.ino +++ b/examples/Compass/CompassAzimuth/CompassAzimuth.ino @@ -4,8 +4,9 @@ // Создаём объект для работы с магнитометром/компасом Compass compass; -// Калибровочные значения, полученные в калибровочной матрице -// из примера compassCalibration +// Калибровочные данные для работы магнитометра в режиме компаса +// Подробности читайте в документации про калиборку модуля +// http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, diff --git a/examples/Compass/CompassCalibration/CompassCalibration.ino b/examples/Compass/CompassCalibration/CompassCalibration.ino index aa4f287..421d5f4 100644 --- a/examples/Compass/CompassCalibration/CompassCalibration.ino +++ b/examples/Compass/CompassCalibration/CompassCalibration.ino @@ -1,3 +1,8 @@ +// Пример выводит сырые значения магнитометра, которые +// используются для последующей калибровки сенсора +// Подробности читайте в документации про калиборку магнитометра +// http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate + // Библиотека для работы с модулями IMU #include diff --git a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino index e4b31dc..509489a 100644 --- a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino +++ b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino @@ -19,8 +19,9 @@ float yaw, pitch, roll; // Переменная для хранения частоты выборок фильтра float fps = 100; -// Калибровочные значения, полученные в калибровочной матрице -// из примера compassCalibration +// Калибровочные данные для работы магнитометра в режиме компаса +// Подробности читайте в документации про калиборку модуля +// http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino index 8baf45d..42a7931 100644 --- a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino @@ -19,8 +19,9 @@ float yaw, pitch, roll; // Переменная для хранения частоты фильтра float fps = 100; -// Калибровочные значения, полученные в калибровочной матрице -// из примера compassCalibration +// Калибровочные данные для работы магнитометра в режиме компаса +// Подробности читайте в документации про калиборку модуля +// http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; const float compassCalibrationMatrix[3][3] = { { 1.909, 0.082, 0.004 }, diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index e06214f..ade6138 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -6,28 +6,30 @@ Accelerometer::Accelerometer(uint8_t slaveAddress) void Accelerometer::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - _ctrlReg1 |= LIS331DLH_CTRL_REG1_X_EN | LIS331DLH_CTRL_REG1_Y_EN - | LIS331DLH_CTRL_REG1_Z_EN; - _ctrlReg1 |= LIS331DLH_CTRL_REG1_PM0; - _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); + _scalingFactor = 1; + uint8_t data = LIS331DLH_CTRL_REG1_X_EN | LIS331DLH_CTRL_REG1_Y_EN + | LIS331DLH_CTRL_REG1_Z_EN; + data |= LIS331DLH_CTRL_REG1_PM0; + _writeByte(BASE_IMU_CTRL_REG1, data); setRange(AccelerometerRange::RANGE_2G); } // Set range scale output data from datasheet void Accelerometer::setRange(AccelerometerRange range) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG4); + data &= ~(LIS331DLH_CTRL_REG4_FS0 | LIS331DLH_CTRL_REG4_FS1); switch (range) { case AccelerometerRange::RANGE_2G: { - _ctrlReg4 = 0; _scalingFactor = SENS_2G * 4 / pow(2, 16); break; } case AccelerometerRange::RANGE_4G: { - _ctrlReg4 = LIS331DLH_CTRL_REG4_FS0; + data |= LIS331DLH_CTRL_REG4_FS0; _scalingFactor = SENS_4G * 4 / pow(2, 16); break; } case AccelerometerRange::RANGE_8G: { - _ctrlReg4 = LIS331DLH_CTRL_REG4_FS0 | LIS331DLH_CTRL_REG4_FS1; + data |= LIS331DLH_CTRL_REG4_FS0 | LIS331DLH_CTRL_REG4_FS1; _scalingFactor = SENS_8G * 4 / pow(2, 16); break; } @@ -35,16 +37,17 @@ void Accelerometer::setRange(AccelerometerRange range) { _scalingFactor = SENS_2G * 4 / pow(2, 16); } break; } - _writeByte(BASE_IMU_CTRL_REG4, _ctrlReg4); + _writeByte(BASE_IMU_CTRL_REG4, data); } void Accelerometer::sleep(bool state) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG1); if (state) - _ctrlReg1 &= ~LIS331DLH_CTRL_REG1_PM0; + data &= ~LIS331DLH_CTRL_REG1_PM0; else - _ctrlReg1 |= LIS331DLH_CTRL_REG1_PM0; + data |= LIS331DLH_CTRL_REG1_PM0; - _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); + _writeByte(BASE_IMU_CTRL_REG1, data); } float Accelerometer::readAccelerationGX() { return readX() * _scalingFactor; } diff --git a/src/Accelerometer.h b/src/Accelerometer.h index ba86901..47db82a 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -19,7 +19,7 @@ constexpr uint8_t LIS331DLH_CTRL_REG1_PM0 = 0x20; constexpr uint8_t LIS331DLH_CTRL_REG1_PM1 = 0x40; constexpr uint8_t LIS331DLH_CTRL_REG1_PM2 = 0x80; -// Constans, Gravity of Earth +// Gravity of Earth constexpr float GRAVITY_EARTH = 9.8; // Sensor sensitivity depending on selectable full scales diff --git a/src/Barometer.cpp b/src/Barometer.cpp index 8adc69a..1db5a43 100644 --- a/src/Barometer.cpp +++ b/src/Barometer.cpp @@ -8,13 +8,14 @@ void Barometer::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); _deviceID = readDeviceID(); + uint8_t data = 0; if (_deviceID == LPS331_WHO_AM_I) { - _ctrlReg1 |= LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR1 | LPS_CTRL_REG1_ODR2; + data |= LPS_CTRL_REG1_ODR0 | LPS_CTRL_REG1_ODR1 | LPS_CTRL_REG1_ODR2; } else if (_deviceID == LPS25HB_WHO_AM_I) { - _ctrlReg1 |= LPS_CTRL_REG1_ODR2; + data |= LPS_CTRL_REG1_ODR2; } - _ctrlReg1 |= LPS_CTRL_REG1_PD; - _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); + data |= LPS_CTRL_REG1_PD; + _writeByte(BASE_IMU_CTRL_REG1, data); } float Barometer::readPressureMillibars() { diff --git a/src/Barometer.h b/src/Barometer.h index 5300129..c755769 100644 --- a/src/Barometer.h +++ b/src/Barometer.h @@ -23,7 +23,7 @@ constexpr uint8_t LPS_TEMP_OUT_H = 0x2C; constexpr uint8_t LPS331_WHO_AM_I = 0xBB; constexpr uint8_t LPS25HB_WHO_AM_I = 0xBD; -// Constans: conversion factors units of measurement +// Conversion factors units of measurement constexpr float CELSIUS_TO_KELVIN = 273.15; constexpr float MILLIBARS_TO_PASCALS = 100; constexpr float MILLIBARS_TO_MILLIMETERSHG = 0.75; @@ -39,7 +39,9 @@ class Barometer : public BaseIMU { float readTemperatureK(); float readTemperatureF(); float readAltitude(); - + // DEPRECATED fuctions + // Use readAccelerationGX instead + private: uint32_t _readPressureRaw(); int16_t _readTemperatureRaw(); diff --git a/src/BaseIMU.cpp b/src/BaseIMU.cpp index c6bbc6d..dea5564 100644 --- a/src/BaseIMU.cpp +++ b/src/BaseIMU.cpp @@ -31,7 +31,6 @@ uint8_t BaseIMU::_readByte(uint8_t regAddress) { _wire->endTransmission(); _wire->requestFrom(_slaveAddress, 1u); data = _wire->read(); - _wire->endTransmission(); return data; } @@ -50,5 +49,4 @@ void BaseIMU::_readBytes(uint8_t regAddress, uint8_t* data, uint8_t length) { for (size_t i = 0; i < length; i++) { *data++ = _wire->read(); } - _wire->endTransmission(); } diff --git a/src/BaseIMU.h b/src/BaseIMU.h index 82663a2..ccd33f5 100644 --- a/src/BaseIMU.h +++ b/src/BaseIMU.h @@ -34,11 +34,6 @@ class BaseIMU { uint8_t _readByte(uint8_t regAddress); void _writeByte(uint8_t regAddress, uint8_t data); void _readBytes(uint8_t regAddress, uint8_t* data, uint8_t length); - uint8_t _ctrlReg1; - uint8_t _ctrlReg2; - uint8_t _ctrlReg3; - uint8_t _ctrlReg4; - uint8_t _ctrlReg5; TwoWire* _wire; private: diff --git a/src/Compass.cpp b/src/Compass.cpp index e12a1c9..c5df4a7 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -6,29 +6,33 @@ Compass::Compass(uint8_t slaveAddress) void Compass::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - _writeByte(BASE_IMU_CTRL_REG3, _ctrlReg3); - setRange(CompassRange::RANGE_16GAUSS); + _scalingFactor = 1; + uint8_t data = 0; + data &= ~(LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1); + _writeByte(BASE_IMU_CTRL_REG3, data); + setRange(CompassRange::RANGE_4GAUSS); } void Compass::setRange(CompassRange range) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG2); + data &= ~(LIS3MDL_CTRL_REG2_FS0 | LIS3MDL_CTRL_REG2_FS1); switch (range) { case CompassRange::RANGE_4GAUSS: { - _ctrlReg2 = 0; _scalingFactor = SENS_4GAUSS; break; } case CompassRange::RANGE_8GAUSS: { - _ctrlReg2 = LIS3MDL_CTRL_REG2_FS0; + data |= LIS3MDL_CTRL_REG2_FS0; _scalingFactor = SENS_8GAUSS; break; } case CompassRange::RANGE_12GAUSS: { - _ctrlReg2 = LIS3MDL_CTRL_REG2_FS1; + data |= LIS3MDL_CTRL_REG2_FS1; _scalingFactor = SENS_12GAUSS; break; } case CompassRange::RANGE_16GAUSS: { - _ctrlReg2 = LIS3MDL_CTRL_REG2_FS0 | LIS3MDL_CTRL_REG2_FS1; + data |= LIS3MDL_CTRL_REG2_FS0 | LIS3MDL_CTRL_REG2_FS1; _scalingFactor = SENS_16GAUSS; break; } @@ -36,16 +40,17 @@ void Compass::setRange(CompassRange range) { _scalingFactor = SENS_4GAUSS; } break; } - _writeByte(BASE_IMU_CTRL_REG2, _ctrlReg2); + _writeByte(BASE_IMU_CTRL_REG2, data); } void Compass::sleep(bool state) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG3); if (state) - _ctrlReg3 |= LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1; + data |= LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1; else - _ctrlReg3 &= ~(LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1); + data &= ~(LIS3MDL_CTRL_REG3_MD0 | LIS3MDL_CTRL_REG3_MD1); - _writeByte(BASE_IMU_CTRL_REG3, _ctrlReg3); + _writeByte(BASE_IMU_CTRL_REG3, data); } float Compass::readMagneticGaussX() { return readX() / _scalingFactor; } diff --git a/src/Compass.h b/src/Compass.h index 84fc367..bf8cb8a 100644 --- a/src/Compass.h +++ b/src/Compass.h @@ -12,10 +12,10 @@ constexpr uint8_t LIS3MDL_CTRL_REG2_FS0 = 0x20; constexpr uint8_t LIS3MDL_CTRL_REG2_FS1 = 0x40; // Registers value bits -constexpr uint8_t LIS3MDL_CTRL_REG3_MD0 = 0x00; +constexpr uint8_t LIS3MDL_CTRL_REG3_MD0 = 0x01; constexpr uint8_t LIS3MDL_CTRL_REG3_MD1 = 0x02; -// Constans: sensor sensitivity depending on selectable full scales +// Sensor sensitivity depending on selectable full scales // Use the datasheet for details constexpr float SENS_4GAUSS = 6842; constexpr float SENS_8GAUSS = 3421; diff --git a/src/Gyroscope.cpp b/src/Gyroscope.cpp index 39873b6..ddfcde5 100644 --- a/src/Gyroscope.cpp +++ b/src/Gyroscope.cpp @@ -6,28 +6,30 @@ Gyroscope::Gyroscope(uint8_t slaveAddress) void Gyroscope::begin(TwoWire& wire) { _wire = &wire; _wire->begin(); - _ctrlReg1 |= L3G4200D_CTRL_REG1_X_EN | L3G4200D_CTRL_REG1_Y_EN - | L3G4200D_CTRL_REG1_Z_EN; - _ctrlReg1 |= L3G4200D_CTRL_REG1_PD; - _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); + _scalingFactor = 1; + uint8_t data = L3G4200D_CTRL_REG1_X_EN | L3G4200D_CTRL_REG1_Y_EN + | L3G4200D_CTRL_REG1_Z_EN; + data |= L3G4200D_CTRL_REG1_PD; + _writeByte(BASE_IMU_CTRL_REG1, data); setRange(GyroscopeRange::RANGE_2000DPS); } // Set range scale output data from datasheet void Gyroscope::setRange(GyroscopeRange range) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG4); + data &= ~(L3G4200D_CTRL_REG4_FS0 | L3G4200D_CTRL_REG4_FS1); switch (range) { case GyroscopeRange::RANGE_250DPS: { - _ctrlReg4 = 0; _scalingFactor = SENS_250DPS; break; } case GyroscopeRange::RANGE_500DPS: { - _ctrlReg4 = L3G4200D_CTRL_REG4_FS0; + data |= L3G4200D_CTRL_REG4_FS0; _scalingFactor = SENS_500DPS; break; } case GyroscopeRange::RANGE_2000DPS: { - _ctrlReg4 = L3G4200D_CTRL_REG4_FS0 | L3G4200D_CTRL_REG4_FS1; + data |= L3G4200D_CTRL_REG4_FS0 | L3G4200D_CTRL_REG4_FS1; _scalingFactor = SENS_2000DPS; break; } @@ -35,16 +37,17 @@ void Gyroscope::setRange(GyroscopeRange range) { _scalingFactor = SENS_250DPS; } break; } - _writeByte(BASE_IMU_CTRL_REG4, _ctrlReg4); + _writeByte(BASE_IMU_CTRL_REG4, data); } void Gyroscope::sleep(bool state) { + uint8_t data = _readByte(BASE_IMU_CTRL_REG1); if (state) { - _ctrlReg1 &= ~L3G4200D_CTRL_REG1_PD; + data &= ~L3G4200D_CTRL_REG1_PD; } else { - _ctrlReg1 |= L3G4200D_CTRL_REG1_PD; + data |= L3G4200D_CTRL_REG1_PD; } - _writeByte(BASE_IMU_CTRL_REG1, _ctrlReg1); + _writeByte(BASE_IMU_CTRL_REG1, data); } float Gyroscope::readRotationDegX() { return readX() * _scalingFactor; } diff --git a/src/Gyroscope.h b/src/Gyroscope.h index dd29a45..9830dcd 100644 --- a/src/Gyroscope.h +++ b/src/Gyroscope.h @@ -17,7 +17,7 @@ constexpr uint8_t L3G4200D_CTRL_REG1_Y_EN = 0x02; constexpr uint8_t L3G4200D_CTRL_REG1_Z_EN = 0x04; constexpr uint8_t L3G4200D_CTRL_REG1_PD = 0x08; -// Constans: sensor sensitivity depending on selectable full scales +// Sensor sensitivity depending on selectable full scales // Use the datasheet for details constexpr float SENS_250DPS = 0.00875; constexpr float SENS_500DPS = 0.0175; From 573a19e3eec2ec573d431492eaaec229022fdf3d Mon Sep 17 00:00:00 2001 From: igor605ds Date: Mon, 2 Nov 2020 06:06:26 +0200 Subject: [PATCH 19/20] apply more suggestions from code review --- examples/Compass/CompassAzimuth/CompassAzimuth.ino | 2 +- examples/IMU/Madgwick6DOF/Madgwick6DOF.ino | 8 ++++---- examples/IMU/Madgwick9DOF/Madgwick9DOF.ino | 10 +++++----- .../Madgwick6DOFVisualization.ino | 10 +++++----- .../Madgwick9DOFVisualization.ino | 10 +++++----- .../MadgwickVisualizationDraw.pde | 8 ++++---- src/Accelerometer.cpp | 6 +++--- src/Accelerometer.h | 6 +++--- src/Compass.cpp | 6 ++---- 9 files changed, 32 insertions(+), 34 deletions(-) diff --git a/examples/Compass/CompassAzimuth/CompassAzimuth.ino b/examples/Compass/CompassAzimuth/CompassAzimuth.ino index 97a6d46..55ae7fe 100644 --- a/examples/Compass/CompassAzimuth/CompassAzimuth.ino +++ b/examples/Compass/CompassAzimuth/CompassAzimuth.ino @@ -5,7 +5,7 @@ Compass compass; // Калибровочные данные для работы магнитометра в режиме компаса -// Подробности читайте в документации про калиборку модуля +// Подробности читайте в документации про калибровку модуля // http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; diff --git a/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino index fdfe6ba..0ce273a 100644 --- a/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino +++ b/examples/IMU/Madgwick6DOF/Madgwick6DOF.ino @@ -8,14 +8,14 @@ Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; -// Переменные для данных с гироска и акселерометра +// Переменные для данных с гироскопа и акселерометра float gx, gy, gz, ax, ay, az; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; // Переменная для хранения частоты выборок фильтра -float fps = 100; +float sampleRate = 100; void setup() { // Открываем последовательный порт @@ -40,7 +40,7 @@ void loop() { // Считываем данные с гироскопа в радианах в секунду gyroscope.readRotationRadXYZ(gx, gy, gz); // Устанавливаем частоту фильтра - filter.setFrequency(fps); + filter.setFrequency(sampleRate); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az); @@ -62,5 +62,5 @@ void loop() { // Вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // Вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; + sampleRate = 1000 / deltaMillis; } diff --git a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino index 509489a..c3b59db 100644 --- a/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino +++ b/examples/IMU/Madgwick9DOF/Madgwick9DOF.ino @@ -10,17 +10,17 @@ Accelerometer accelerometer; // Создаём объект для работы с магнитометром/компасом Compass compass; -// Переменные для данных с гироска, акселерометра и компаса +// Переменные для данных с гироскопа, акселерометра и компаса float gx, gy, gz, ax, ay, az, mx, my, mz; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; // Переменная для хранения частоты выборок фильтра -float fps = 100; +float sampleRate = 100; // Калибровочные данные для работы магнитометра в режиме компаса -// Подробности читайте в документации про калиборку модуля +// Подробности читайте в документации про калибровку модуля // http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 }; @@ -59,7 +59,7 @@ void loop() { // Считываем данные с компаса в Гауссах compass.readCalibrateMagneticGaussXYZ(mx, my, mz); // Устанавливаем частоту фильтра - filter.setFrequency(fps); + filter.setFrequency(sampleRate); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); @@ -81,5 +81,5 @@ void loop() { // Вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // Вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; + sampleRate = 1000 / deltaMillis; } diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino index 5493383..603747a 100644 --- a/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick6DOFVisualization/Madgwick6DOFVisualization.ino @@ -8,14 +8,14 @@ Gyroscope gyroscope; // Создаём объект для работы с акселерометром Accelerometer accelerometer; -// Переменные для данных с гироска и акселерометра и компаса +// Переменные для данных с гироскопа и акселерометра и компаса float gx, gy, gz, ax, ay, az; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; // Переменная для хранения частоты выборок фильтра -float fps = 100; +float sampleRate = 100; void setup() { // Открываем последовательный порт @@ -38,7 +38,7 @@ void loop() { gyroscope.readRotationRadXYZ(gx, gy, gz); // Устанавливаем частоту фильтра - filter.setFrequency(fps); + filter.setFrequency(sampleRate); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az); @@ -48,7 +48,7 @@ void loop() { if (val == 's') { float q0, q1, q2, q3; filter.readQuaternion(q0, q1, q2, q3); - // Выводим кватернионы в serial-порт + // Выводим кватернион в serial-порт Serial.print(q0); Serial.print(","); Serial.print(q1); @@ -61,5 +61,5 @@ void loop() { // Вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // Вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; + sampleRate = 1000 / deltaMillis; } diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino index 42a7931..b50bc97 100644 --- a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino @@ -10,14 +10,14 @@ Accelerometer accelerometer; // Создаём объект для работы с магнитометром/компасом Compass compass; -// Переменные для данных с гироска, акселерометра и компаса +// Переменные для данных с гироскопа, акселерометра и компаса float gx, gy, gz, ax, ay, az, mx, my, mz; // Переменные для хранения самолётных углов ориентации float yaw, pitch, roll; // Переменная для хранения частоты фильтра -float fps = 100; +float sampleRate = 100; // Калибровочные данные для работы магнитометра в режиме компаса // Подробности читайте в документации про калиборку модуля @@ -55,7 +55,7 @@ void loop() { // Считываем данные с компаса в Гауссах compass.readCalibrateMagneticGaussXYZ(mx, my, mz); // Устанавливаем частоту фильтра - filter.setFrequency(fps); + filter.setFrequency(sampleRate); // Обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); @@ -65,7 +65,7 @@ void loop() { if (val == 's') { float q0, q1, q2, q3; filter.readQuaternion(q0, q1, q2, q3); - // Выводим кватернионы в serial-порт + // Выводим кватернион в serial-порт Serial.print(q0); Serial.print(","); Serial.print(q1); @@ -78,5 +78,5 @@ void loop() { // Вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // Вычисляем частоту обработки фильтра - fps = 1000 / deltaMillis; + sampleRate = 1000 / deltaMillis; } diff --git a/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde b/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde index 339cacc..b834f74 100644 --- a/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde +++ b/examples/IMU/MadgwickVisualization/Processing/MadgwickVisualizationDraw/MadgwickVisualizationDraw.pde @@ -21,7 +21,7 @@ String [] massQ = new String [4]; float[] ypr = new float[3]; void setup() { - // Size form 300x300 + // Size form 400x400 size(400, 400, P3D); // Open serial port // Replace "COM7" with the COM port on which your arduino is connected @@ -47,8 +47,8 @@ void draw() { drawCylinder(); // Draw Triangles drawTriangles(); - // Draw Quards - drawQuards(); + // Draw Quads + drawQuads(); // End of object popMatrix(); // Send character 's' to Arduino @@ -150,7 +150,7 @@ void drawTriangles() { endShape(); } -void drawQuards() { +void drawQuads() { beginShape(QUADS); vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); diff --git a/src/Accelerometer.cpp b/src/Accelerometer.cpp index ade6138..8a70046 100644 --- a/src/Accelerometer.cpp +++ b/src/Accelerometer.cpp @@ -20,17 +20,17 @@ void Accelerometer::setRange(AccelerometerRange range) { data &= ~(LIS331DLH_CTRL_REG4_FS0 | LIS331DLH_CTRL_REG4_FS1); switch (range) { case AccelerometerRange::RANGE_2G: { - _scalingFactor = SENS_2G * 4 / pow(2, 16); + _scalingFactor = SENS_2G; break; } case AccelerometerRange::RANGE_4G: { data |= LIS331DLH_CTRL_REG4_FS0; - _scalingFactor = SENS_4G * 4 / pow(2, 16); + _scalingFactor = SENS_4G; break; } case AccelerometerRange::RANGE_8G: { data |= LIS331DLH_CTRL_REG4_FS0 | LIS331DLH_CTRL_REG4_FS1; - _scalingFactor = SENS_8G * 4 / pow(2, 16); + _scalingFactor = SENS_8G; break; } default: { diff --git a/src/Accelerometer.h b/src/Accelerometer.h index 47db82a..b08b1c9 100644 --- a/src/Accelerometer.h +++ b/src/Accelerometer.h @@ -24,9 +24,9 @@ constexpr float GRAVITY_EARTH = 9.8; // Sensor sensitivity depending on selectable full scales // Use the datasheet for details -constexpr float SENS_2G = 1; -constexpr float SENS_4G = 2; -constexpr float SENS_8G = 3.9; +constexpr float SENS_2G = 1 * 4 / pow(2, 16); +constexpr float SENS_4G = 2 * 4 / pow(2, 16); +constexpr float SENS_8G = 3.9 * 4 / pow(2, 16); enum class AccelerometerRange { RANGE_2G = 1, RANGE_4G = 2, RANGE_8G = 3 }; diff --git a/src/Compass.cpp b/src/Compass.cpp index c5df4a7..56cafe2 100644 --- a/src/Compass.cpp +++ b/src/Compass.cpp @@ -108,11 +108,9 @@ float Compass::readAzimut() { float mx = x, my = y, mz = z; _calibrate(mx, my, mz); float heading = atan2(mx, my); - - if (heading < 0) + if (heading < 0) { heading += TWO_PI; - else if (heading > TWO_PI) - heading -= TWO_PI; + } float headingDegrees = heading * RAD_TO_DEG; return headingDegrees; } From 35f0b2252d364e0f9154be44886ba705125c3b16 Mon Sep 17 00:00:00 2001 From: igor605ds Date: Mon, 2 Nov 2020 13:38:05 +0200 Subject: [PATCH 20/20] fix grammatical errors --- examples/Barometer/BarometerSimple/BarometerSimple.ino | 2 +- examples/Compass/CompassCalibration/CompassCalibration.ino | 2 +- .../Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/Barometer/BarometerSimple/BarometerSimple.ino b/examples/Barometer/BarometerSimple/BarometerSimple.ino index 2fe731d..f0a4b44 100644 --- a/examples/Barometer/BarometerSimple/BarometerSimple.ino +++ b/examples/Barometer/BarometerSimple/BarometerSimple.ino @@ -5,7 +5,7 @@ Barometer barometer; void setup() { - // Окрываем Serial-порт + // Открываем Serial-порт Serial.begin(9600); // Выводим сообщение о начале инициализации Serial.println("Barometer begin"); diff --git a/examples/Compass/CompassCalibration/CompassCalibration.ino b/examples/Compass/CompassCalibration/CompassCalibration.ino index 421d5f4..26fdb80 100644 --- a/examples/Compass/CompassCalibration/CompassCalibration.ino +++ b/examples/Compass/CompassCalibration/CompassCalibration.ino @@ -1,6 +1,6 @@ // Пример выводит сырые значения магнитометра, которые // используются для последующей калибровки сенсора -// Подробности читайте в документации про калиборку магнитометра +// Подробности читайте в документации про калибровку магнитометра // http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate // Библиотека для работы с модулями IMU diff --git a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino index b50bc97..aacd248 100644 --- a/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino +++ b/examples/IMU/MadgwickVisualization/Arduino/Madgwick9DOFVisualization/Madgwick9DOFVisualization.ino @@ -20,7 +20,7 @@ float yaw, pitch, roll; float sampleRate = 100; // Калибровочные данные для работы магнитометра в режиме компаса -// Подробности читайте в документации про калиборку модуля +// Подробности читайте в документации про калибровку модуля // http://wiki.amperka.ru/articles:troyka-magnetometer-compass-calibrate const float compassCalibrationBias[3] = { 567.893, -825.35, 1061.436 };