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

Add files via upload

master
Kris Winer 7 years ago committed by GitHub
parent
9e4300bf44
commit
6c4ce9ae32
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 3377 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. 701
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly.ino
  2. 244
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LIS2MDL.cpp
  3. 74
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LIS2MDL.h
  4. 132
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LPS22HB.cpp
  5. 71
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LPS22HB.h
  6. 289
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LSM6DSM.cpp
  7. 179
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LSM6DSM.h
  8. 97
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/MadgwickFilter.ino
  9. 1262
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/USFS.cpp
  10. 328
      EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/USFS.h

701
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly.ino
Unescape View File

@ -0,0 +1,701 @@
#include "LSM6DSM.h"
#include "LIS2MDL.h"
#include "LPS22HB.h"
#include "USFS.h"
#include <RTC.h>
bool SerialDebug = true; // set to true to get Serial output for debugging
bool passThru = false;
#define myLed 13
const char *build_date = __DATE__; // 11 characters MMM DD YYYY
const char *build_time = __TIME__; // 8 characters HH:MM:SS
// 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 A12, 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); // 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); // 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);
// RTC set time using STM32L4 natve RTC class
/* Change these values to set the current initial time */
uint8_t seconds = 0;
uint8_t minutes = 33;
uint8_t hours = 12;
/* Change these values to set the current initial date */
uint8_t day = 2;
uint8_t month = 12;
uint8_t year = 17;
uint8_t Seconds, Minutes, Hours, Day, Month, Year;
bool alarmFlag = false; // for RTC alarm interrupt
const uint8_t USFS_intPin = 8;
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, MPU9250 and MS5637 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
*/
uint8_t accBW = 0x03, gyroBW = 0x03, QRtDiv = 0x01, magRt = 0x64, accRt = 0x14, gyroRt = 0x14, baroRt = 0x32;
/*
Choose MPU9250 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 = 0x08, gyroFS = 0x7D0, magFS = 0x3E8;
USFS USFS(USFS_intPin, passThru);
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
delay(4000);
// Configure led
pinMode(myLed, OUTPUT);
digitalWrite(myLed, HIGH); // start with led off
Wire.begin(TWI_PINS_20_21); // set master mode
Wire.setClock(400000); // I2C frequency at 400 kHz
delay(1000);
LSM6DSM.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
// Set the time
SetDefaultRTC();
/* Set up the RTC alarm interrupt */
RTC.enableAlarm(RTC.MATCH_ANY); // alarm once a second
RTC.attachInterrupt(alarmMatch); // interrupt every time the alarm sounds
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
}
}
/* 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++;
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
/*RTC*/
if(alarmFlag) { // update RTC output (serial display) whenever the RTC alarm condition is achieved and the MPU9250 is awake
alarmFlag = false;
// Read RTC
if(SerialDebug)
{
Serial.println("RTC:");
Day = RTC.getDay();
Month = RTC.getMonth();
Year = RTC.getYear();
Seconds = RTC.getSeconds();
Minutes = RTC.getMinutes();
Hours = RTC.getHours();
if(Hours < 10) {Serial.print("0"); Serial.print(Hours);} else Serial.print(Hours);
Serial.print(":");
if(Minutes < 10) {Serial.print("0"); Serial.print(Minutes);} else Serial.print(Minutes);
Serial.print(":");
if(Seconds < 10) {Serial.print("0"); Serial.println(Seconds);} else Serial.println(Seconds);
Serial.print(Month); Serial.print("/"); Serial.print(Day); Serial.print("/"); Serial.println(Year);
Serial.println(" ");
}
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:
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("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 - pow(((Pressure) / 1013.25f), 0.190284f));
Serial.print("Altitude = ");
Serial.print(Altitude, 2);
Serial.println(" feet");
Serial.println(" ");
}
} // end of "if(!passThru)" handling
} // end of RTC alarm handling
digitalWrite(myLed, LOW); delay(10); digitalWrite(myLed, HIGH); // flash led for 10 milliseconds
STM32.sleep();
} //end of loop
/* End of main loop */
void myinthandler1()
{
newLSM6DSMData = true;
}
void myinthandler2()
{
newLIS2MDLData = true;
}
void myinthandler3()
{
newLPS22HData = true;
}
void EM7180intHandler()
{
newEM7180Data = true;
}
void alarmMatch()
{
alarmFlag = true;
}
void SetDefaultRTC() // Sets the RTC to the FW build date-time...
{
char Build_mo[3];
// Convert month string to integer
Build_mo[0] = build_date[0];
Build_mo[1] = build_date[1];
Build_mo[2] = build_date[2];
String build_mo = Build_mo;
if(build_mo == "Jan")
{
month = 1;
} else if(build_mo == "Feb")
{
month = 2;
} else if(build_mo == "Mar")
{
month = 3;
} else if(build_mo == "Apr")
{
month = 4;
} else if(build_mo == "May")
{
month = 5;
} else if(build_mo == "Jun")
{
month = 6;
} else if(build_mo == "Jul")
{
month = 7;
} else if(build_mo == "Aug")
{
month = 8;
} else if(build_mo == "Sep")
{
month = 9;
} else if(build_mo == "Oct")
{
month = 10;
} else if(build_mo == "Nov")
{
month = 11;
} else if(build_mo == "Dec")
{
month = 12;
} else
{
month = 1; // Default to January if something goes wrong...
}
// Convert ASCII strings to integers
day = (build_date[4] - 48)*10 + build_date[5] - 48; // ASCII "0" = 48
year = (build_date[9] - 48)*10 + build_date[10] - 48;
hours = (build_time[0] - 48)*10 + build_time[1] - 48;
minutes = (build_time[3] - 48)*10 + build_time[4] - 48;
seconds = (build_time[6] - 48)*10 + build_time[7] - 48;
// Set the date/time
RTC.setDay(day);
RTC.setMonth(month);
RTC.setYear(year);
RTC.setHours(hours);
RTC.setMinutes(minutes);
RTC.setSeconds(seconds);
}

