dan
/
EM7180_STM32
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Add files via upload

master
Kris Winer 6 years ago committed by GitHub
parent
7c9cdbbead
commit
4617668123
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 2398 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 593
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32.ino
  2. 173
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/I2Cdev.cpp
  3. 48
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/I2Cdev.h
  4. 175
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/LIS2MDL.cpp
  5. 75
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/LIS2MDL.h
  6. 60
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/LPS22HB.cpp
  7. 73
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/LPS22HB.h
  8. 225
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/LSM6DSM.cpp
  9. 180
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/LSM6DSM.h
  10. 670
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/USFS.cpp
  11. 126
      EM7180_LSM6DSM_LIS2MDL_LPS22HB/USFS.h

593
EM7180_LSM6DSM_LIS2MDL_LPS22HB/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32.ino
Unescape View File

@ -0,0 +1,593 @@
#include "LSM6DSM.h"
#include "LIS2MDL.h"
#include "LPS22HB.h"
#include "USFS.h"
#define I2C_BUS Wire // Define the I2C bus (Wire instance) you wish to use
I2Cdev i2c_0(&I2C_BUS); // Instantiate the I2Cdev object and point to the desired I2C bus
bool SerialDebug = true; // set to true to get Serial output for debugging
bool passThru = false;
#define myLed 5
#define pinGND 12
#define pin3V3 13
void EM7180intHandler();
void myinthandler1();
void myinthandler2();
void myinthandler3();
// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
float pi = 3.141592653589793238462643383279502884f;
float GyroMeasError = pi * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s)
float GyroMeasDrift = pi * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
float beta = sqrtf(3.0f / 4.0f) * GyroMeasError; // compute beta
float zeta = sqrtf(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
uint32_t delt_t = 0; // used to control display output rate
uint32_t sumCount = 0; // used to control display output rate
float pitch, yaw, roll, Yaw, Pitch, Roll;
float a12, a22, a31, a32, a33; // rotation matrix coefficients for Euler angles and gravity components
float A112, A22, A31, A32, A33; // rotation matrix coefficients for Hardware Euler angles and gravity components
float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
uint32_t Now = 0; // used to calculate integration interval
float lin_ax, lin_ay, lin_az; // linear acceleration (acceleration with gravity component subtracted)
float lin_Ax, lin_Ay, lin_Az; // Hardware linear acceleration (acceleration with gravity component subtracted)
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
float Q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // hardware quaternion data register
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
//LSM6DSM definitions
#define LSM6DSM_intPin1 10 // interrupt1 pin definitions, significant motion
#define LSM6DSM_intPin2 9 // interrupt2 pin definitions, data ready
/* Specify sensor parameters (sample rate is twice the bandwidth)
* choices are:
AFS_2G, AFS_4G, AFS_8G, AFS_16G
GFS_245DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
AODR_12_5Hz, AODR_26Hz, AODR_52Hz, AODR_104Hz, AODR_208Hz, AODR_416Hz, AODR_833Hz, AODR_1660Hz, AODR_3330Hz, AODR_6660Hz
GODR_12_5Hz, GODR_26Hz, GODR_52Hz, GODR_104Hz, GODR_208Hz, GODR_416Hz, GODR_833Hz, GODR_1660Hz, GODR_3330Hz, GODR_6660Hz
*/
uint8_t Ascale = AFS_2G, Gscale = GFS_245DPS, AODR = AODR_208Hz, GODR = GODR_416Hz;
float aRes, gRes; // scale resolutions per LSB for the accel and gyro sensor2
float accelBias[3] = {-0.00499, 0.01540, 0.02902}, gyroBias[3] = {-0.50, 0.14, 0.28}; // offset biases for the accel and gyro
int16_t LSM6DSMData[7]; // Stores the 16-bit signed sensor output
float Gtemperature; // Stores the real internal gyro temperature in degrees Celsius
float ax, ay, az, gx, gy, gz; // variables to hold latest accel/gyro data values
bool newLSM6DSMData = false;
bool newLSM6DSMTap = false;
LSM6DSM LSM6DSM(LSM6DSM_intPin1, LSM6DSM_intPin2, &i2c_0); // instantiate LSM6DSM class
//LIS2MDL definitions
#define LIS2MDL_intPin 8 // interrupt for magnetometer data ready
/* Specify sensor parameters (sample rate is twice the bandwidth)
* choices are: MODR_10Hz, MOIDR_20Hz, MODR_50 Hz and MODR_100Hz
*/
uint8_t MODR = MODR_100Hz;
float mRes = 0.0015f; // mag sensitivity
float magBias[3] = {0,0,0}, magScale[3] = {0,0,0}; // Bias corrections for magnetometer
int16_t LIS2MDLData[4]; // Stores the 16-bit signed sensor output
float Mtemperature; // Stores the real internal chip temperature in degrees Celsius
float mx, my, mz; // variables to hold latest mag data values
uint8_t LIS2MDLstatus;
bool newLIS2MDLData = false;
LIS2MDL LIS2MDL(LIS2MDL_intPin, &i2c_0); // instantiate LIS2MDL class
// LPS22H definitions
uint8_t LPS22H_intPin = 5;
/* Specify sensor parameters (sample rate is twice the bandwidth)
Choices are P_1Hz, P_10Hz P_25 Hz, P_50Hz, and P_75Hz
*/
uint8_t PODR = P_25Hz; // set pressure amd temperature output data rate
uint8_t LPS22Hstatus;
float temperature, pressure, altitude;
bool newLPS22HData = false;
LPS22H LPS22H(LPS22H_intPin, &i2c_0);
const uint8_t USFS_intPin = 27;
bool newEM7180Data = false;
int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
int16_t tempCount, rawPressure, rawTemperature; // temperature raw count output
float Temperature, Pressure, Altitude; // temperature in degrees Celsius, pressure in mbar
float Ax, Ay, Az, Gx, Gy, Gz, Mx, My, Mz; // variables to hold latest sensor data values
/* Choose EM7180, LSM6DSM, LIS2MDL sample rates and bandwidths
Choices are:
accBW, gyroBW 0x00 = 250 Hz, 0x01 = 184 Hz, 0x02 = 92 Hz, 0x03 = 41 Hz, 0x04 = 20 Hz, 0x05 = 10 Hz, 0x06 = 5 Hz, 0x07 = no filter (3600 Hz)
QRtDiv 0x00, 0x01, 0x02, etc quat rate = gyroRt/(1 + QRtDiv)
magRt 8 Hz = 0x08 or 100 Hz 0x64
accRt, gyroRt 1000, 500, 250, 200, 125, 100, 50 Hz enter by choosing desired rate
and dividing by 10, so 200 Hz would be 200/10 = 20 = 0x14
sample rate of barometer is baroRt/2 so for 25 Hz enter 50 = 0x32
LSM6DSM accel/gyro rates 0f 833 Hz set Rt variables to 0x53
*/
uint8_t accBW = 0x03, gyroBW = 0x03, QRtDiv = 0x03, magRt = 0x64, accRt = 0x53, gyroRt = 0x53, baroRt = 0x32;
/*
Choose sensor full ranges
Choices are 2, 4, 8, 16 g for accFS, 250, 500, 1000, and 2000 dps for gyro FS and 1000 uT for magFS expressed as HEX values
*/
uint16_t accFS = 0x02, gyroFS = 0x7D0, magFS = 0x3E8;
USFS USFS(USFS_intPin, passThru, &i2c_0);
void setup() {
Serial.begin(115200);
delay(4000);
// Configure led
pinMode(myLed, OUTPUT);
digitalWrite(myLed, HIGH); // start with led on
pinMode(pinGND, OUTPUT);
digitalWrite(pinGND, LOW);
pinMode(pin3V3, OUTPUT);
digitalWrite(pin3V3, HIGH);
pinMode(USFS_intPin, INPUT);
Wire.begin(16, 15, 400000); //(SDA, SCL) (21,22) are default on ESP32, 400 kHz I2C clock
delay(1000);
i2c_0.I2Cscan(); // which I2C device are on the bus?
if(!passThru)
{
// Initialize the USFS
USFS.getChipID(); // check ROM/RAM version of EM7180
USFS.loadfwfromEEPROM(); // load EM7180 firmware from EEPROM
USFS.initEM7180(accBW, gyroBW, accFS, gyroFS, magFS, QRtDiv, magRt, accRt, gyroRt, baroRt); // set MPU and MS5637 sensor parameters
} // end of "if(!passThru)" handling
if(passThru)
{
// Read the LSM6DSM Chip ID register, this is a good test of communication
Serial.println("LSM6DSM accel/gyro...");
byte c = LSM6DSM.getChipID(); // Read CHIP_ID register for LSM6DSM
Serial.print("LSM6DSM "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x6A, HEX);
Serial.println(" ");
delay(1000);
// Read the LIS2MDL Chip ID register, this is a good test of communication
Serial.println("LIS2MDL mag...");
byte d = LIS2MDL.getChipID(); // Read CHIP_ID register for LSM6DSM
Serial.print("LIS2MDL "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x40, HEX);
Serial.println(" ");
delay(1000);
Serial.println("LPS22HB barometer...");
uint8_t e = LPS22H.getChipID();
Serial.print("LPS25H "); Serial.print("I AM "); Serial.print(e, HEX); Serial.print(" I should be "); Serial.println(0xB1, HEX);
delay(1000);
if(c == 0x6A && d == 0x40 && e == 0xB1) // check if all I2C sensors have acknowledged
{
Serial.println("LSM6DSM and LIS2MDL and LPS22HB are online..."); Serial.println(" ");
digitalWrite(myLed, LOW);
LSM6DSM.reset(); // software reset LSM6DSM to default registers
// get sensor resolutions, only need to do this once
aRes = LSM6DSM.getAres(Ascale);
gRes = LSM6DSM.getGres(Gscale);
LSM6DSM.init(Ascale, Gscale, AODR, GODR);
LSM6DSM.selfTest();
LSM6DSM.offsetBias(gyroBias, accelBias);
Serial.println("accel biases (mg)"); Serial.println(1000.0f * accelBias[0]); Serial.println(1000.0f * accelBias[1]); Serial.println(1000.0f * accelBias[2]);
Serial.println("gyro biases (dps)"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);
delay(1000);
LIS2MDL.reset(); // software reset LIS2MDL to default registers
mRes = 0.0015f; // fixed sensitivity and full scale (+/- 49.152 Gauss);
LIS2MDL.init(MODR);
LIS2MDL.selfTest();
LIS2MDL.offsetBias(magBias, magScale);
Serial.println("mag biases (mG)"); Serial.println(1000.0f * magBias[0]); Serial.println(1000.0f * magBias[1]); Serial.println(1000.0f * magBias[2]);
Serial.println("mag scale (mG)"); Serial.println(magScale[0]); Serial.println(magScale[1]); Serial.println(magScale[2]);
delay(2000); // add delay to see results before serial spew of data
LPS22H.Init(PODR); // Initialize LPS22H altimeter
delay(1000);
digitalWrite(myLed, HIGH);
}
else
{
if(c != 0x6A) Serial.println(" LSM6DSM not functioning!");
if(d != 0x40) Serial.println(" LIS2MDL not functioning!");
if(e != 0xB1) Serial.println(" LPS22HB not functioning!");
while(1){};
}
} // end of "if(passThru)" handling
if(!passThru)
{
attachInterrupt(USFS_intPin, EM7180intHandler, RISING); // define interrupt for INT pin output of EM7180
USFS.checkEM7180Status();
}
if(passThru)
{
attachInterrupt(LSM6DSM_intPin2, myinthandler1, RISING); // define interrupt for intPin2 output of LSM6DSM
attachInterrupt(LIS2MDL_intPin , myinthandler2, RISING); // define interrupt for intPin output of LIS2MDL
attachInterrupt(LPS22H_intPin , myinthandler3, RISING); // define interrupt for intPin output of LPS22HB
LIS2MDLstatus = LIS2MDL.status(); // read status register to clear interrupt before main loop
}
digitalWrite(myLed, LOW); // turn led off when successfully through setup
}
/* End of setup */
void loop() {
if(passThru)
{
// If intPin goes high, either all data registers have new data
if(newLSM6DSMData == true) { // On interrupt, read data
newLSM6DSMData = false; // reset newData flag
LSM6DSM.readData(LSM6DSMData); // INT2 cleared on any read
// Now we'll calculate the accleration value into actual g's
ax = (float)LSM6DSMData[4]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
ay = (float)LSM6DSMData[5]*aRes - accelBias[1];
az = (float)LSM6DSMData[6]*aRes - accelBias[2];
// Calculate the gyro value into actual degrees per second
gx = (float)LSM6DSMData[1]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
gy = (float)LSM6DSMData[2]*gRes - gyroBias[1];
gz = (float)LSM6DSMData[3]*gRes - gyroBias[2];
for(uint8_t i = 0; i < 10; i++) { // iterate a fixed number of times per data read cycle
Now = micros();
deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
sum += deltat; // sum for averaging filter update rate
sumCount++;
USFS.MadgwickQuaternionUpdate(-ax, ay, az, gx*pi/180.0f, -gy*pi/180.0f, -gz*pi/180.0f, mx, my, -mz);
}
}
// If intPin goes high, either all data registers have new data
if(newLIS2MDLData == true) { // On interrupt, read data
newLIS2MDLData = false; // reset newData flag
LIS2MDLstatus = LIS2MDL.status();
if(LIS2MDLstatus & 0x08) // if all axes have new data ready
{
LIS2MDL.readData(LIS2MDLData);
// Now we'll calculate the accleration value into actual G's
mx = (float)LIS2MDLData[0]*mRes - magBias[0]; // get actual G value
my = (float)LIS2MDLData[1]*mRes - magBias[1];
mz = (float)LIS2MDLData[2]*mRes - magBias[2];
mx *= magScale[0];
my *= magScale[1];
mz *= magScale[2];
}
}
} // end of "if(passThru)" handling
if(!passThru)
{
/*EM7180*/
// If intpin goes high, all data registers have new data
if (newEM7180Data == true) { // On interrupt, read data
newEM7180Data = false; // reset newData flag
// Check event status register, way to chech data ready by polling rather than interrupt
uint8_t eventStatus = USFS.checkEM7180Status(); // reading clears the register
// Check for errors
if (eventStatus & 0x02) { // error detected, what is it?
uint8_t errorStatus = USFS.checkEM7180Errors();
if (errorStatus != 0x00) { // is there an error?
Serial.print(" EM7180 sensor status = "); Serial.println(errorStatus);
if (errorStatus == 0x11) Serial.print("Magnetometer failure!");
if (errorStatus == 0x12) Serial.print("Accelerometer failure!");
if (errorStatus == 0x14) Serial.print("Gyro failure!");
if (errorStatus == 0x21) Serial.print("Magnetometer initialization failure!");
if (errorStatus == 0x22) Serial.print("Accelerometer initialization failure!");
if (errorStatus == 0x24) Serial.print("Gyro initialization failure!");
if (errorStatus == 0x30) Serial.print("Math error!");
if (errorStatus == 0x80) Serial.print("Invalid sample rate!");
}
// Handle errors ToDo
}
// if no errors, see if new data is ready
if (eventStatus & 0x10) { // new acceleration data available
USFS.readSENtralAccelData(accelCount);
// Now we'll calculate the accleration value into actual g's
Ax = (float)accelCount[0] * 0.000488f; // get actual g value
Ay = (float)accelCount[1] * 0.000488f;
Az = (float)accelCount[2] * 0.000488f;
}
if (eventStatus & 0x20) { // new gyro data available
USFS.readSENtralGyroData(gyroCount);
// Now we'll calculate the gyro value into actual dps's
Gx = (float)gyroCount[0] * 0.153f; // get actual dps value
Gy = (float)gyroCount[1] * 0.153f;
Gz = (float)gyroCount[2] * 0.153f;
}
if (eventStatus & 0x08) { // new mag data available
USFS.readSENtralMagData(magCount);
// Now we'll calculate the mag value into actual G's
Mx = (float)magCount[0] * 0.305176f; // get actual G value
My = (float)magCount[1] * 0.305176f;
Mz = (float)magCount[2] * 0.305176f;
}
if (eventStatus & 0x04) { // new quaternion data available
USFS.readSENtralQuatData(Q);
}
// get MS5637 pressure
if (eventStatus & 0x40) { // new baro data available
rawPressure = USFS.readSENtralBaroData();
Pressure = (float)rawPressure * 0.01f + 1013.25f; // pressure in mBar
// get MS5637 temperature
rawTemperature = USFS.readSENtralTempData();
Temperature = (float) rawTemperature * 0.01f; // temperature in degrees C
}
}
} // end of "if(!passThru)" handling
// end sensor interrupt handling
if(passThru)
{
if(SerialDebug) {
Serial.print("ax = "); Serial.print((int)1000*ax);
Serial.print(" ay = "); Serial.print((int)1000*ay);
Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
Serial.print("gx = "); Serial.print( gx, 2);
Serial.print(" gy = "); Serial.print( gy, 2);
Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
Serial.print("mx = "); Serial.print((int)1000*mx);
Serial.print(" my = "); Serial.print((int)1000*my);
Serial.print(" mz = "); Serial.print((int)1000*mz); Serial.println(" mG");
Serial.print("q0 = "); Serial.print(q[0]);
Serial.print(" qx = "); Serial.print(q[1]);
Serial.print(" qy = "); Serial.print(q[2]);
Serial.print(" qz = "); Serial.println(q[3]);
}
// get pressure and temperature from the LPS22HB
LPS22Hstatus = LPS22H.status();
if(LPS22Hstatus & 0x01) { // if new pressure data available
pressure = (float) LPS22H.readAltimeterPressure()/4096.0f;
temperature = (float) LPS22H.readAltimeterTemperature()/100.0f;
altitude = 145366.45f*(1.0f - pow((pressure/1013.25f), 0.190284f));
if(SerialDebug) {
Serial.print("Altimeter temperature = "); Serial.print( temperature, 2); Serial.println(" C"); // temperature in degrees Celsius
Serial.print("Altimeter temperature = "); Serial.print(9.0f*temperature/5.0f + 32.0f, 2); Serial.println(" F"); // temperature in degrees Fahrenheit
Serial.print("Altimeter pressure = "); Serial.print(pressure, 2); Serial.println(" mbar");// pressure in millibar
Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet");
}
}
Gtemperature = ((float) LSM6DSMData[0]) / 256.0f + 25.0f; // Gyro chip temperature in degrees Centigrade
// Print temperature in degrees Centigrade
if(SerialDebug) {
Serial.print("Gyro temperature is "); Serial.print(Gtemperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C
}
LIS2MDLData[3] = LIS2MDL.readTemperature();
Mtemperature = ((float) LIS2MDLData[3]) / 8.0f + 25.0f; // Mag chip temperature in degrees Centigrade
// Print temperature in degrees Centigrade
if(SerialDebug) {
Serial.print("Mag temperature is "); Serial.print(Mtemperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C
}
a12 = 2.0f * (q[1] * q[2] + q[0] * q[3]);
a22 = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
a31 = 2.0f * (q[0] * q[1] + q[2] * q[3]);
a32 = 2.0f * (q[1] * q[3] - q[0] * q[2]);
a33 = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
pitch = -asinf(a32);
roll = atan2f(a31, a33);
yaw = atan2f(a12, a22);
pitch *= 180.0f / pi;
yaw *= 180.0f / pi;
yaw += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
if(yaw < 0) yaw += 360.0f; // Ensure yaw stays between 0 and 360
roll *= 180.0f / pi;
lin_ax = ax + a31;
lin_ay = ay + a32;
lin_az = az - a33;
if(SerialDebug) {
Serial.print("Yaw, Pitch, Roll: ");
Serial.print(yaw, 2);
Serial.print(", ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);
Serial.print("Grav_x, Grav_y, Grav_z: ");
Serial.print(-a31*1000.0f, 2);
Serial.print(", ");
Serial.print(-a32*1000.0f, 2);
Serial.print(", ");
Serial.print(a33*1000.0f, 2); Serial.println(" mg");
Serial.print("Lin_ax, Lin_ay, Lin_az: ");
Serial.print(lin_ax*1000.0f, 2);
Serial.print(", ");
Serial.print(lin_ay*1000.0f, 2);
Serial.print(", ");
Serial.print(lin_az*1000.0f, 2); Serial.println(" mg");
Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
}
// Serial.print(millis()/1000);Serial.print(",");
// Serial.print(yaw, 2); Serial.print(","); Serial.print(pitch, 2); Serial.print(","); Serial.print(roll, 2); Serial.print(","); Serial.println(Pressure, 2);
sumCount = 0;
sum = 0;
} // end of "if(passThru)" handling
if(!passThru)
{
if (SerialDebug) {
Serial.print("Ax = "); Serial.print((int)1000 * Ax);
Serial.print(" Ay = "); Serial.print((int)1000 * Ay);
Serial.print(" Az = "); Serial.print((int)1000 * Az); Serial.println(" mg");
Serial.print("Gx = "); Serial.print( Gx, 2);
Serial.print(" Gy = "); Serial.print( Gy, 2);
Serial.print(" Gz = "); Serial.print( Gz, 2); Serial.println(" deg/s");
Serial.print("Mx = "); Serial.print( (int)Mx);
Serial.print(" My = "); Serial.print( (int)My);
Serial.print(" Mz = "); Serial.print( (int)Mz); Serial.println(" mG");
Serial.println("Hardware quaternions:");
Serial.print("Q0 = "); Serial.print(Q[0]);
Serial.print(" Qx = "); Serial.print(Q[1]);
Serial.print(" Qy = "); Serial.print(Q[2]);
Serial.print(" Qz = "); Serial.println(Q[3]);
}
//Hardware AHRS:
A112 = 2.0f * (Q[1] * Q[2] + Q[0] * Q[3]);
A22 = Q[0] * Q[0] + Q[1] * Q[1] - Q[2] * Q[2] - Q[3] * Q[3];
A31 = 2.0f * (Q[0] * Q[1] + Q[2] * Q[3]);
A32 = 2.0f * (Q[1] * Q[3] - Q[0] * Q[2]);
A33 = Q[0] * Q[0] - Q[1] * Q[1] - Q[2] * Q[2] + Q[3] * Q[3];
Pitch = -asinf(A32);
Roll = atan2f(A31, A33);
Yaw = atan2f(A112, A22);
Pitch *= 180.0f / pi;
Yaw *= 180.0f / pi;
Yaw += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
if (Yaw < 0) Yaw += 360.0f ; // Ensure yaw stays between 0 and 360
Roll *= 180.0f / pi;
lin_Ax = Ax + A31;
lin_Ay = Ay + A32;
lin_Az = Az - A33;
if (SerialDebug) {
Serial.print("Hardware Yaw, pitch, Roll: ");
Serial.print(Yaw, 2);
Serial.print(", ");
Serial.print(Pitch, 2);
Serial.print(", ");
Serial.println(Roll, 2);
Serial.print("Hardware Grav_x, Grav_y, Grav_z: ");
Serial.print(-A31 * 1000, 2);
Serial.print(", ");
Serial.print(-A32 * 1000, 2);
Serial.print(", ");
Serial.print(A33 * 1000, 2); Serial.println(" mg");
Serial.print("Hardware Lin_ax, Lin_ay, Lin_az: ");
Serial.print(lin_Ax * 1000, 2);
Serial.print(", ");
Serial.print(lin_Ay * 1000, 2);
Serial.print(", ");
Serial.print(lin_Az * 1000, 2); Serial.println(" mg");
Serial.println("MS5637:");
Serial.print("Altimeter temperature = ");
Serial.print(Temperature, 2);
Serial.println(" C"); // temperature in degrees Celsius
Serial.print("Altimeter temperature = ");
Serial.print(9.0f * Temperature / 5.0f + 32.0f, 2);
Serial.println(" F"); // temperature in degrees Fahrenheit
Serial.print("Altimeter pressure = ");
Serial.print(Pressure, 2);
Serial.println(" mbar");// pressure in millibar
Altitude = 145366.45f * (1.0f - powf(((Pressure) / 1013.25f), 0.190284f));
Serial.print("Altitude = ");
Serial.print(Altitude, 2);
Serial.println(" feet");
Serial.println(" ");
}
} // end of "if(!passThru)" handling
digitalWrite(myLed, HIGH); delay(1); digitalWrite(myLed, LOW); // flash led for 10 milliseconds
delay(500);
} //end of loop
/* End of main loop */
void myinthandler1()
{
newLSM6DSMData = true;
}
void myinthandler2()
{
newLIS2MDLData = true;
}
void myinthandler3()
{
newLPS22HData = true;
}
void EM7180intHandler()
{
newEM7180Data = true;
}

173
EM7180_LSM6DSM_LIS2MDL_LPS22HB/I2Cdev.cpp
Unescape View File

@ -0,0 +1,173 @@
/*
* Copyright (c) 2018 Tlera Corp. All rights reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to
* deal with the Software without restriction, including without limitation the
* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
* sell copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimers.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimers in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of Tlera Corp, nor the names of its contributors
* may be used to endorse or promote products derived from this Software
* without specific prior written permission.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
* WITH THE SOFTWARE.
*/
#include "Arduino.h"
#include "I2Cdev.h"
I2Cdev::I2Cdev(TwoWire* i2c_bus) // Class constructor
{
_i2c_bus = i2c_bus;
}
I2Cdev::~I2Cdev() // Class destructor
{
}
/**
* @fn: readByte(uint8_t address, uint8_t subAddress)
*
* @brief: Read one byte from an I2C device
*
* @params: I2C slave device address, Register subAddress
* @returns: unsigned short read
*/
uint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)
{
uint8_t data = 0; // `data` will store the register data
_i2c_bus->beginTransmission(address); // Initialize the Tx buffer
_i2c_bus->write(subAddress); // Put slave register address in Tx buffer
_i2c_bus->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
_i2c_bus->requestFrom(address, 1); // Read one byte from slave register address
data = _i2c_bus->read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}
/**
* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
*
* @brief: Read multiple bytes from an I2C device
*
* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data
* @returns: void
*/
void I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{
_i2c_bus->beginTransmission(address); // Initialize the Tx buffer
_i2c_bus->write(subAddress); // Put slave register address in Tx buffer
_i2c_bus->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
uint8_t i = 0;
_i2c_bus->requestFrom(address, count); // Read bytes from slave register address
while (_i2c_bus->available()) {
dest[i++] = _i2c_bus->read(); } // Put read results in the Rx buffer
}
/**
* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write one byte to an I2C device
*
* @params: I2C slave device address, Register subAddress, data to be written
* @returns: void
*/
void I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
{
_i2c_bus->beginTransmission(devAddr); // Initialize the Tx buffer
_i2c_bus->write(regAddr); // Put slave register address in Tx buffer
_i2c_bus->write(data); // Put data in Tx buffer
_i2c_bus->endTransmission(); // Send the Tx buffer
}
/**
* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write multiple bytes to an I2C device
*
* @params: I2C slave device address, Register subAddress, byte count, data array to be written
* @returns: void
*/
void I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)
{
uint8_t temp[1 + count];
temp[0] = regAddr;
for (uint8_t ii = 0; ii < count; ii++)
{
temp[ii + 1] = dest[ii];
}
_i2c_bus->beginTransmission(devAddr); // Initialize the Tx buffer
for (uint8_t jj = 0; jj < count + 1; jj++)
{
_i2c_bus->write(temp[jj]); // Put data in Tx buffer
}
_i2c_bus->endTransmission(); // Send the Tx buffer
}
/**
* @fn:I2Cscan()
* @brief: Scan the I2C bus for active I2C slave devices
*
* @params: void
* @returns: void
*/
void I2Cdev::I2Cscan()
{
// Scan for i2c devices
byte error, address;
int nDevices;
Serial.println("Scanning...");
nDevices = 0;
for(address = 1; address < 127; address++ )
{
// The i2c_scanner uses the return value of the Wire.endTransmisstion to see if a device did acknowledge to the address.
_i2c_bus->beginTransmission(address);
error = _i2c_bus->endTransmission();
if (error == 0)
{
Serial.print("I2C device found at address 0x");
if (address<16)
Serial.print("0");
Serial.print(address,HEX);
Serial.println(" !");
nDevices++;
}
else if (error==4)
{
Serial.print("Unknown error at address 0x");
if (address<16)
Serial.print("0");
Serial.println(address,HEX);
}
}
if (nDevices == 0)
Serial.println("No I2C devices found\n");
else
Serial.println("I2C scan complete\n");
}

48
EM7180_LSM6DSM_LIS2MDL_LPS22HB/I2Cdev.h
Unescape View File

@ -0,0 +1,48 @@
/*
* Copyright (c) 2018 Tlera Corp. All rights reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to
* deal with the Software without restriction, including without limitation the
* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
* sell copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimers.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimers in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of Tlera Corp, nor the names of its contributors
* may be used to endorse or promote products derived from this Software
* without specific prior written permission.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
* WITH THE SOFTWARE.
*/
#ifndef _I2CDEV_H_
#define _I2CDEV_H_
#include <Wire.h>
class I2Cdev {
public:
I2Cdev(TwoWire*);
~I2Cdev(); // Class destructor for durable instances
uint8_t readByte(uint8_t address, uint8_t subAddress);
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
void writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
void writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);
void I2Cscan();
private:
TwoWire* _i2c_bus; // Class constructor argument
};
#endif //_I2CDEV_H_

175
EM7180_LSM6DSM_LIS2MDL_LPS22HB/LIS2MDL.cpp
Unescape View File

@ -0,0 +1,175 @@
/* 09/23/2017 Copyright Tlera Corporation
Created by Kris Winer
This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution.
Library may be used freely and without limit with attribution.
*/
#include "LIS2MDL.h"
LIS2MDL::LIS2MDL(uint8_t intPin, I2Cdev* i2c_bus)
{
_intPin = intPin;
_i2c_bus = i2c_bus;
}
uint8_t LIS2MDL::getChipID()
{
uint8_t c = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
return c;
}
void LIS2MDL::reset()
{
// reset device
uint8_t temp = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
_i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
delay(1);
_i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
delay(100); // Wait for all registers to reset
}
void LIS2MDL::init(uint8_t MODR)
{
// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
_i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR<<2);
// enable low pass filter (bit 0 == 1), set to ODR/4
_i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)
_i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
}
uint8_t LIS2MDL::status()
{
// Read the status register of the altimeter
uint8_t temp = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG);
return temp;
}
void LIS2MDL::readData(int16_t * destination)
{
uint8_t rawData[6]; // x/y/z mag register data stored here
_i2c_bus->readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8, &rawData[0]); // Read the 6 raw data registers into data array
destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
}
int16_t LIS2MDL::readTemperature()
{
uint8_t rawData[2]; // x/y/z mag register data stored here
_i2c_bus->readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2, &rawData[0]); // Read the 8 raw data registers into data array
int16_t temp = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
return temp;
}
void LIS2MDL::offsetBias(float * dest1, float * dest2)
{
int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
float _mRes = 0.0015f;
Serial.println("Calculate mag offset bias: move all around to sample the complete response surface!");
delay(4000);
for (int ii = 0; ii < 4000; ii++)
{
readData(mag_temp);
for (int jj = 0; jj < 3; jj++) {
if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
}
delay(12);
}
_mRes = 0.0015f; // fixed sensitivity
// Get hard iron correction
mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
dest1[0] = (float) mag_bias[0] * _mRes; // save mag biases in G for main program
dest1[1] = (float) mag_bias[1] * _mRes;
dest1[2] = (float) mag_bias[2] * _mRes;
// Get soft iron correction estimate
mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts
mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts
mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts
float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
avg_rad /= 3.0f;
dest2[0] = avg_rad/((float)mag_scale[0]);
dest2[1] = avg_rad/((float)mag_scale[1]);
dest2[2] = avg_rad/((float)mag_scale[2]);
Serial.println("Mag Calibration done!");
}
void LIS2MDL::selfTest()
{
int16_t temp[3] = {0, 0, 0};
float magTest[3] = {0., 0., 0.};
float magNom[3] = {0., 0., 0.};
int32_t sum[3] = {0, 0, 0};
float _mRes = 0.0015f;
// first, get average response with self test disabled
for (int ii = 0; ii < 50; ii++)
{
readData(temp);
sum[0] += temp[0];
sum[1] += temp[1];
sum[2] += temp[2];
delay(50);
}
magNom[0] = (float) sum[0] / 50.0f;
magNom[1] = (float) sum[1] / 50.0f;
magNom[2] = (float) sum[2] / 50.0f;
uint8_t c = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
_i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
delay(100); // let mag respond
sum[0] = 0;
sum[1] = 0;
sum[2] = 0;
for (int ii = 0; ii < 50; ii++)
{
readData(temp);
sum[0] += temp[0];
sum[1] += temp[1];
sum[2] += temp[2];
delay(50);
}
magTest[0] = (float) sum[0] / 50.0f;
magTest[1] = (float) sum[1] / 50.0f;
magTest[2] = (float) sum[2] / 50.0f;
_i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
delay(100); // let mag respond
Serial.println("Mag Self Test:");
Serial.print("Mx results:"); Serial.print( (magTest[0] - magNom[0]) * _mRes * 1000.0); Serial.println(" mG");
Serial.print("My results:"); Serial.println((magTest[0] - magNom[0]) * _mRes * 1000.0);
Serial.print("Mz results:"); Serial.println((magTest[1] - magNom[1]) * _mRes * 1000.0);
Serial.println("Should be between 15 and 500 mG");
delay(2000); // give some time to read the screen
}

