Skip to content

New version 2.0 for the LSM9DS1 Library #12

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.

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

Open
wants to merge 18 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
16 changes: 16 additions & 0 deletions CHANGELOG
Original file line number Diff line number Diff line change
Expand Up @@ -7,3 +7,19 @@ Arduino_LSM9DS1 1.0.0 - 2019.07.31
Arduino_LSM9DS1 1.1.0 - 2020.02.11

* Added support for FIFO continuous reading of values

Arduino_LSM9DS1 2.0.0 - 2020.07.10

* Added support for Full scale setting,
* Added support for Operational mode: off, Accel only, Gyro + Accel
* Added support for ODR sample rate, ODR values are automatically calibrated, include fast ODR for magnet
* Added support for Band width
* Added support for changing output unit
* Added support for separate or combined sensor calibration on all 9 DOF
* Calibration parameters are dimensionless, independent of each other and of the other settings
* Includes DIY calibration programs producing copy/paste-able code
* Includes readme.md and Getting Started.md
* Includes examples RPM_meter_Rev_Counter, Water_Leveler, XY_Compass, SimpleGyroscope, SimpleAccelerometer, SimpleMagnetometer
* Includes Register_test: demonstrates and verifies all new settings
* The included programs use a minimal amount of RAM to enable running on something as small as an Arduino Uno.
* The IMU.end() function restores the system to low power usage
68 changes: 68 additions & 0 deletions Getting Started.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
# Getting started with LSM9DS1 V2 library

