Skip to content

Comitted changes made by Femme Verbeek #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
wants to merge 1 commit into from
Closed

Comitted changes made by Femme Verbeek #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
228 changes: 185 additions & 43 deletions src/LSM9DS1.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,11 @@
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

Modifications by Femme Verbeek, Pijnacker, the Netherlands 23 may 2020,
Released to the public domain
version 2.0.0

*/

#include "LSM9DS1.h"
Expand All @@ -38,6 +43,14 @@
#define LSM9DS1_STATUS_REG_M 0x27
#define LSM9DS1_OUT_X_L_M 0x28

// Add these right after defines at the beginning
#define LSM9DS1_OFFSET_X_REG_L_M 0x05
#define LSM9DS1_OFFSET_X_REG_H_M 0x06
#define LSM9DS1_OFFSET_Y_REG_L_M 0x07
#define LSM9DS1_OFFSET_Y_REG_H_M 0x08
#define LSM9DS1_OFFSET_Z_REG_L_M 0x09
#define LSM9DS1_OFFSET_Z_REG_H_M 0x0A

LSM9DS1Class::LSM9DS1Class(TwoWire& wire) :
continuousMode(false), _wire(&wire)
{
Expand Down Expand Up @@ -106,26 +119,25 @@ void LSM9DS1Class::end()
_wire->end();
}

int LSM9DS1Class::readAcceleration(float& x, float& y, float& z)
{
int16_t data[3];
//************************************ Accelleration *****************************************

int LSM9DS1Class::readAccel(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;

// See releasenotes read = Unit * Slope * (PFS / 32786 * Data - Offset )
float scale = getAccelFS()/32768.0 ;
x = accelUnit * accelSlope[0] * (scale * data[0] - accelOffset[0]);
y = accelUnit * accelSlope[1] * (scale * data[1] - accelOffset[1]);
z = accelUnit * accelSlope[2] * (scale * data[2] - accelOffset[2]);
return 1;
}

int LSM9DS1Class::accelerationAvailable()
int LSM9DS1Class::accelAvailable()
{
if (continuousMode) {
// Read FIFO_SRC. If any of the rightmost 8 bits have a value, there is data.
Expand All @@ -137,81 +149,211 @@ int LSM9DS1Class::accelerationAvailable()
return 1;
}
}

return 0;
}

float LSM9DS1Class::accelerationSampleRate()
{
return 119.0F;
void LSM9DS1Class::setAccelOffset(float x, float y, float z)
{ accelOffset[0] = x /(accelUnit * gyroSlope[0]);
accelOffset[1] = y /(accelUnit * gyroSlope[1]);
accelOffset[2] = z /(accelUnit * gyroSlope[2]);
}

int LSM9DS1Class::readGyroscope(float& x, float& y, float& z)
{
int16_t data[3];
void LSM9DS1Class::setAccelSlope(float x, float y, float z)
{ accelSlope[0] = x ;
accelSlope[1] = y ;
accelSlope[2] = z ;
}

if (!readRegisters(LSM9DS1_ADDRESS, LSM9DS1_OUT_X_G, (uint8_t*)data, sizeof(data))) {
x = NAN;
int LSM9DS1Class::setAccelODR(int8_t range) //Sample Rate 0:off, 1:10Hz, 2:50Hz, 3:119Hz, 4:238Hz, 5:476Hz, 6:952Hz, 7:NA
{ if (range==7) range =0;
range = (range & 0b00000111) << 5;
uint8_t setting = ((readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL) & 0b00011111) | range);
return writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL,setting) ;
}
float LSM9DS1Class::getAccelODR()
{ float Ranges[] ={0.0, 10.0, 50.0, 119.0, 238.0, 476.0, 952.0, 0.0 };
uint8_t setting = readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL) >> 5;
return Ranges [setting];
}

float LSM9DS1Class::setAccelBW(int8_t range) //0,1,2,3 Override autoBandwidth setting see doc.table 67
{ range = range & 0b00000011;
uint8_t RegIs = readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL) & 0b11111000;
RegIs = RegIs | 0b00000100 | range ;
return writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL,RegIs) ;
}

float LSM9DS1Class::getAccelBW() //Bandwidth setting 0,1,2,3 see documentation table 67
{ float autoRange[] ={0.0, 408.0, 408.0, 50.0, 105.0, 211.0, 408.0, 0.0 };
float BWXLRange[] ={ 408.0, 211.0, 105.0, 50.0 };
uint8_t RegIs = readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL);
if (bitRead(2,RegIs)) return BWXLRange [RegIs & 0b00000011];
else return autoRange [ RegIs >> 5 ];
}

int LSM9DS1Class::setAccelFS(int8_t range) // 0: ±2g ; 1: ±16g ; 2: ±4g ; 3: ±8g
{ range = (range & 0b00000011) << 3;
uint8_t setting = ((readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL) & 0xE7) | range);
return writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL,setting) ;
}

float LSM9DS1Class::getAccelFS() // Full scale (output = 2.0, 16.0 , 4.0 , 8.0)
{ float ranges[] ={2.0, 16.0, 4.0, 8.0}; //g
uint8_t setting = (readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL) & 0x18) >> 3;
return ranges[setting] ;
}

//************************************ Gyroscope *****************************************

int LSM9DS1Class::readGyro(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;

float scale = getGyroFS() / 32768.0;
x = gyroUnit * gyroSlope[0] * (scale * data[0] - gyroOffset[0]);
y = gyroUnit * gyroSlope[1] * (scale * data[1] - gyroOffset[1]);
z = gyroUnit * gyroSlope[2] * (scale * data[2] - gyroOffset[2]);
return 1;
}