75
EM7180_LSM6DSM_LIS2MDL_LPS22HB/LIS2MDL.h
Unescape View File

@ -0,0 +1,75 @@
/* 09/23/2017 Copyright Tlera Corporation
Created by Kris Winer
This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution.
Library may be used freely and without limit with attribution.
*/
#ifndef LIS2MDL_h
#define LIS2MDL_h
#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"
//Register map for LIS2MDL'
// http://www.st.com/content/ccc/resource/technical/document/datasheet/group3/29/13/d1/e0/9a/4d/4f/30/DM00395193/files/DM00395193.pdf/jcr:content/translations/en.DM00395193.pdf
#define LIS2MDL_OFFSET_X_REG_L 0x45
#define LIS2MDL_OFFSET_X_REG_L 0x46
#define LIS2MDL_OFFSET_X_REG_L 0x47
#define LIS2MDL_OFFSET_X_REG_L 0x48
#define LIS2MDL_OFFSET_X_REG_L 0x49
#define LIS2MDL_OFFSET_X_REG_L 0x4A
#define LIS2MDL_WHO_AM_I 0x4F
#define LIS2MDL_CFG_REG_A 0x60
#define LIS2MDL_CFG_REG_B 0x61
#define LIS2MDL_CFG_REG_C 0x62
#define LIS2MDL_INT_CTRL_REG 0x63
#define LIS2MDL_INT_SOURCE_REG 0x64
#define LIS2MDL_INT_THS_L_REG 0x65
#define LIS2MDL_INT_THS_H_REG 0x66
#define LIS2MDL_STATUS_REG 0x67
#define LIS2MDL_OUTX_L_REG 0x68
#define LIS2MDL_OUTX_H_REG 0x69
#define LIS2MDL_OUTY_L_REG 0x6A
#define LIS2MDL_OUTY_H_REG 0x6B
#define LIS2MDL_OUTZ_L_REG 0x6C
#define LIS2MDL_OUTZ_H_REG 0x6D
#define LIS2MDL_TEMP_OUT_L_REG 0x6E
#define LIS2MDL_TEMP_OUT_H_REG 0x6F
#define LIS2MDL_ADDRESS 0x1E
#define MODR_10Hz 0x00
#define MODR_20Hz 0x01
#define MODR_50Hz 0x02
#define MODR_100Hz 0x03
class LIS2MDL
{
public:
LIS2MDL(uint8_t intPin, I2Cdev* i2c_bus);
uint8_t getChipID();
void init(uint8_t MODR);
void offsetBias(float * dest1, float * dest2);
void reset();
void selfTest();
uint8_t status();
void readData(int16_t * destination);
int16_t readTemperature();
void I2Cscan();
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
uint8_t readByte(uint8_t address, uint8_t subAddress);
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
private:
uint8_t _intPin;
float _mRes;
I2Cdev* _i2c_bus;
};
#endif