244
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LIS2MDL.cpp
Unescape View File

@ -0,0 +1,244 @@
/* 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)
{
pinMode(intPin, INPUT);
_intPin = intPin;
}
uint8_t LIS2MDL::getChipID()
{
uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
return c;
}
void LIS2MDL::reset()
{
// reset device
uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
delay(1);
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)
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR<<2);
// enable low pass filter (bit 0 == 1), set to ODR/4
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)
writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
}
uint8_t LIS2MDL::status()
{
// Read the status register of the altimeter
uint8_t temp = 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
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
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 = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
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;
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
}
// I2C scan function
void LIS2MDL::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 Write.endTransmission to see if
// a device did acknowledge to the address.
// Wire.beginTransmission(address);
// error = Wire.endTransmission();
error = Wire.transfer(address, NULL, 0, NULL, 0);
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("done\n");
}
// I2C read/write functions for the LIS2MDL
void LIS2MDL::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
uint8_t temp[2];
temp[0] = subAddress;
temp[1] = data;
Wire.transfer(address, &temp[0], 2, NULL, 0);
}
uint8_t LIS2MDL::readByte(uint8_t address, uint8_t subAddress) {
uint8_t temp[1];
Wire.transfer(address, &subAddress, 1, &temp[0], 1);
return temp[0];
}
void LIS2MDL::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) {
Wire.transfer(address, &subAddress, 1, dest, count);
}

74
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LIS2MDL.h
Unescape View File

@ -0,0 +1,74 @@
/* 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>
//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);
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;
};
#endif

132
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LPS22HB.cpp
Unescape View File

@ -0,0 +1,132 @@
/* 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)
{
pinMode(intPin, INPUT);
_intPin = intPin;
}
uint8_t LPS22H::getChipID()
{
// Read the WHO_AM_I register of the altimeter this is a good test of communication
uint8_t temp = 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 = readByte(LPS22H_ADDRESS, LPS22H_STATUS);
return temp;
}
int32_t LPS22H::readAltimeterPressure()
{
uint8_t rawData[3]; // 24-bit pressure register data stored here
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
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
writeByte(LPS22H_ADDRESS, LPS22H_CTRL_REG1, PODR << 4 | 0x08 | 0x02);
writeByte(LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04); // enable data ready as interrupt source
}
// I2C scan function
void LPS22H::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 Write.endTransmission to see if
// a device did acknowledge to the address.
Wire.beginTransmission(address);
error = Wire.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("done\n");
}
void LPS22H::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.write(data); // Put data in Tx buffer
Wire.endTransmission(); // Send the Tx buffer
}
uint8_t LPS22H::readByte(uint8_t address, uint8_t subAddress)
{
uint8_t data; // `data` will store the register data
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
Wire.requestFrom(address, (size_t) 1); // Read one uint8_t from slave register address
data = Wire.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}
void LPS22H::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // 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(address, (size_t)count); // Read bytes from slave register address
while (Wire.available()) {
dest[i++] = Wire.read(); } // Put read results in the Rx buffer
}

71
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LPS22HB.h
Unescape View File

@ -0,0 +1,71 @@
/* 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"
// 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);
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;
};
#endif

289
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LSM6DSM.cpp
Unescape View File

@ -0,0 +1,289 @@
/* 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)
{
pinMode(intPin1, INPUT);
_intPin1 = intPin1;
pinMode(intPin2, INPUT);
_intPin2 = intPin2;
}
uint8_t LSM6DSM::getChipID()
{
uint8_t c = 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 = readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
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)
{
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL, AODR << 4 | Ascale << 2);
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G, GODR << 4 | Gscale << 2);
uint8_t temp = readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
// enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
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
writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08 );
// interrupt handling
writeByte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40); // enable significant motion interrupts on INT1
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];
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];
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];
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];
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];
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
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] ;
}
// I2C scan function
void LSM6DSM::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 Write.endTransmission to see if
// a device did acknowledge to the address.
// Wire.beginTransmission(address);
// error = Wire.endTransmission();
error = Wire.transfer(address, NULL, 0, NULL, 0);
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("done\n");
}
// I2C read/write functions for the LSM6DSM
void LSM6DSM::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
uint8_t temp[2];
temp[0] = subAddress;
temp[1] = data;
Wire.transfer(address, &temp[0], 2, NULL, 0);
}
uint8_t LSM6DSM::readByte(uint8_t address, uint8_t subAddress) {
uint8_t temp[1];
Wire.transfer(address, &subAddress, 1, &temp[0], 1);
return temp[0];
}
void LSM6DSM::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) {
Wire.transfer(address, &subAddress, 1, dest, count);
}

179
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/LSM6DSM.h
Unescape View File

@ -0,0 +1,179 @@
/* 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>
/* 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);
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;
};
#endif

97
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/MadgwickFilter.ino
Unescape View File

@ -0,0 +1,97 @@
// 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!
__attribute__((optimize("O3"))) void 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 = sqrtf(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 = sqrtf(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 = sqrtf(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 = sqrtf(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 = sqrtf(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;
}

1262
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/USFS.cpp
View File

File diff suppressed because it is too large Load Diff

328
EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly/USFS.h
Unescape View File

@ -0,0 +1,328 @@
#ifndef USFS_h
#define USFSh_h
#include "Arduino.h"
#include "Wire.h"
// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet
#define MS5637_RESET 0x1E
#define MS5637_CONVERT_D1 0x40
#define MS5637_CONVERT_D2 0x50
#define MS5637_ADC_READ 0x00
// See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in
// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
//
//Magnetometer Registers
#define AK8963_ADDRESS 0x0C
#define WHO_AM_I_AK8963 0x00 // should return 0x48
#define INFO 0x01
#define AK8963_ST1 0x02 // data ready status bit 0
#define AK8963_XOUT_L 0x03 // data
#define AK8963_XOUT_H 0x04
#define AK8963_YOUT_L 0x05
#define AK8963_YOUT_H 0x06
#define AK8963_ZOUT_L 0x07
#define AK8963_ZOUT_H 0x08
#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC 0x0C // Self test control
#define AK8963_I2CDIS 0x0F // I2C disable
#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
#define SELF_TEST_X_GYRO 0x00
#define SELF_TEST_Y_GYRO 0x01
#define SELF_TEST_Z_GYRO 0x02
/*#define X_FINE_GAIN 0x03 // [7:0] fine gain
#define Y_FINE_GAIN 0x04
#define Z_FINE_GAIN 0x05
#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
#define XA_OFFSET_L_TC 0x07
#define YA_OFFSET_H 0x08
#define YA_OFFSET_L_TC 0x09
#define ZA_OFFSET_H 0x0A
#define ZA_OFFSET_L_TC 0x0B */
#define SELF_TEST_X_ACCEL 0x0D
#define SELF_TEST_Y_ACCEL 0x0E
#define SELF_TEST_Z_ACCEL 0x0F
#define SELF_TEST_A 0x10
#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
#define XG_OFFSET_L 0x14
#define YG_OFFSET_H 0x15
#define YG_OFFSET_L 0x16
#define ZG_OFFSET_H 0x17
#define ZG_OFFSET_L 0x18
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_CONFIG2 0x1D
#define LP_ACCEL_ODR 0x1E
#define WOM_THR 0x1F
#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
#define FIFO_EN 0x23
#define I2C_MST_CTRL 0x24
#define I2C_SLV0_ADDR 0x25
#define I2C_SLV0_REG 0x26
#define I2C_SLV0_CTRL 0x27
#define I2C_SLV1_ADDR 0x28
#define I2C_SLV1_REG 0x29
#define I2C_SLV1_CTRL 0x2A
#define I2C_SLV2_ADDR 0x2B
#define I2C_SLV2_REG 0x2C
#define I2C_SLV2_CTRL 0x2D
#define I2C_SLV3_ADDR 0x2E
#define I2C_SLV3_REG 0x2F
#define I2C_SLV3_CTRL 0x30
#define I2C_SLV4_ADDR 0x31
#define I2C_SLV4_REG 0x32
#define I2C_SLV4_DO 0x33
#define I2C_SLV4_CTRL 0x34
#define I2C_SLV4_DI 0x35
#define I2C_MST_STATUS 0x36
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define DMP_INT_STATUS 0x39 // Check DMP interrupt
#define INT_STATUS 0x3A
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define EXT_SENS_DATA_00 0x49
#define EXT_SENS_DATA_01 0x4A
#define EXT_SENS_DATA_02 0x4B
#define EXT_SENS_DATA_03 0x4C
#define EXT_SENS_DATA_04 0x4D
#define EXT_SENS_DATA_05 0x4E
#define EXT_SENS_DATA_06 0x4F
#define EXT_SENS_DATA_07 0x50
#define EXT_SENS_DATA_08 0x51
#define EXT_SENS_DATA_09 0x52
#define EXT_SENS_DATA_10 0x53
#define EXT_SENS_DATA_11 0x54
#define EXT_SENS_DATA_12 0x55
#define EXT_SENS_DATA_13 0x56
#define EXT_SENS_DATA_14 0x57
#define EXT_SENS_DATA_15 0x58
#define EXT_SENS_DATA_16 0x59
#define EXT_SENS_DATA_17 0x5A
#define EXT_SENS_DATA_18 0x5B
#define EXT_SENS_DATA_19 0x5C
#define EXT_SENS_DATA_20 0x5D
#define EXT_SENS_DATA_21 0x5E
#define EXT_SENS_DATA_22 0x5F
#define EXT_SENS_DATA_23 0x60
#define MOT_DETECT_STATUS 0x61
#define I2C_SLV0_DO 0x63
#define I2C_SLV1_DO 0x64
#define I2C_SLV2_DO 0x65
#define I2C_SLV3_DO 0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET 0x68
#define MOT_DETECT_CTRL 0x69
#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
#define PWR_MGMT_2 0x6C
#define DMP_BANK 0x6D // Activates a specific bank in the DMP
#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
#define DMP_REG_1 0x70
#define DMP_REG_2 0x71
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
#define XA_OFFSET_H 0x77
#define XA_OFFSET_L 0x78
#define YA_OFFSET_H 0x7A
#define YA_OFFSET_L 0x7B
#define ZA_OFFSET_H 0x7D
#define ZA_OFFSET_L 0x7E
// 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
#define AFS_2G 0
#define AFS_4G 1
#define AFS_8G 2
#define AFS_16G 3
#define GFS_250DPS 0
#define GFS_500DPS 1
#define GFS_1000DPS 2
#define GFS_2000DPS 3
#define MFS_14BITS 0 // 0.6 mG per LSB
#define MFS_16BITS 1 // 0.15 mG per LSB
#define ADC_256 0x00 // define pressure and temperature conversion rates
#define ADC_512 0x02
#define ADC_1024 0x04
#define ADC_2048 0x06
#define ADC_4096 0x08
#define ADC_8192 0x0A
#define ADC_D1 0x40
#define ADC_D2 0x50
class USFS
{
public:
USFS(uint8_t intPin, bool passThru);
float getAres(uint8_t Ascale);
float getGres(uint8_t Gscale);
float getMres(uint8_t Mscale);
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 readAccelData(int16_t * destination);
void readGyroData(int16_t * destination);
void readMagData(int16_t * destination);
int16_t readTempData();
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);
void initAK8963(uint8_t Mscale, uint8_t Mmode, float * destination);
void initMPU9250(uint8_t Ascale, uint8_t Gscale);
void accelgyrocalMPU9250(float * dest1, float * dest2);
void magcalMPU9250(float * dest1, float * dest2);
void MPU9250SelfTest(float * destination);
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 MS5637Reset();
void MS5637PromRead(uint16_t * destination);
uint32_t MS5637Read(uint8_t CMD, uint8_t OSR);
unsigned char MS5637checkCRC(uint16_t * n_prom);
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();
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;
bool _passThru;
float _aRes;
float _gRes;
float _mRes;
uint8_t _Mmode;
float _fuseROMx;
float _fuseROMy;
float _fuseROMz;
float _q[4];
float _beta;
float _deltat;
float _Kp;
float _Ki;
};
#endif
Write Preview
Loading…
Cancel
Save