The library provides methods to get IMU data out of the LSM9DS1 chip and was developed on an
Arduino Nano 33 BLE Sense board.
This V2 library was designed to replace the older version 1.01
More information about the LSM9DS1 chip can be found in the [datasheet](https://www.st.com/resource/en/datasheet/lsm9ds1.pdf)

## Installation

In the Arduino IDE select: *Sketch \ Include Library \ Add .ZIP Library* and point to the location where the .Zip file is stored.
When the library has been added to the Arduino library list, it will be possible to use the
Library manager as well.

If there was a previous version of the LSM9DS1 library already installed you should probably delete or remove that first
from the Arduino library installation directory

..../Arduino/libraries/Arduino_LSM9DS1

## Calibration

If you want to make use of the new calibration possibilities run the interactive example sketches
DIY_Calibration first. Follow the directions on the screen of the serial monitor. Copy the code
from the screen into the setup() of your sketch.
See [instruction video](https://youtu.be/BLvYFXoP33o)

It turned out that the best calibration for the Gyroscope depended on the chip's Full Scale and Output Data Rate.
For this reason changing these settings has been added in the calibration programs.

The functions readAccel, readGyro, readMagnet produce calibrated data in the unit of choise
The functions readRawAccel, readRawGyro, readRawMagnet produce uncalibrated data and are meant for calibration purposes.

## Use any of the new setting possibilities by placing commands in your sketch
All the "set" methods have a corresponding "get" function.

**Output Data Rate**
(for read... and readRaw... functions)
```
(range)= (0..5) -> { off, 10, 50, 119, 238, 476} Hz default = 119hz
IMU.setAccelODR(range);
IMU.setGyroODR (range);

(range)= (0..8) -> {0.625, 1.25, 2.5, 5, 10, 20, 40, 80, 400} Hz default = 20hz
IMU.setMagnetODR(range);
```

**Full Scale setting**
(for read... and readRaw... functions)
```
IMU.setAccelFS(range); // 0: ±2g ; 1: ±24g ; 2: ±4g ; 3: ±8g
IMU.setGyroFS(range); // (0= ±245, 1= ±500, 2= ±1000, 3= ±2000) °/s
IMU.setMagnetFS(range); // 0=±400.0; 1=±800.0; 2=±1200.0 , 3=±1600.0 (µT)
```

**Output unit unit you want to get the output in**
(for read... and readRaw... functions)
```
IMU.accelUnit = GRAVITY; // GRAVITY OR METERPERSECOND2
IMU.gyroUnit = DEGREEPERSECOND; // DEGREEPERSECOND RADIANSPERSECOND REVSPERMINUTE REVSPERSECOND
IMU.magnetUnit = MICROTESLA; // GAUSS, MICROTESLA NANOTESLA
```
**Choose a BandWidth filter**
For advanced users only
```
IMU.setAccelBW(range); //0,1,2,3 Override autoBandwidth setting see doc.table 67
IMU.setGyroBW(range); //Bandwidth setting 0,1,2,3 see doc. table 46 and 47

```

1 change: 1 addition & 0 deletions README.adoc → Licence.adoc
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
image:https://github.com/arduino-libraries/Arduino_LSM9DS1/workflows/Compile%20Examples/badge.svg["Compile Examples Status", link="https://github.com/arduino-libraries/Arduino_LSM9DS1/actions?workflow=Compile+Examples"] image:https://github.com/arduino-libraries/Arduino_LSM9DS1/workflows/Spell%20Check/badge.svg["Spell Check Status", link="https://github.com/arduino-libraries/Arduino_LSM9DS1/actions?workflow=Spell+Check"]

Allows you to read the accelerometer, magnetometer and gyroscope values from the LSM9DS1 IMU on your Arduino Nano 33 BLE Sense.
For more accurate measurements the sensor must be calibrated first.

== License ==

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,214 @@
/* DIY calibration program for the LSM9DS1 chip
*
* Follow the instructions on the screen how to do calibration measurements.
* See instruction video https://youtu.be/BLvYFXoP33o
* No special tools or setups are needed, however it is handy if the board with the LSM9DS1 chip is fitted inside
* a non-metalic rectangular box.
* The Full Scale (FS)and Output DATA Rate (ODR) settings as well as offset and slope factors
* are displayed on screen as code that can be copy/pasted directly into a sketch.
* Each new instance of the chip will require it's own unique set of calibration factors.
* It is recommended that the sketch uses the same FS and ODR settings as the calibration program
*
* Menu operation: type a letter in the input box of the serial monitor followed by enter
*
* written by Femme Verbeek 6 July 2020
*
* This program uses V2 of the LSM9DS1 library
* Tested on an Arduino Nano 33 BLE Sense board.
*
*
*/

#include <Arduino_LSM9DS1.h>


const float accelCriterion = 0.1;
char xyz[3]= {'X','Y','Z'};
float maxAX = 1, maxAY=1, maxAZ=1, minAX=-1, minAY=-1, minAZ=-1; // Accel Slope
float zeroAX1 =0,zeroAX2 =0,zeroAY1 =0,zeroAY2 =0,zeroAZ1 =0,zeroAZ2 =0; //Accel Offset
boolean accelOK=false;
uint8_t acceMMlOK=0; // bit 0..2 maxXYZ bit 3..5 minXYZ
uint8_t accelODRindex=5; // Sample Rate 0:off, 1:10Hz, 2:50Hz, 3:119Hz, 4:238Hz, 5:476Hz, (6:952Hz=na)
uint8_t accelFSindex=3; // Full Scale// 0: ±2g ; 1: ±24g ; 2: ±4g ; 3: ±8g

void setup() {
Serial.begin(115200);
while (!Serial);
pinMode(LED_BUILTIN,OUTPUT);
delay(10);
if (!IMU.begin()) { Serial.println(F("Failed to initialize IMU!")); while (1); }
IMU.setAccelFS(accelFSindex);
IMU.setAccelODR(accelODRindex);
calibrateAccelMenu();
}

void loop(){ }

void printParam(char txt[], float param[3])
{ for (int i= 0; i<=2 ; i++)
{ Serial.print(txt);Serial.print("[");
Serial.print(i);Serial.print("] = ");
Serial.print(param[i],6);Serial.print(";");
}
}
void printSetParam(char txt[], float param[3])
{ Serial.print(txt);Serial.print("(");
Serial.print(param[0],6);Serial.print(", ");
Serial.print(param[1],6);Serial.print(", ");
Serial.print(param[2],6);Serial.print(");");
}
//**********************************************************************************************************************************
//********************************************* Accelerometer **********************************************
//**********************************************************************************************************************************

void calibrateAccelMenu()
{char incomingByte = 0;
byte b;
uint16_t NofCalibrationSamples = 1000;
while (1) //(incomingByte!='X')
{ Serial.println(F("\n\n"));
Serial.println(F(" Calibrate Accelerometer Offset and Slope"));
Serial.println(F(" Before calibrating choose the Full Scale (FS) setting and Output Data Rate (ODR). The accelerometer and the"));
Serial.println(F(" gyroscope share their ODR, so the setting here must be the same as in the DIY_Calibration_Gyroscope sketch."));
Serial.println(F(" Place the board on a horizontal surface with one of its axes vertical and hit enter to start a calibration"));
Serial.println(F(" measurement. Each of the axes must be measured pointing up and pointing down, so a total of 6 measurements."));
Serial.println(F(" The program recognises which axis is vertical and shows which were measured successfully. If the angle is to"));
Serial.println(F(" far oblique the measurement is not valid.\n "));
Serial.println(F(" (enter) Start a calibration measurement. "));
Serial.print (F(" (N) Number of calibration samples "));Serial.println(NofCalibrationSamples);
Serial.print (F(" (F) Full Scale setting "));Serial.print(accelFSindex);Serial.print(" = ");Serial.print(IMU.getAccelFS(),0);Serial.println(F("g"));
Serial.print (F(" (R) Output Data Rate (ODR) setting "));Serial.print(accelODRindex);Serial.print(" = ");Serial.print(IMU.getAccelODR(),0);Serial.println(F("Hz (actual value)"));

// Serial.println("Press (X) to exit \n");

Serial.print(F(" Measured status of axis \n "));
for (int i=0;i<=2;i++){ Serial.print(xyz[i]); if (bitRead(acceMMlOK,i)==1)Serial.print("+ = ( -OK- ) "); else Serial.print("+ = not done "); }
Serial.print("\n ");
for (int i=0;i<=2;i++){ Serial.print(xyz[i]); if (bitRead(acceMMlOK,i+3)==1)Serial.print("- = ( -OK- ) "); else Serial.print("- = not done "); }

// Serial.println(F("\n\nCurrent accelerometer calibration values (copy/paste-able)\n"));
Serial.println(F("\n\n // Accelerometer code"));
Serial.print(F(" IMU.setAccelFS(")); Serial.print(accelFSindex);
Serial.print(F(");\n IMU.setAccelODR("));Serial.print(accelODRindex);Serial.println(");");

printSetParam(" IMU.setAccelOffset",IMU.accelOffset);
Serial.println();
printSetParam (" IMU.setAccelSlope ",IMU.accelSlope);
Serial.println("\n\n");
incomingByte= readChar(); //wait for and get keyboard input
switch (incomingByte)
{ case 'F': { Serial.print (F("\n\nEnter new FS nr 0: ±2g ; 1: ±24g ; 2: ±4g ; 3: ±8g > "));
b= readChar()-48; Serial.println(b);
if (b!=accelFSindex && b >=0 && b<=3) accelFSindex=b;
IMU.setAccelFS(accelFSindex);
Serial.print("\n\n\n\n\n\n\n\n\n");
break;}
case 'R': { Serial.print (F("\n\nEnter new ODR nr 1:10,2:50 3:119,4:238,5:476 Hz > "));
b= readChar()-48; //Serial.println(b);
if (b!=accelODRindex && b>=1 && b<=5) accelODRindex=b;
IMU.setAccelODR(accelODRindex);
Serial.print("\n\n\n\n\n\n\n\n\n");
break;
}
case 'N': { readAnswer("\n\n\n\n\n\nThe number of calibration samples ", NofCalibrationSamples);
break;}
case 'C': {};
default : calibrateAccel(NofCalibrationSamples);
}
}
}

void calibrateAccel(uint16_t NofSamples)
{ boolean validMmt=false;
float x,y,z;
Serial.println(F("\n\n\n\n\n\n\n\n\n\n\n"));
Serial.println(F("measuring \n"));
// IMU.setAccelODR(5); //476 Hz
raw_N_Accel(NofSamples,x,y,z);
if (abs(x)>max(abs(y),abs(z)))
{ Serial.println(F("X detected"));
if (sqrt(y*y+z*z)/x<accelCriterion)
{ validMmt= true;
if (x>0) {maxAX=x;
acceMMlOK=acceMMlOK | 0b00000001 ;}
else {minAX=x;
acceMMlOK=acceMMlOK | 0b00001000 ; }
}
}
if (abs(y)>max(abs(x),abs(z)))
{ Serial.println(F("Y detected"));
if (sqrt(x*x+z*z)/y<accelCriterion)
{ validMmt= true;
if (y>0) {maxAY=y;
acceMMlOK=acceMMlOK | 0b00000010 ; }
else {minAY=y;
acceMMlOK=acceMMlOK | 0b00010000 ; }
}
}
if (abs(z)>max(abs(x),abs(y)))
{ Serial.println(F("Z detected"));
if ( sqrt(x*x+y*y)/z<accelCriterion)
{ validMmt= true;
if (z>0) {maxAZ=z;
acceMMlOK=acceMMlOK | 0b00000100 ; }
else {minAZ=z;
acceMMlOK=acceMMlOK | 0b00100000 ; }
}
}
IMU.setAccelOffset((maxAX+minAX)/2,(maxAY+minAY)/2,(maxAZ+minAZ)/2);
IMU.setAccelSlope ((maxAX-minAX)/2,(maxAY-minAY)/2,(maxAZ-minAZ)/2);
if (acceMMlOK==0b00111111) accelOK = true;

if ( !validMmt )
{ Serial.print(F("\n\n\nNot a valid measurement! "));
Serial.println(" x=");Serial.print(x);Serial.print(" y=");Serial.print(y);Serial.println(" z=");Serial.print(z);
}
}

char readChar()
{ char ch;
while (!Serial.available()) ; // wait for character to be entered
ch= toupper(Serial.read());
delay(10);
while (Serial.available()){Serial.read();delay(1);} // empty readbuffer
return ch;
}

void readAnswer(char msg[], uint16_t& param)
{ char ch=0;
byte count=0;
const byte NofChars = 8;
char ans[NofChars];
while (Serial.available()){Serial.read();} //empty read buffer
Serial.print(msg);
Serial.print(param);
Serial.print(F(" Enter new value "));
while (byte(ch)!=10 && byte(ch)!=13 && count<(NofChars-1) )
{ if (Serial.available())
{ ch= Serial.read();
ans[count]=ch;
count++;
}
}
ans[count]=0;
Serial.println(ans);
if (count>1) param= atoi(ans);
while (Serial.available()){Serial.read();}
Serial.println(F("\n\n\n\n\n\n\n"));
}



void raw_N_Accel(uint16_t N, float& averX, float& averY, float& averZ)
{ float x, y, z;
averX=0; averY =0;averZ =0;
for (int i=1;i<=N;i++)
{ while (!IMU.accelAvailable());
IMU.readRawAccel(x, y, z);
averX += x; averY += y; averZ += z;
digitalWrite(LED_BUILTIN, (millis()/125)%2); // blink onboard led every 250ms
if ((i%30)==0)Serial.print('.');
}
averX /= N; averY /= N; averZ /= N;
digitalWrite(LED_BUILTIN,0); // led off
}
Loading