60
EM7180_LSM6DSM_LIS2MDL_LPS22HB/LPS22HB.cpp
Unescape View File

@ -0,0 +1,60 @@
/* 09/23/2017 Copyright Tlera Corporation
Created by Kris Winer
This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
The LPS22HB is a low power barometerr.
Library may be used freely and without limit with attribution.
*/
#include "LPS22HB.h"
#include "Wire.h"
LPS22H::LPS22H(uint8_t intPin, I2Cdev* i2c_bus)
{
_intPin = intPin;
_i2c_bus = i2c_bus;
}
uint8_t LPS22H::getChipID()
{
// Read the WHO_AM_I register of the altimeter this is a good test of communication
uint8_t temp = _i2c_bus->readByte(LPS22H_ADDRESS, LPS22H_WHOAMI); // Read WHO_AM_I register for LPS22H
return temp;
}
uint8_t LPS22H::status()
{
// Read the status register of the altimeter
uint8_t temp = _i2c_bus->readByte(LPS22H_ADDRESS, LPS22H_STATUS);
return temp;
}
int32_t LPS22H::readAltimeterPressure()
{
uint8_t rawData[3]; // 24-bit pressure register data stored here
_i2c_bus->readBytes(LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), 3, &rawData[0]); // bit 7 must be one to read multiple bytes
return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8 | rawData[0]);
}
int16_t LPS22H::readAltimeterTemperature()
{
uint8_t rawData[2]; // 16-bit pressure register data stored here
_i2c_bus->readBytes(LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), 2, &rawData[0]); // bit 7 must be one to read multiple bytes
return (int16_t)((int16_t) rawData[1] << 8 | rawData[0]);
}
void LPS22H::Init(uint8_t PODR)
{
// set sample rate by setting bits 6:4
// enable low-pass filter by setting bit 3 to one
// bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20
// make sure data not updated during read by setting block data udate (bit 1) to 1
_i2c_bus->writeByte(LPS22H_ADDRESS, LPS22H_CTRL_REG1, PODR << 4 | 0x08 | 0x02);
_i2c_bus->writeByte(LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
}

73
EM7180_LSM6DSM_LIS2MDL_LPS22HB/LPS22HB.h
Unescape View File

@ -0,0 +1,73 @@
/* 09/23/2017 Copyright Tlera Corporation
Created by Kris Winer
This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
The LPS22HB is a low power barometerr.
Library may be used freely and without limit with attribution.
*/
#ifndef LPS22HB_h
#define LPS22HB_h
#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"
// See LPS22H "MEMS pressure sensor: 260-1260 hPa absolute digital output barometer" Data Sheet
// http://www.st.com/content/ccc/resource/technical/document/datasheet/bf/c1/4f/23/61/17/44/8a/DM00140895.pdf/files/DM00140895.pdf/jcr:content/translations/en.DM00140895.pdf
#define LPS22H_INTERRUPT_CFG 0x0B
#define LPS22H_THS_P_L 0x0C
#define LPS22H_THS_P_H 0x0D
#define LPS22H_WHOAMI 0x0F // should return 0xB1
#define LPS22H_CTRL_REG1 0x10
#define LPS22H_CTRL_REG2 0x11
#define LPS22H_CTRL_REG3 0x12
#define LPS22H_FIFO_CTRL 0x14
#define LPS22H_REF_P_XL 0x15
#define LPS22H_REF_P_L 0x16
#define LPS22H_REF_P_H 0x17
#define LPS22H_RPDS_L 0x18
#define LPS22H_RPDS_H 0x19
#define LPS22H_RES_CONF 0x1A
#define LPS22H_INT_SOURCE 0x25
#define LPS22H_FIFO_STATUS 0x26
#define LPS22H_STATUS 0x27
#define LPS22H_PRESS_OUT_XL 0x28
#define LPS22H_PRESS_OUT_L 0x29
#define LPS22H_PRESS_OUT_H 0x2A
#define LPS22H_TEMP_OUT_L 0x2B
#define LPS22H_TEMP_OUT_H 0x2C
#define LPS22H_LPFP_RES 0x33
#define LPS22H_ADDRESS 0x5C // Address of altimeter
// Altimeter output data rate
#define P_1shot 0x00;
#define P_1Hz 0x01;
#define P_10Hz 0x02;
#define P_25Hz 0x03; // 25 Hz output data rate
#define P_50Hz 0x04;
#define P_75Hz 0x05;
class LPS22H
{
public:
LPS22H(uint8_t intPin, I2Cdev* i2c_bus);
void Init(uint8_t PODR);
uint8_t getChipID();
uint8_t status();
int32_t readAltimeterPressure();
int16_t readAltimeterTemperature();
void I2Cscan();
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
uint8_t readByte(uint8_t address, uint8_t subAddress);
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
private:
uint8_t _intPin;
I2Cdev* _i2c_bus;
};
#endif

225
EM7180_LSM6DSM_LIS2MDL_LPS22HB/LSM6DSM.cpp
Unescape View File

@ -0,0 +1,225 @@
/* 09/23/2017 Copyright Tlera Corporation
Created by Kris Winer
This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
The LSM6DSM is a sensor hub with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution.
Library may be used freely and without limit with attribution.
*/
#include "LSM6DSM.h"
LSM6DSM::LSM6DSM(uint8_t intPin1, uint8_t intPin2, I2Cdev* i2c_bus)
{
_intPin1 = intPin1;
_intPin2 = intPin2;
_i2c_bus = i2c_bus;
}
uint8_t LSM6DSM::getChipID()
{
uint8_t c = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);
return c;
}
float LSM6DSM::getAres(uint8_t Ascale) {
switch (Ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G:
_aRes = 2.0f/32768.0f;
return _aRes;
break;
case AFS_4G:
_aRes = 4.0f/32768.0f;
return _aRes;
break;
case AFS_8G:
_aRes = 8.0f/32768.0f;
return _aRes;
break;
case AFS_16G:
_aRes = 16.0f/32768.0f;
return _aRes;
break;
}
}
float LSM6DSM::getGres(uint8_t Gscale) {
switch (Gscale)
{
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
case GFS_245DPS:
_gRes = 245.0f/32768.0f;
return _gRes;
break;
case GFS_500DPS:
_gRes = 500.0f/32768.0f;
return _gRes;
break;
case GFS_1000DPS:
_gRes = 1000.0f/32768.0f;
return _gRes;
break;
case GFS_2000DPS:
_gRes = 2000.0f/32768.0f;
return _gRes;
break;
}
}
void LSM6DSM::reset()
{
// reset device
uint8_t temp = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
delay(100); // Wait for all registers to reset
}
void LSM6DSM::init(uint8_t Ascale, uint8_t Gscale, uint8_t AODR, uint8_t GODR)
{
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL, AODR << 4 | Ascale << 2);
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G, GODR << 4 | Gscale << 2);
uint8_t temp = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04);
// by default, interrupts active HIGH, push pull, little endian data
// (can be changed by writing to bits 5, 4, and 1, resp to above register)
// enable accel LP2 (bit 7 = 1), set LP2 tp ODR/9 (bit 6 = 1), enable input_composite (bit 3) for low noise
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08 );
// interrupt handling
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03); // enable accel/gyro data ready interrupts on INT2
}
void LSM6DSM::selfTest()
{
int16_t temp[7] = {0, 0, 0, 0, 0, 0, 0};
int16_t accelPTest[3] = {0, 0, 0}, accelNTest[3] = {0, 0, 0}, gyroPTest[3] = {0, 0, 0}, gyroNTest[3] = {0, 0, 0};
int16_t accelNom[3] = {0, 0, 0}, gyroNom[3] = {0, 0, 0};
readData(temp);
accelNom[0] = temp[4];
accelNom[1] = temp[5];
accelNom[2] = temp[6];
gyroNom[0] = temp[1];
gyroNom[1] = temp[2];
gyroNom[2] = temp[3];
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
delay(100); // let accel respond
readData(temp);
accelPTest[0] = temp[4];
accelPTest[1] = temp[5];
accelPTest[2] = temp[6];
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
delay(100); // let accel respond
readData(temp);
accelNTest[0] = temp[4];
accelNTest[1] = temp[5];
accelNTest[2] = temp[6];
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
delay(100); // let gyro respond
readData(temp);
gyroPTest[0] = temp[1];
gyroPTest[1] = temp[2];
gyroPTest[2] = temp[3];
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
delay(100); // let gyro respond
readData(temp);
gyroNTest[0] = temp[1];
gyroNTest[1] = temp[2];
gyroNTest[2] = temp[3];
_i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
delay(100); // let accel and gyro respond
Serial.println("Accel Self Test:");
Serial.print("+Ax results:"); Serial.print( (accelPTest[0] - accelNom[0]) * _aRes * 1000.0); Serial.println(" mg");
Serial.print("-Ax results:"); Serial.println((accelNTest[0] - accelNom[0]) * _aRes * 1000.0);
Serial.print("+Ay results:"); Serial.println((accelPTest[1] - accelNom[1]) * _aRes * 1000.0);
Serial.print("-Ay results:"); Serial.println((accelNTest[1] - accelNom[1]) * _aRes * 1000.0);
Serial.print("+Az results:"); Serial.println((accelPTest[2] - accelNom[2]) * _aRes * 1000.0);
Serial.print("-Az results:"); Serial.println((accelNTest[2] - accelNom[2]) * _aRes * 1000.0);
Serial.println("Should be between 90 and 1700 mg");
Serial.println("Gyro Self Test:");
Serial.print("+Gx results:"); Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); Serial.println(" dps");
Serial.print("-Gx results:"); Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes);
Serial.print("+Gy results:"); Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes);
Serial.print("-Gy results:"); Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes);
Serial.print("+Gz results:"); Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes);
Serial.print("-Gz results:"); Serial.println((gyroNTest[2] - gyroNom[2]) * _gRes);
Serial.println("Should be between 20 and 80 dps");
delay(2000);
}
void LSM6DSM::offsetBias(float * dest1, float * dest2)
{
int16_t temp[7] = {0, 0, 0, 0, 0, 0, 0};
int32_t sum[7] = {0, 0, 0, 0, 0, 0, 0};
Serial.println("Calculate accel and gyro offset biases: keep sensor flat and motionless!");
delay(4000);
for (int ii = 0; ii < 128; ii++)
{
readData(temp);
sum[1] += temp[1];
sum[2] += temp[2];
sum[3] += temp[3];
sum[4] += temp[4];
sum[5] += temp[5];
sum[6] += temp[6];
delay(50);
}
dest1[0] = sum[1]*_gRes/128.0f;
dest1[1] = sum[2]*_gRes/128.0f;
dest1[2] = sum[3]*_gRes/128.0f;
dest2[0] = sum[4]*_aRes/128.0f;
dest2[1] = sum[5]*_aRes/128.0f;
dest2[2] = sum[6]*_aRes/128.0f;
if(dest2[0] > 0.8f) {dest2[0] -= 1.0f;} // Remove gravity from the x-axis accelerometer bias calculation
if(dest2[0] < -0.8f) {dest2[0] += 1.0f;} // Remove gravity from the x-axis accelerometer bias calculation
if(dest2[1] > 0.8f) {dest2[1] -= 1.0f;} // Remove gravity from the y-axis accelerometer bias calculation
if(dest2[1] < -0.8f) {dest2[1] += 1.0f;} // Remove gravity from the y-axis accelerometer bias calculation
if(dest2[2] > 0.8f) {dest2[2] -= 1.0f;} // Remove gravity from the z-axis accelerometer bias calculation
if(dest2[2] < -0.8f) {dest2[2] += 1.0f;} // Remove gravity from the z-axis accelerometer bias calculation
}
void LSM6DSM::readData(int16_t * destination)
{
uint8_t rawData[14]; // x/y/z accel register data stored here
_i2c_bus->readBytes(LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, 14, &rawData[0]); // Read the 14 raw data registers into data array
destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
destination[3] = ((int16_t)rawData[7] << 8) | rawData[6] ;
destination[4] = ((int16_t)rawData[9] << 8) | rawData[8] ;
destination[5] = ((int16_t)rawData[11] << 8) | rawData[10] ;
destination[6] = ((int16_t)rawData[13] << 8) | rawData[12] ;
}

