/*
This file is part of the Arduino_LSM9DS1 library.
Copyright (c) 2019 Arduino SA. All rights reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "libLSM9DS1/LSM9DS1.h"
#define LSM9DS1_ADDRESS 0x6b
#define LSM9DS1_WHO_AM_I 0x0f
#define LSM9DS1_CTRL_REG1_G 0x10
#define LSM9DS1_STATUS_REG 0x17
#define LSM9DS1_OUT_X_G 0x18
#define LSM9DS1_CTRL_REG6_XL 0x20
#define LSM9DS1_CTRL_REG8 0x22
#define LSM9DS1_OUT_X_XL 0x28
// magnetometer
#define LSM9DS1_ADDRESS_M 0x1e
#define LSM9DS1_CTRL_REG1_M 0x20
#define LSM9DS1_CTRL_REG2_M 0x21
#define LSM9DS1_CTRL_REG3_M 0x22
#define LSM9DS1_STATUS_REG_M 0x27
#define LSM9DS1_OUT_X_L_M 0x28
LSM9DS1Class::LSM9DS1Class(I2C_HandleTypeDef* i2c) :
continuousMode(false), _i2c(i2c)
{
}
LSM9DS1Class::~LSM9DS1Class()
{
}
int LSM9DS1Class::begin()
{
// reset
writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG8, 0x05);
writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG2_M, 0x0c);
HAL_Delay(10);
if (readRegister(LSM9DS1_ADDRESS, LSM9DS1_WHO_AM_I) != 0x68) {
end();
return 0;
}
if (readRegister(LSM9DS1_ADDRESS_M, LSM9DS1_WHO_AM_I) != 0x3d) {
end();
return 0;
}
writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G, 0x78); // 119 Hz, 2000 dps, 16 Hz BW
writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL, 0x70); // 119 Hz, 4g
writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG1_M, 0xb4); // Temperature compensation enable, medium performance, 20 Hz
writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG2_M, 0x00); // 4 gauss
writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG3_M, 0x00); // Continuous conversion mode
return 1;
}
void LSM9DS1Class::setContinuousMode() {
// Enable FIFO (see docs https://www.st.com/resource/en/datasheet/DM00103319.pdf)
writeRegister(LSM9DS1_ADDRESS, 0x23, 0x02);
// Set continuous mode
writeRegister(LSM9DS1_ADDRESS, 0x2E, 0xC0);
continuousMode = true;
}
void LSM9DS1Class::setOneShotMode() {
// Disable FIFO (see docs https://www.st.com/resource/en/datasheet/DM00103319.pdf)
writeRegister(LSM9DS1_ADDRESS, 0x23, 0x00);
// Disable continuous mode
writeRegister(LSM9DS1_ADDRESS, 0x2E, 0x00);
continuousMode = false;
}
void LSM9DS1Class::end()
{
writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG3_M, 0x03);
writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G, 0x00);
writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL, 0x00);
}
int LSM9DS1Class::readAcceleration(float& x, float& y, float& z)
{
int16_t data[3];
if (!readRegisters(LSM9DS1_ADDRESS, LSM9DS1_OUT_X_XL, (uint8_t*)data, sizeof(data))) {
x = NAN;
y = NAN;
z = NAN;
return 0;
}
x = data[0] * 4.0 / 32768.0;
y = data[1] * 4.0 / 32768.0;
z = data[2] * 4.0 / 32768.0;
return 1;
}
int LSM9DS1Class::accelerationAvailable()
{
if (continuousMode) {
// Read FIFO_SRC. If any of the rightmost 8 bits have a value, there is data.
if (readRegister(LSM9DS1_ADDRESS, 0x2F) & 63) {
return 1;
}
} else {
if (readRegister(LSM9DS1_ADDRESS, LSM9DS1_STATUS_REG) & 0x01) {
return 1;
}
}
return 0;
}
float LSM9DS1Class::accelerationSampleRate()
{
return 119.0F;
}
int LSM9DS1Class::readGyroscope(float& x, float& y, float& z)
{
int16_t data[3];
if (!readRegisters(LSM9DS1_ADDRESS, LSM9DS1_OUT_X_G, (uint8_t*)data, sizeof(data))) {
x = NAN;
y = NAN;
z = NAN;
return 0;
}
x = data[0] * 2000.0 / 32768.0;
y = data[1] * 2000.0 / 32768.0;
z = data[2] * 2000.0 / 32768.0;
return 1;
}
int LSM9DS1Class::gyroscopeAvailable()
{
if (readRegister(LSM9DS1_ADDRESS, LSM9DS1_STATUS_REG) & 0x02) {
return 1;
}
return 0;
}
float LSM9DS1Class::gyroscopeSampleRate()
{
return 119.0F;
}
int LSM9DS1Class::readMagneticField(float& x, float& y, float& z)
{
int16_t data[3];
if (!readRegisters(LSM9DS1_ADDRESS_M, LSM9DS1_OUT_X_L_M, (uint8_t*)data, sizeof(data))) {
x = NAN;
y = NAN;
z = NAN;
return 0;
}
x = data[0] * 4.0 * 100.0 / 32768.0;
y = data[1] * 4.0 * 100.0 / 32768.0;
z = data[2] * 4.0 * 100.0 / 32768.0;
return 1;
}
int LSM9DS1Class::magneticFieldAvailable()
{
if (readRegister(LSM9DS1_ADDRESS_M, LSM9DS1_STATUS_REG_M) & 0x08) {
return 1;
}
return 0;
}
float LSM9DS1Class::magneticFieldSampleRate()
{
return 20.0;
}
int LSM9DS1Class::readRegister(uint8_t slaveAddress, uint8_t address)
{
uint8_t c;
HAL_StatusTypeDef st = HAL_I2C_Mem_Read(_i2c, slaveAddress<<1, address, 1, &c, 1, 10000);
return c;
}
int LSM9DS1Class::readRegisters(uint8_t slaveAddress, uint8_t address, uint8_t* data, size_t length)
{
HAL_StatusTypeDef st = HAL_I2C_Mem_Read(_i2c, slaveAddress<<1, address, 1, data, length, 10000);
return st == HAL_OK ? 1 : 0;
}
int LSM9DS1Class::writeRegister(uint8_t slaveAddress, uint8_t address, uint8_t value)
{
uint8_t c= value;
HAL_StatusTypeDef st = HAL_I2C_Mem_Write (_i2c, slaveAddress << 1, address, 1,
&c, 1, 10000);
return st == HAL_OK ? 1 : 0;
}