int LSM9DS1Class::gyroscopeAvailable()
int LSM9DS1Class::gyroAvailable()
{
if (readRegister(LSM9DS1_ADDRESS, LSM9DS1_STATUS_REG) & 0x02) {
return 1;
}

return 0;
}

float LSM9DS1Class::gyroscopeSampleRate()
{
return 119.0F;
void LSM9DS1Class::setGyroOffset(float x, float y, float z)
{ gyroOffset[0] = x /(gyroUnit * gyroSlope[0]);
gyroOffset[1] = y /(gyroUnit * gyroSlope[1]);
gyroOffset[2] = z /(gyroUnit * gyroSlope[2]);
}

void LSM9DS1Class::setGyroSlope(float x, float y, float z)
{ gyroSlope[0] = x ;
gyroSlope[1] = y ;
gyroSlope[2] = z ;
}

int LSM9DS1Class::setGyroODR(int8_t range) // 0:off, 1:10Hz, 2:50Hz, 3:119Hz, 4:238Hz, 5:476Hz, 6:952Hz, 7:NA
{ if (range==7) range =0;
range = (range & 0b00000111) << 5;
uint8_t setting = ((readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G) & 0b00011111) | range);
return writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G,setting) ;
}

float LSM9DS1Class::getGyroODR()
{ float Ranges[] ={0.0, 10.0, 50.0, 119.0, 238.0, 476.0, 952.0, 0.0 }; //Hz
uint8_t setting = readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G) >> 5;
return Ranges [setting]; // return 119.0F;
}

int LSM9DS1Class::setGyroBW(int8_t range)
{ range = range & 0b00000011;
uint8_t setting = readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G) & 0b11111100;
return writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G,setting | range) ;
}

#define ODRrows 8
#define BWcols 4
float BWtable[ ODRrows ][ BWcols ] = // acc to
{ { 0, 0, 0, 0 },
{ 0, 0, 0, 0 },
{ 16, 16, 16, 16 },
{ 14, 31, 31, 31 },
{ 14, 29, 63, 78 },
{ 21, 28, 57, 100 },
{ 33, 40, 58, 100 },
{ 0, 0, 0, 0 } };

float LSM9DS1Class::getGyroBW()
{ uint8_t setting = readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G) ;
uint8_t ODR = setting >> 5;
uint8_t BW = setting & 0b00000011;
return BWtable[ODR][BW];
}

int LSM9DS1Class::setGyroFS(int8_t range) // (0: 245 dps; 1: 500 dps; 2: 1000 dps; 3: 2000 dps)
{ range = (range & 0b00000011) << 3;
uint8_t setting = ((readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G) & 0xE7) | range );
return writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G,setting) ;
}

float LSM9DS1Class::getGyroFS() //
{ float Ranges[] ={245.0, 500.0, 1000.0, 2000.0}; //dps
uint8_t setting = (readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G) & 0x18) >> 3;
return Ranges[setting] ;
}

//************************************ Magnetic field *****************************************

int LSM9DS1Class::readMagneticField(float& x, float& y, float& z)
{
int16_t data[3];
{ 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;

float scale = getMagnetFS() / 32768.0;
x = magnetUnit * magnetSlope[0] * (scale * data[0] - magnetOffset[0]);
y = magnetUnit * magnetSlope[1] * (scale * data[1] - magnetOffset[1]);
z = magnetUnit * magnetSlope[2] * (scale * data[2] - magnetOffset[2]);
return 1;
}

int LSM9DS1Class::magneticFieldAvailable()
{
{ //return (readRegister(LSM9DS1_ADDRESS_M, LSM9DS1_STATUS_REG_M) & 0x08)==0x08;
if (readRegister(LSM9DS1_ADDRESS_M, LSM9DS1_STATUS_REG_M) & 0x08) {
return 1;
}

return 0;
}

float LSM9DS1Class::magneticFieldSampleRate()
{
return 20.0;
void LSM9DS1Class::setMagnetOffset(float x, float y, float z)
{ magnetOffset[0] = x /(magnetUnit * magnetSlope[0]);
magnetOffset[1] = y /(magnetUnit * magnetSlope[1]);
magnetOffset[2] = z /(magnetUnit * magnetSlope[2]);
}

void LSM9DS1Class::setMagnetSlope(float x, float y, float z)
{ magnetSlope[0] = x ;
magnetSlope[1] = y ;
magnetSlope[2] = z ;
}

int LSM9DS1Class::setMagnetFS(int8_t range) // 0=400.0; 1=800.0; 2=1200.0 , 3=1600.0 (µT)
{ range = (range & 0b00000011) << 5;
return writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG2_M,range) ;
}

float LSM9DS1Class::getMagnetFS() //
{ const float Ranges[] ={400.0, 800.0, 1200.0, 1600.0}; //
uint8_t setting = readRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG2_M) >> 5;
return Ranges[setting] ;
}

int LSM9DS1Class::setMagnetODR(int8_t range) // range (0..7) corresponds to {0.625,1.25,2.5,5.0,10.0,20.0,40.0,80.0}Hz
{ range = (range & 0b00000111) << 2;
uint8_t setting = ((readRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG1_M) & 0b11100011) | range);
return writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG1_M,setting) ;
}

float LSM9DS1Class::getMagnetODR() // Output {0.625, 1.25, 2.5, 5.0, 10.0, 20.0, 40.0 , 80.0}; //Hz
{ const float ranges[] ={0.625, 1.25,2.5, 5.0, 10.0, 20.0, 40.0 , 80.0}; //Hz
uint8_t setting = (readRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG1_M) & 0b00011100) >> 2;
return ranges[setting];
}

//************************************ Private functions *****************************************


int LSM9DS1Class::readRegister(uint8_t slaveAddress, uint8_t address)
{
_wire->beginTransmission(slaveAddress);
Expand Down
Loading