180
EM7180_LSM6DSM_LIS2MDL_LPS22HB/LSM6DSM.h
Unescape View File

@ -0,0 +1,180 @@
/* 09/23/2017 Copyright Tlera Corporation
Created by Kris Winer
This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
The LSM6DSM is a sensor hub with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution.
Library may be used freely and without limit with attribution.
*/
#ifndef LSM6DSM_h
#define LSM6DSM_h
#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"
/* LSM6DSM registers
http://www.st.com/content/ccc/resource/technical/document/datasheet/76/27/cf/88/c5/03/42/6b/DM00218116.pdf/files/DM00218116.pdf/jcr:content/translations/en.DM00218116.pdf
*/
#define LSM6DSM_FUNC_CFG_ACCESS 0x01
#define LSM6DSM_SENSOR_SYNC_TIME_FRAME 0x04
#define LSM6DSM_SENSOR_SYNC_RES_RATIO 0x05
#define LSM6DSM_FIFO_CTRL1 0x06
#define LSM6DSM_FIFO_CTRL2 0x07
#define LSM6DSM_FIFO_CTRL3 0x08
#define LSM6DSM_FIFO_CTRL4 0x09
#define LSM6DSM_FIFO_CTRL5 0x0A
#define LSM6DSM_DRDY_PULSE_CFG 0x0B
#define LSM6DSM_INT1_CTRL 0x0D
#define LSM6DSM_INT2_CTRL 0x0E
#define LSM6DSM_WHO_AM_I 0x0F // should be 0x6A
#define LSM6DSM_CTRL1_XL 0x10
#define LSM6DSM_CTRL2_G 0x11
#define LSM6DSM_CTRL3_C 0x12
#define LSM6DSM_CTRL4_C 0x13
#define LSM6DSM_CTRL5_C 0x14
#define LSM6DSM_CTRL6_C 0x15
#define LSM6DSM_CTRL7_G 0x16
#define LSM6DSM_CTRL8_XL 0x17
#define LSM6DSM_CTRL9_XL 0x18
#define LSM6DSM_CTRL10_C 0x19
#define LSM6DSM_MASTER_CONFIG 0x1A
#define LSM6DSM_WAKE_UP_SRC 0x1B
#define LSM6DSM_TAP_SRC 0x1C
#define LSM6DSM_D6D_SRC 0x1D
#define LSM6DSM_STATUS_REG 0x1E
#define LSM6DSM_OUT_TEMP_L 0x20
#define LSM6DSM_OUT_TEMP_H 0x21
#define LSM6DSM_OUTX_L_G 0x22
#define LSM6DSM_OUTX_H_G 0x23
#define LSM6DSM_OUTY_L_G 0x24
#define LSM6DSM_OUTY_H_G 0x25
#define LSM6DSM_OUTZ_L_G 0x26
#define LSM6DSM_OUTZ_H_G 0x27
#define LSM6DSM_OUTX_L_XL 0x28
#define LSM6DSM_OUTX_H_XL 0x29
#define LSM6DSM_OUTY_L_XL 0x2A
#define LSM6DSM_OUTY_H_XL 0x2B
#define LSM6DSM_OUTZ_L_XL 0x2C
#define LSM6DSM_OUTZ_H_XL 0x2D
#define LSM6DSM_SENSORHUB1_REG 0x2E
#define LSM6DSM_SENSORHUB2_REG 0x2F
#define LSM6DSM_SENSORHUB3_REG 0x30
#define LSM6DSM_SENSORHUB4_REG 0x31
#define LSM6DSM_SENSORHUB5_REG 0x32
#define LSM6DSM_SENSORHUB6_REG 0x33
#define LSM6DSM_SENSORHUB7_REG 0x34
#define LSM6DSM_SENSORHUB8_REG 0x35
#define LSM6DSM_SENSORHUB9_REG 0x36
#define LSM6DSM_SENSORHUB10_REG 0x37
#define LSM6DSM_SENSORHUB11_REG 0x38
#define LSM6DSM_SENSORHUB12_REG 0x39
#define LSM6DSM_FIFO_STATUS1 0x3A
#define LSM6DSM_FIFO_STATUS2 0x3B
#define LSM6DSM_FIFO_STATUS3 0x3C
#define LSM6DSM_FIFO_STATUS4 0x3D
#define LSM6DSM_FIFO_DATA_OUT_L 0x3E
#define LSM6DSM_FIFO_DATA_OUT_H 0x3F
#define LSM6DSM_TIMESTAMP0_REG 0x40
#define LSM6DSM_TIMESTAMP1_REG 0x41
#define LSM6DSM_TIMESTAMP2_REG 0x42
#define LSM6DSM_STEP_TIMESTAMP_L 0x49
#define LSM6DSM_STEP_TIMESTAMP_H 0x4A
#define LSM6DSM_STEP_COUNTER_L 0x4B
#define LSM6DSM_STEP_COUNTER_H 0x4C
#define LSM6DSM_SENSORHUB13_REG 0x4D
#define LSM6DSM_SENSORHUB14_REG 0x4E
#define LSM6DSM_SENSORHUB15_REG 0x4F
#define LSM6DSM_SENSORHUB16_REG 0x50
#define LSM6DSM_SENSORHUB17_REG 0x51
#define LSM6DSM_SENSORHUB18_REG 0x52
#define LSM6DSM_FUNC_SRC1 0x53
#define LSM6DSM_FUNC_SRC2 0x54
#define LSM6DSM_WRIST_TILT_IA 0x55
#define LSM6DSM_TAP_CFG 0x58
#define LSM6DSM_TAP_THS_6D 0x59
#define LSM6DSM_INT_DUR2 0x5A
#define LSM6DSM_WAKE_UP_THS 0x5B
#define LSM6DSM_WAKE_UP_DUR 0x5C
#define LSM6DSM_FREE_FALL 0x5D
#define LSM6DSM_MD1_CFG 0x5E
#define LSM6DSM_MD2_CFG 0x5F
#define LSM6DSM_MASTER_MODE_CODE 0x60
#define LSM6DSM_SENS_SYNC_SPI_ERROR_CODE 0x61
#define LSM6DSM_OUT_MAG_RAW_X_L 0x66
#define LSM6DSM_OUT_MAG_RAW_X_H 0x67
#define LSM6DSM_OUT_MAG_RAW_Y_L 0x68
#define LSM6DSM_OUT_MAG_RAW_Y_H 0x69
#define LSM6DSM_OUT_MAG_RAW_Z_L 0x6A
#define LSM6DSM_OUT_MAG_RAW_Z_H 0x6B
#define LSM6DSM_INT_OIS 0x6F
#define LSM6DSM_CTRL1_OIS 0x70
#define LSM6DSM_CTRL2_OIS 0x71
#define LSM6DSM_CTRL3_OIS 0x72
#define LSM6DSM_X_OFS_USR 0x73
#define LSM6DSM_Y_OFS_USR 0x74
#define LSM6DSM_Z_OFS_USR 0x75
#define LSM6DSM_ADDRESS 0x6A // Address of LSM6DSM accel/gyro when ADO = 0
#define AFS_2G 0x00
#define AFS_4G 0x02
#define AFS_8G 0x03
#define AFS_16G 0x01
#define GFS_245DPS 0x00
#define GFS_500DPS 0x01
#define GFS_1000DPS 0x02
#define GFS_2000DPS 0x03
#define AODR_12_5Hz 0x01 // same for accel and gyro in normal mode
#define AODR_26Hz 0x02
#define AODR_52Hz 0x03
#define AODR_104Hz 0x04
#define AODR_208Hz 0x05
#define AODR_416Hz 0x06
#define AODR_833Hz 0x07
#define AODR_1660Hz 0x08
#define AODR_3330Hz 0x09
#define AODR_6660Hz 0x0A
#define GODR_12_5Hz 0x01
#define GODR_26Hz 0x02
#define GODR_52Hz 0x03
#define GODR_104Hz 0x04
#define GODR_208Hz 0x05
#define GODR_416Hz 0x06
#define GODR_833Hz 0x07
#define GODR_1660Hz 0x08
#define GODR_3330Hz 0x09
#define GODR_6660Hz 0x0A
class LSM6DSM
{
public:
LSM6DSM(uint8_t intPin1, uint8_t intPin2, I2Cdev* i2c_bus);
float getAres(uint8_t Ascale);
float getGres(uint8_t Gscale);
uint8_t getChipID();
void init(uint8_t Ascale, uint8_t Gscale, uint8_t AODR, uint8_t GODR);
void offsetBias(float * dest1, float * dest2);
void reset();
void selfTest();
void readData(int16_t * destination);
void I2Cscan();
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
uint8_t readByte(uint8_t address, uint8_t subAddress);
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
private:
uint8_t _intPin1;
uint8_t _intPin2;
float _aRes, _gRes;
I2Cdev* _i2c_bus;
};
#endif

670
EM7180_LSM6DSM_LIS2MDL_LPS22HB/USFS.cpp
Unescape View File

@ -0,0 +1,670 @@
/* 06/29/2017 Copyright Tlera Corporation
*
* Created by Kris Winer
*
*
* Library may be used freely and without limit with attribution.
*
*/
#include "USFS.h"
USFS::USFS(uint8_t intPin, bool passThru, I2Cdev* i2c_bus)
{
_intPin = intPin;
_passThru = passThru;
_i2c_bus = i2c_bus;
}
void USFS::getChipID()
{
// Read SENtral device information
uint16_t ROM1 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ROMVersion1);
uint16_t ROM2 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ROMVersion2);
Serial.print("EM7180 ROM Version: 0x"); Serial.print(ROM1, HEX); Serial.println(ROM2, HEX); Serial.println("Should be: 0xE609");
uint16_t RAM1 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RAMVersion1);
uint16_t RAM2 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RAMVersion2);
Serial.print("EM7180 RAM Version: 0x"); Serial.print(RAM1); Serial.println(RAM2);
uint8_t PID = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ProductID);
Serial.print("EM7180 ProductID: 0x"); Serial.print(PID, HEX); Serial.println(" Should be: 0x80");
uint8_t RID = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RevisionID);
Serial.print("EM7180 RevisionID: 0x"); Serial.print(RID, HEX); Serial.println(" Should be: 0x02");
}
void USFS::loadfwfromEEPROM()
{
// Check which sensors can be detected by the EM7180
uint8_t featureflag = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_FeatureFlags);
if(featureflag & 0x01) Serial.println("A barometer is installed");
if(featureflag & 0x02) Serial.println("A humidity sensor is installed");
if(featureflag & 0x04) Serial.println("A temperature sensor is installed");
if(featureflag & 0x08) Serial.println("A custom sensor is installed");
if(featureflag & 0x10) Serial.println("A second custom sensor is installed");
if(featureflag & 0x20) Serial.println("A third custom sensor is installed");
delay(1000); // give some time to read the screen
// Check SENtral status, make sure EEPROM upload of firmware was accomplished
byte STAT = (_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01) Serial.println("EEPROM detected on the sensor bus!");
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02) Serial.println("EEPROM uploaded config file!");
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04) Serial.println("EEPROM CRC incorrect!");
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08) Serial.println("EM7180 in initialized state!");
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10) Serial.println("No EEPROM detected!");
int count = 0;
while(!STAT) {
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
delay(500);
count++;
STAT = (_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01) Serial.println("EEPROM detected on the sensor bus!");
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02) Serial.println("EEPROM uploaded config file!");
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04) Serial.println("EEPROM CRC incorrect!");
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08) Serial.println("EM7180 in initialized state!");
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10) Serial.println("No EEPROM detected!");
if(count > 10) break;
}
if(!(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)) Serial.println("EEPROM upload successful!");
}
uint8_t USFS::checkEM7180Status(){
// Check event status register, way to check data ready by polling rather than interrupt
uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
return c;
}
uint8_t USFS::checkEM7180Errors(){
uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ErrorRegister); // check error register
return c;
}
void USFS::initEM7180(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, uint16_t magFS, uint8_t QRtDiv, uint8_t magRt, uint8_t accRt, uint8_t gyroRt, uint8_t baroRt)
{
uint16_t EM7180_mag_fs, EM7180_acc_fs, EM7180_gyro_fs; // EM7180 sensor full scale ranges
uint8_t param[4];
// Enter EM7180 initialized state
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
//Setup LPF bandwidth (BEFORE setting ODR's)
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz
// Set accel/gyro/mag desired ODR rates
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_QRateDivisor, QRtDiv); // quat rate = gyroRt/(1 QRTDiv)
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_MagRate, magRt); // 0x64 = 100 Hz
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AccelRate, accRt); // 200/10 Hz, 0x14 = 200 Hz
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_GyroRate, gyroRt); // 200/10 Hz, 0x14 = 200 Hz
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_BaroRate, 0x80 | baroRt); // set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz
// Configure operating mode
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data
// Enable interrupt to host upon certain events
// choose host interrupts when any sensor updated (0x40), new gyro data (0x20), new accel data (0x10),
// new mag data (0x08), quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01)
_i2c_bus-> writeByte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
// Enable EM7180 run mode
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
delay(100);
// EM7180 parameter adjustments
Serial.println("Beginning Parameter Adjustments");
// Read sensor default FS values from parameter space
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
byte param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer==0x4A)) {
param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_mag_fs = ((int16_t)(param[1]<<8) | param[0]);
EM7180_acc_fs = ((int16_t)(param[3]<<8) | param[2]);
Serial.print("Magnetometer Default Full Scale Range: +/-"); Serial.print(EM7180_mag_fs); Serial.println("uT");
Serial.print("Accelerometer Default Full Scale Range: +/-"); Serial.print(EM7180_acc_fs); Serial.println("g");
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75
param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer==0x4B)) {
param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_gyro_fs = ((int16_t)(param[1]<<8) | param[0]);
Serial.print("Gyroscope Default Full Scale Range: +/-"); Serial.print(EM7180_gyro_fs); Serial.println("dps");
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm
//Disable stillness mode for balancing robot application
EM7180_set_integer_param (0x49, 0x00);
//Write desired sensor full scale ranges to the EM7180
EM7180_set_mag_acc_FS (magFS, accFS); // 1000 uT == 0x3E8, 8 g == 0x08
EM7180_set_gyro_FS (gyroFS); // 2000 dps == 0x7D0
// Read sensor new FS values from parameter space
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer==0x4A)) {
param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_mag_fs = ((int16_t)(param[1]<<8) | param[0]);
EM7180_acc_fs = ((int16_t)(param[3]<<8) | param[2]);
Serial.print("Magnetometer New Full Scale Range: +/-"); Serial.print(EM7180_mag_fs); Serial.println("uT");
Serial.print("Accelerometer New Full Scale Range: +/-"); Serial.print(EM7180_acc_fs); Serial.println("g");
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75
param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer==0x4B)) {
param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);
EM7180_gyro_fs = ((int16_t)(param[1]<<8) | param[0]);
Serial.print("Gyroscope New Full Scale Range: +/-"); Serial.print(EM7180_gyro_fs); Serial.println("dps");
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm
// Read EM7180 status
uint8_t runStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RunStatus);
if(runStatus & 0x01) Serial.println(" EM7180 run status = normal mode");
uint8_t algoStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
if(algoStatus & 0x01) Serial.println(" EM7180 standby status");
if(algoStatus & 0x02) Serial.println(" EM7180 algorithm slow");
if(algoStatus & 0x04) Serial.println(" EM7180 in stillness mode");
if(algoStatus & 0x08) Serial.println(" EM7180 mag calibration completed");
if(algoStatus & 0x10) Serial.println(" EM7180 magnetic anomaly detected");
if(algoStatus & 0x20) Serial.println(" EM7180 unreliable sensor data");
uint8_t passthruStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_PassThruStatus);
if(passthruStatus & 0x01) Serial.print(" EM7180 in passthru mode!");
uint8_t eventStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_EventStatus);
if(eventStatus & 0x01) Serial.println(" EM7180 CPU reset");
if(eventStatus & 0x02) Serial.println(" EM7180 Error");
if(eventStatus & 0x04) Serial.println(" EM7180 new quaternion result");
if(eventStatus & 0x08) Serial.println(" EM7180 new mag result");
if(eventStatus & 0x10) Serial.println(" EM7180 new accel result");
if(eventStatus & 0x20) Serial.println(" EM7180 new gyro result");
delay(1000); // give some time to read the screen
// Check sensor status
uint8_t sensorStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SensorStatus);
Serial.print(" EM7180 sensor status = "); Serial.println(sensorStatus);
if(sensorStatus & 0x01) Serial.print("Magnetometer not acknowledging!");
if(sensorStatus & 0x02) Serial.print("Accelerometer not acknowledging!");
if(sensorStatus & 0x04) Serial.print("Gyro not acknowledging!");
if(sensorStatus & 0x10) Serial.print("Magnetometer ID not recognized!");
if(sensorStatus & 0x20) Serial.print("Accelerometer ID not recognized!");
if(sensorStatus & 0x40) Serial.print("Gyro ID not recognized!");
Serial.print("Actual MagRate = "); Serial.print(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualMagRate)); Serial.println(" Hz");
Serial.print("Actual AccelRate = "); Serial.print(10*(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualAccelRate))); Serial.println(" Hz");
Serial.print("Actual GyroRate = "); Serial.print(10*(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualGyroRate))); Serial.println(" Hz");
Serial.print("Actual BaroRate = "); Serial.print(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualBaroRate)); Serial.println(" Hz");
}
float USFS::uint32_reg_to_float (uint8_t *buf)
{
union {
uint32_t ui32;
float f;
} u;
u.ui32 = (((uint32_t)buf[0]) +
(((uint32_t)buf[1]) << 8) +
(((uint32_t)buf[2]) << 16) +
(((uint32_t)buf[3]) << 24));
return u.f;
}
float USFS::int32_reg_to_float (uint8_t *buf)
{
union {
int32_t i32;
float f;
} u;
u.i32 = (((int32_t)buf[0]) +
(((int32_t)buf[1]) << 8) +
(((int32_t)buf[2]) << 16) +
(((int32_t)buf[3]) << 24));
return u.f;
}
void USFS::float_to_bytes (float param_val, uint8_t *buf) {
union {
float f;
uint8_t comp[sizeof(float)];
} u;
u.f = param_val;
for (uint8_t i=0; i < sizeof(float); i++) {
buf[i] = u.comp[i];
}
//Convert to LITTLE ENDIAN
for (uint8_t i=0; i < sizeof(float); i++) {
buf[i] = buf[(sizeof(float)-1) - i];
}
}
void USFS::EM7180_set_gyro_FS (uint16_t gyro_fs) {
uint8_t bytes[4], STAT;
bytes[0] = gyro_fs & (0xFF);
bytes[1] = (gyro_fs >> 8) & (0xFF);
bytes[2] = 0x00;
bytes[3] = 0x00;
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Gyro LSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Gyro MSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Unused
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Unused
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); //Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a paramter write processs
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT==0xCB)) {
STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void USFS::EM7180_set_mag_acc_FS (uint16_t mag_fs, uint16_t acc_fs) {
uint8_t bytes[4], STAT;
bytes[0] = mag_fs & (0xFF);
bytes[1] = (mag_fs >> 8) & (0xFF);
bytes[2] = acc_fs & (0xFF);
bytes[3] = (acc_fs >> 8) & (0xFF);
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Mag LSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Mag MSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Acc LSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Acc MSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); //Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT==0xCA)) {
STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void USFS::EM7180_set_integer_param (uint8_t param, uint32_t param_val) {
uint8_t bytes[4], STAT;
bytes[0] = param_val & (0xFF);
bytes[1] = (param_val >> 8) & (0xFF);
bytes[2] = (param_val >> 16) & (0xFF);
bytes[3] = (param_val >> 24) & (0xFF);
param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, param);
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT==param)) {
STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void USFS::EM7180_set_float_param (uint8_t param, float param_val) {
uint8_t bytes[4], STAT;
float_to_bytes (param_val, &bytes[0]);
param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, param);
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT==param)) {
STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void USFS::readSENtralQuatData(float * destination)
{
uint8_t rawData[16]; // x/y/z quaternion register data stored here
_i2c_bus->readBytes(EM7180_ADDRESS, EM7180_QX, 16, &rawData[0]); // Read the sixteen raw data registers into data array
destination[1] = uint32_reg_to_float (&rawData[0]);
destination[2] = uint32_reg_to_float (&rawData[4]);
destination[3] = uint32_reg_to_float (&rawData[8]);
destination[0] = uint32_reg_to_float (&rawData[12]); // SENtral stores quats as qx, qy, qz, q0!
}
void USFS::readSENtralAccelData(int16_t * destination)
{
uint8_t rawData[6]; // x/y/z accel register data stored here
_i2c_bus->readBytes(EM7180_ADDRESS, EM7180_AX, 6, &rawData[0]); // Read the six raw data registers into data array
destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);
destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]);
}
void USFS::readSENtralGyroData(int16_t * destination)
{
uint8_t rawData[6]; // x/y/z gyro register data stored here
_i2c_bus->readBytes(EM7180_ADDRESS, EM7180_GX, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);
destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]);
}
void USFS::readSENtralMagData(int16_t * destination)
{
uint8_t rawData[6]; // x/y/z gyro register data stored here
_i2c_bus->readBytes(EM7180_ADDRESS, EM7180_MX, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);
destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]);
}
int16_t USFS::readSENtralBaroData()
{
uint8_t rawData[2]; // x/y/z gyro register data stored here
_i2c_bus->readBytes(EM7180_ADDRESS, EM7180_Baro, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
return (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
}
int16_t USFS::readSENtralTempData()
{
uint8_t rawData[2]; // x/y/z gyro register data stored here
_i2c_bus->readBytes(EM7180_ADDRESS, EM7180_Temp, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
return (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
}
void USFS::SENtralPassThroughMode()
{
// First put SENtral in standby mode
uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_AlgorithmControl);
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, c | 0x01);
// c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
// Serial.print("c = "); Serial.println(c);
// Verify standby status
// if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) {
Serial.println("SENtral in standby mode");
// Place SENtral in pass-through mode
_i2c_bus->writeByte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01) {
Serial.println("SENtral in pass-through mode");
}
else {
Serial.println("ERROR! SENtral not in pass-through mode!");
}
}
// I2C communication with the M24512DFM EEPROM is a little different from I2C communication with the usual motion sensor
// since the address is defined by two bytes
void USFS::M24512DFMwriteByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t data)
{
Wire.beginTransmission(device_address); // Initialize the Tx buffer
Wire.write(data_address1); // Put slave register address in Tx buffer
Wire.write(data_address2); // Put slave register address in Tx buffer
Wire.write(data); // Put data in Tx buffer
Wire.endTransmission(); // Send the Tx buffer
}
void USFS::M24512DFMwriteBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest)
{
if(count > 128)
{
count = 128;
Serial.print("Page count cannot be more than 128 bytes!");
}
Wire.beginTransmission(device_address); // Initialize the Tx buffer
Wire.write(data_address1); // Put slave register address in Tx buffer
Wire.write(data_address2); // Put slave register address in Tx buffer
for(uint8_t i=0; i < count; i++) {
Wire.write(dest[i]); // Put data in Tx buffer
}
Wire.endTransmission(); // Send the Tx buffer
}
uint8_t USFS::M24512DFMreadByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2)
{
uint8_t data; // `data` will store the register data
Wire.beginTransmission(device_address); // Initialize the Tx buffer
Wire.write(data_address1); // Put slave register address in Tx buffer
Wire.write(data_address2); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
Wire.requestFrom(device_address, 1); // Read one byte from slave register address
data = Wire.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}
void USFS::M24512DFMreadBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest)
{
uint8_t temp[2] = {data_address1, data_address2};
Wire.beginTransmission(device_address); // Initialize the Tx buffer
Wire.write(data_address1); // Put slave register address in Tx buffer
Wire.write(data_address2); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
uint8_t i = 0;
Wire.requestFrom(device_address, count); // Read bytes from slave register address
while (Wire.available()) {
dest[i++] = Wire.read(); } // Put read results in the Rx buffer
}
// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
void USFS::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = _q[0], q2 = _q[1], q3 = _q[2], q4 = _q[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - _beta * s1;
qDot2 = 0.5f * ( q1 * gx + q3 * gz - q4 * gy) - _beta * s2;
qDot3 = 0.5f * ( q1 * gy - q2 * gz + q4 * gx) - _beta * s3;
qDot4 = 0.5f * ( q1 * gz + q2 * gy - q3 * gx) - _beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * _deltat;
q2 += qDot2 * _deltat;
q3 += qDot3 * _deltat;
q4 += qDot4 * _deltat;
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
_q[0] = q1 * norm;
_q[1] = q2 * norm;
_q[2] = q3 * norm;
_q[3] = q4 * norm;
}
// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
// measured ones.
void USFS::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = _q[0], q2 = _q[1], q3 = _q[2], q4 = _q[3]; // short name local variable for readability
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
bx = sqrt((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 - q1q3);
vy = 2.0f * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;
wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
if (_Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx + _Kp * ex + _Ki * eInt[0];
gy = gy + _Kp * ey + _Ki * eInt[1];
gz = gz + _Kp * ez + _Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * _deltat);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * _deltat);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * _deltat);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * _deltat);
// Normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
_q[0] = q1 * norm;
_q[1] = q2 * norm;
_q[2] = q3 * norm;
_q[3] = q4 * norm;
}

126
EM7180_LSM6DSM_LIS2MDL_LPS22HB/USFS.h
Unescape View File

@ -0,0 +1,126 @@
#ifndef USFS_h
#define USFSh_h
#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"
// EM7180 SENtral register map
// see http://www.emdeveloper.com/downloads/7180/EMSentral_EM7180_Register_Map_v1_3.pdf
//
#define EM7180_QX 0x00 // this is a 32-bit normalized floating point number read from registers 0x00-03
#define EM7180_QY 0x04 // this is a 32-bit normalized floating point number read from registers 0x04-07
#define EM7180_QZ 0x08 // this is a 32-bit normalized floating point number read from registers 0x08-0B
#define EM7180_QW 0x0C // this is a 32-bit normalized floating point number read from registers 0x0C-0F
#define EM7180_QTIME 0x10 // this is a 16-bit unsigned integer read from registers 0x10-11
#define EM7180_MX 0x12 // int16_t from registers 0x12-13
#define EM7180_MY 0x14 // int16_t from registers 0x14-15
#define EM7180_MZ 0x16 // int16_t from registers 0x16-17
#define EM7180_MTIME 0x18 // uint16_t from registers 0x18-19
#define EM7180_AX 0x1A // int16_t from registers 0x1A-1B
#define EM7180_AY 0x1C // int16_t from registers 0x1C-1D
#define EM7180_AZ 0x1E // int16_t from registers 0x1E-1F
#define EM7180_ATIME 0x20 // uint16_t from registers 0x20-21
#define EM7180_GX 0x22 // int16_t from registers 0x22-23
#define EM7180_GY 0x24 // int16_t from registers 0x24-25
#define EM7180_GZ 0x26 // int16_t from registers 0x26-27
#define EM7180_GTIME 0x28 // uint16_t from registers 0x28-29
#define EM7180_Baro 0x2A // start of two-byte MS5637 pressure data, 16-bit signed interger
#define EM7180_BaroTIME 0x2C // start of two-byte MS5637 pressure timestamp, 16-bit unsigned
#define EM7180_Temp 0x2E // start of two-byte MS5637 temperature data, 16-bit signed interger
#define EM7180_TempTIME 0x30 // start of two-byte MS5637 temperature timestamp, 16-bit unsigned
#define EM7180_QRateDivisor 0x32 // uint8_t
#define EM7180_EnableEvents 0x33
#define EM7180_HostControl 0x34
#define EM7180_EventStatus 0x35
#define EM7180_SensorStatus 0x36
#define EM7180_SentralStatus 0x37
#define EM7180_AlgorithmStatus 0x38
#define EM7180_FeatureFlags 0x39
#define EM7180_ParamAcknowledge 0x3A
#define EM7180_SavedParamByte0 0x3B
#define EM7180_SavedParamByte1 0x3C
#define EM7180_SavedParamByte2 0x3D
#define EM7180_SavedParamByte3 0x3E
#define EM7180_ActualMagRate 0x45
#define EM7180_ActualAccelRate 0x46
#define EM7180_ActualGyroRate 0x47
#define EM7180_ActualBaroRate 0x48
#define EM7180_ActualTempRate 0x49
#define EM7180_ErrorRegister 0x50
#define EM7180_AlgorithmControl 0x54
#define EM7180_MagRate 0x55
#define EM7180_AccelRate 0x56
#define EM7180_GyroRate 0x57
#define EM7180_BaroRate 0x58
#define EM7180_TempRate 0x59
#define EM7180_LoadParamByte0 0x60
#define EM7180_LoadParamByte1 0x61
#define EM7180_LoadParamByte2 0x62
#define EM7180_LoadParamByte3 0x63
#define EM7180_ParamRequest 0x64
#define EM7180_ROMVersion1 0x70
#define EM7180_ROMVersion2 0x71
#define EM7180_RAMVersion1 0x72
#define EM7180_RAMVersion2 0x73
#define EM7180_ProductID 0x90
#define EM7180_RevisionID 0x91
#define EM7180_RunStatus 0x92
#define EM7180_UploadAddress 0x94 // uint16_t registers 0x94 (MSB)-5(LSB)
#define EM7180_UploadData 0x96
#define EM7180_CRCHost 0x97 // uint32_t from registers 0x97-9A
#define EM7180_ResetRequest 0x9B
#define EM7180_PassThruStatus 0x9E
#define EM7180_PassThruControl 0xA0
#define EM7180_ACC_LPF_BW 0x5B //Register GP36
#define EM7180_GYRO_LPF_BW 0x5C //Register GP37
#define EM7180_BARO_LPF_BW 0x5D //Register GP38
#define EM7180_ADDRESS 0x28 // Address of the EM7180 SENtral sensor hub
#define M24512DFM_DATA_ADDRESS 0x50 // Address of the 500 page M24512DFM EEPROM data buffer, 1024 bits (128 8-bit bytes) per page
#define M24512DFM_IDPAGE_ADDRESS 0x58 // Address of the single M24512DFM lockable EEPROM ID page
#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0
#define AK8963_ADDRESS 0x0C // Address of magnetometer
#define MS5637_ADDRESS 0x76 // Address of altimeter
class USFS
{
public:
USFS(uint8_t intPin, bool passThru, I2Cdev* i2c_bus);
float uint32_reg_to_float (uint8_t *buf);
float int32_reg_to_float (uint8_t *buf);
void float_to_bytes (float param_val, uint8_t *buf);
void EM7180_set_gyro_FS (uint16_t gyro_fs);
void EM7180_set_mag_acc_FS (uint16_t mag_fs, uint16_t acc_fs);
void EM7180_set_integer_param (uint8_t param, uint32_t param_val);
void EM7180_set_float_param (uint8_t param, float param_val);
void readSENtralQuatData(float * destination);
void readSENtralAccelData(int16_t * destination);
void readSENtralGyroData(int16_t * destination);
void readSENtralMagData(int16_t * destination);
void initEM7180(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, uint16_t magFS, uint8_t QRtDiv, uint8_t magRt, uint8_t accRt, uint8_t gyroRt, uint8_t baroRt);
int16_t readSENtralBaroData();
int16_t readSENtralTempData();
void SENtralPassThroughMode();
void M24512DFMwriteByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t data);
void M24512DFMwriteBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest);
uint8_t M24512DFMreadByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2);
void M24512DFMreadBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest);
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
void getChipID();
void loadfwfromEEPROM();
uint8_t checkEM7180Status();
uint8_t checkEM7180Errors();
private:
uint8_t _intPin;
bool _passThru;
float _q[4];
float _beta;
float _deltat;
float _Kp;
float _Ki;
I2Cdev* _i2c_bus;
};
#endif
Write Preview
Loading…
Cancel
Save