41 changed files with 299 additions and 22720 deletions
@ -1,13 +1,18 @@
@@ -1,13 +1,18 @@
|
||||
/* 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. |
||||
|
||||
*/ |
||||
/*
|
||||
* lis2mdl.c |
||||
* The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 10 DoF |
||||
* absolute orientation solution. |
||||
* |
||||
* Created on: Jan 18, 2021 |
||||
* Author: Daniel Peter Chokola |
||||
* |
||||
* Adapted From: |
||||
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly |
||||
* by: Kris Winer |
||||
* 09/23/2017 Copyright Tlera Corporation |
||||
* |
||||
* Library may be used freely and without limit with attribution. |
||||
*/ |
||||
|
||||
#ifndef LIS2MDL_h |
||||
#define LIS2MDL_h |
||||
@ -1,13 +1,18 @@
@@ -1,13 +1,18 @@
|
||||
/* 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. |
||||
|
||||
*/ |
||||
/*
|
||||
* lps22hb.h |
||||
* The LPS22HB is a low power barometer, here used as 1 DoF in a 10 DoF |
||||
* absolute orientation solution. |
||||
* |
||||
* Created on: Jan 18, 2021 |
||||
* Author: Daniel Peter Chokola |
||||
* |
||||
* Adapted From: |
||||
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly |
||||
* by: Kris Winer |
||||
* 09/23/2017 Copyright Tlera Corporation |
||||
* |
||||
* Library may be used freely and without limit with attribution. |
||||
*/ |
||||
|
||||
#ifndef LPS22HB_h |
||||
#define LPS22HB_h |
||||
@ -1,13 +1,18 @@
@@ -1,13 +1,18 @@
|
||||
/* 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. |
||||
|
||||
*/ |
||||
/*
|
||||
* lsm6dsm.h |
||||
* The LSM6DSM is a sensor hub with embedded accelerometer and gyroscope, here |
||||
* used as 6 DoF in a 10 DoF absolute orientation solution. |
||||
* |
||||
* Created on: Jan 18, 2021 |
||||
* Author: Daniel Peter Chokola |
||||
* |
||||
* Adapted From: |
||||
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly |
||||
* by: Kris Winer |
||||
* 09/23/2017 Copyright Tlera Corporation |
||||
* |
||||
* Library may be used freely and without limit with attribution. |
||||
*/ |
||||
|
||||
#ifndef LSM6DSM_h |
||||
#define LSM6DSM_h |
||||
@ -1,12 +1,19 @@
@@ -1,12 +1,19 @@
|
||||
/* 06/29/2017 Copyright Tlera Corporation
|
||||
/*
|
||||
* em7180.c |
||||
* |
||||
* Created by Kris Winer |
||||
* Created on: Jan 18, 2021 |
||||
* Author: Daniel Peter Chokola |
||||
* |
||||
* Adapted From: |
||||
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly |
||||
* by: Kris Winer |
||||
* 06/29/2017 Copyright Tlera Corporation |
||||
* |
||||
* Library may be used freely and without limit with attribution. |
||||
*
|
||||
*/ |
||||
#include "USFS.h" |
||||
|
||||
/* Includes */ |
||||
#include "em7180.h" |
||||
|
||||
USFS::USFS(uint8_t intPin, bool passThru) |
||||
{ |
||||
@ -1,15 +1,20 @@
@@ -1,15 +1,20 @@
|
||||
/* 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.c |
||||
* The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 10 DoF |
||||
* absolute orientation solution. |
||||
* |
||||
* Created on: Jan 18, 2021 |
||||
* Author: Daniel Peter Chokola |
||||
* |
||||
* Adapted From: |
||||
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly |
||||
* by: Kris Winer |
||||
* 09/23/2017 Copyright Tlera Corporation |
||||
* |
||||
* Library may be used freely and without limit with attribution. |
||||
*/ |
||||
|
||||
#include "lis2mdl.h" |
||||
|
||||
LIS2MDL::LIS2MDL(uint8_t intPin) |
||||
{ |
||||
@ -1,15 +1,20 @@
@@ -1,15 +1,20 @@
|
||||
/* 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" |
||||
/*
|
||||
* lps22hb.c |
||||
* The LPS22HB is a low power barometer, here used as 1 DoF in a 10 DoF |
||||
* absolute orientation solution. |
||||
* |
||||
* Created on: Jan 18, 2021 |
||||
* Author: Daniel Peter Chokola |
||||
* |
||||
* Adapted From: |
||||
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly |
||||
* by: Kris Winer |
||||
* 09/23/2017 Copyright Tlera Corporation |
||||
* |
||||
* Library may be used freely and without limit with attribution. |
||||
*/ |
||||
|
||||
#include "lps22hb.h" |
||||
#include "Wire.h" |
||||
|
||||
LPS22H::LPS22H(uint8_t intPin) |
||||
@ -1,15 +1,20 @@
@@ -1,15 +1,20 @@
|
||||
/* 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.c |
||||
* The LSM6DSM is a sensor hub with embedded accelerometer and gyroscope, here |
||||
* used as 6 DoF in a 10 DoF absolute orientation solution. |
||||
* |
||||
* Created on: Jan 18, 2021 |
||||
* Author: Daniel Peter Chokola |
||||
* |
||||
* Adapted From: |
||||
* EM7180_LSM6DSM_LIS2MDL_LPS22HB_Butterfly |
||||
* by: Kris Winer |
||||
* 09/23/2017 Copyright Tlera Corporation |
||||
* |
||||
* Library may be used freely and without limit with attribution. |
||||
*/ |
||||
|
||||
#include "lsm6dsm.h" |
||||
|
||||
LSM6DSM::LSM6DSM(uint8_t intPin1, uint8_t intPin2) |
||||
{ |
||||
File diff suppressed because it is too large
Load Diff
@ -1,593 +0,0 @@
@@ -1,593 +0,0 @@
|
||||
#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; |
||||
} |
||||
|
||||
|
||||
|
||||
|
||||
@ -1,173 +0,0 @@
@@ -1,173 +0,0 @@
|
||||
/*
|
||||
* 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"); |
||||
} |
||||
|
||||
@ -1,48 +0,0 @@
@@ -1,48 +0,0 @@
|
||||
/*
|
||||
* 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_
|
||||
@ -1,175 +0,0 @@
@@ -1,175 +0,0 @@
|
||||
/* 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
|
||||
} |
||||
|
||||
@ -1,75 +0,0 @@
@@ -1,75 +0,0 @@
|
||||
/* 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 |
||||
@ -1,60 +0,0 @@
@@ -1,60 +0,0 @@
|
||||
/* 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
|
||||
} |
||||
|
||||
|
||||
@ -1,73 +0,0 @@
@@ -1,73 +0,0 @@
|
||||
/* 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 |
||||
@ -1,225 +0,0 @@
@@ -1,225 +0,0 @@
|
||||
/* 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] ;
|
||||
} |
||||
@ -1,180 +0,0 @@
@@ -1,180 +0,0 @@
|
||||
/* 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 |
||||
@ -1 +0,0 @@
@@ -1 +0,0 @@
|
||||
Sample sketch for the EM7180 as mster to LSM6DSM accel/gyro, LIS2MDL magnetometer, and LPS22HB barometer. Should be able to run on any Arduino-compatible platform. Intended for master (normal) mode use, passthrough mode might need some work to get it to function (mostly setting up the interrupts properly). |
||||
@ -1,670 +0,0 @@
@@ -1,670 +0,0 @@
|
||||
/* 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; |
||||
|
||||
} |
||||
@ -1,126 +0,0 @@
@@ -1,126 +0,0 @@
|
||||
#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 |
||||
@ -1,701 +0,0 @@
@@ -1,701 +0,0 @@
|
||||
#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); |
||||
} |
||||
|
||||
|
||||
@ -1,97 +0,0 @@
@@ -1,97 +0,0 @@
|
||||
// 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; |
||||
|
||||
} |
||||
@ -1,2 +0,0 @@
@@ -1,2 +0,0 @@
|
||||
Sketch for the newest Ultimate Sensor Fusion Solution using the latest ST motion sensors: combination accel/gyro LSM6DSM, magnetometer LIS2MDL, and barometer LPS22HB. Now sold on [Tindie](https://www.tindie.com/products/onehorse/ultimate-sensor-fusion-solution-lsm6dsm--lis2md/). |
||||
 |
||||
File diff suppressed because it is too large
Load Diff
@ -1,382 +0,0 @@
@@ -1,382 +0,0 @@
|
||||
#include <SdFat.h> |
||||
#include <i2c_t3.h> |
||||
|
||||
#define SD_CS 10 |
||||
#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_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_ErrorRegister 0x50 |
||||
#define EM7180_AlgorithmControl 0x54 |
||||
#define EM7180_MagRate 0x55 |
||||
#define EM7180_AccelRate 0x56 |
||||
#define EM7180_GyroRate 0x57 |
||||
#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_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 |
||||
|
||||
// Using the Teensy Mini Add-On board, BMX055 SDO1 = SDO2 = CSB3 = GND as designed
|
||||
// Seven-bit BMX055 device addresses are ACC = 0x18, GYRO = 0x68, MAG = 0x10
|
||||
#define BMX055_ACC_ADDRESS 0x18 // Address of BMX055 accelerometer
|
||||
#define BMX055_GYRO_ADDRESS 0x68 // Address of BMX055 gyroscope
|
||||
#define BMX055_MAG_ADDRESS 0x10 // Address of BMX055 magnetometer
|
||||
#define MS5637_ADDRESS 0x76 // Address of MS5637 altimeter
|
||||
#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_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
|
||||
|
||||
SdFat SD; |
||||
SdFile sd_file; |
||||
|
||||
void setup() { |
||||
|
||||
// Setup for Master mode, pins 18/19, external pullups, 400kHz for Teensy 3.1
|
||||
Wire.begin(I2C_MASTER, 0x00, I2C_PINS_16_17, I2C_PULLUP_EXT, I2C_RATE_400); |
||||
|
||||
Serial.begin(9600); |
||||
delay(5000); |
||||
|
||||
while (!SD.begin(SD_CS, SPI_HALF_SPEED)) { |
||||
Serial.println("failed to init sd"); |
||||
Serial.printf("err: %02x\n", SD.card()->errorCode()); |
||||
} |
||||
Serial.println("sd init"); |
||||
|
||||
I2Cscan(); |
||||
|
||||
// Put EM7180 SENtral into pass-through mode
|
||||
SENtralPassThroughMode(); |
||||
delay(1000); |
||||
|
||||
I2Cscan(); |
||||
|
||||
sd_file.open("/EM6500.fw", O_RDONLY);
|
||||
Serial.println("File Open!"); |
||||
|
||||
uint8_t buffer[128]; |
||||
uint8_t numbytes= 0, MSadd = 0, totnum = 0; |
||||
|
||||
Serial.println("erasing EEPROM"); |
||||
uint8_t eraseBuffer[128] = { |
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; |
||||
|
||||
for (MSadd = 0; MSadd < 256; MSadd++) { // MS address byte, 0 to 255
|
||||
M24512DFMwriteBytes(M24512DFM_DATA_ADDRESS, MSadd, 0x00, 128, eraseBuffer); // write data starting at first byte of page MSadd
|
||||
delay(100); |
||||
M24512DFMwriteBytes(M24512DFM_DATA_ADDRESS, MSadd, 0x80, 128, eraseBuffer); // write data starting at 128th byte of page MSadd
|
||||
delay(100); |
||||
|
||||
totnum++; |
||||
if (MSadd = 255) { break; } |
||||
Serial.print("totnum"); Serial.println(totnum); |
||||
Serial.print("MSadd 0x"); Serial.println(MSadd, HEX); |
||||
} |
||||
|
||||
// Verify EEPROM ihas been erased
|
||||
// Read first page of EEPROM
|
||||
uint8_t data[128]; |
||||
M24512DFMreadBytes(M24512DFM_DATA_ADDRESS, 0x00, 0x00, 128, data); |
||||
Serial.println("EEPROM first page");
|
||||
|
||||
for (int i = 0; i < 16; i++) { |
||||
Serial.println(" "); |
||||
|
||||
for (int j = 0; j < 8; j++) { |
||||
Serial.print(data[i*8 + j], HEX); Serial.print(" "); |
||||
} |
||||
} |
||||
|
||||
// Read second page of EEPROM
|
||||
M24512DFMreadBytes(M24512DFM_DATA_ADDRESS, 0x00, 0x80, 128, data); |
||||
Serial.println("");Serial.println("EEPROM second page");
|
||||
|
||||
for (int i = 0; i < 16; i++) { |
||||
Serial.println(" "); |
||||
|
||||
for (int j = 0; j < 8; j++) { |
||||
Serial.print(data[i*8 + j], HEX); Serial.print(" "); |
||||
} |
||||
} |
||||
|
||||
// Read third page of EEPROM
|
||||
M24512DFMreadBytes(M24512DFM_DATA_ADDRESS, 0x01, 0x00, 128, data); |
||||
Serial.println("");Serial.println("EEPROM third page");
|
||||
|
||||
for (int i = 0; i < 16; i++) { |
||||
Serial.println(" "); |
||||
|
||||
for (int j = 0; j < 8; j++) { |
||||
Serial.print(data[i*8 + j], HEX); Serial.print(" "); |
||||
} |
||||
} |
||||
|
||||
// write configuration file to EEPROM
|
||||
Serial.println("writing data to EEPROM"); |
||||
for (MSadd = 0; MSadd < 256; MSadd++) { // MS address byte, 0 to 255
|
||||
numbytes = sd_file.read(buffer, 128); // 128 bytes per page, 500 pages
|
||||
Serial.print("first two bytes: "); Serial.print("0x"); Serial.print(buffer[0], HEX); Serial.print("0x"); Serial.println(buffer[1], HEX); |
||||
Serial.print("Number of bytes = "); Serial.println(numbytes); // print number of bytes read
|
||||
M24512DFMwriteBytes(M24512DFM_DATA_ADDRESS, MSadd, 0x00, 128, buffer); // write data starting at first byte of page MSadd
|
||||
delay(100); |
||||
numbytes = sd_file.read(buffer, 128); // 128 bytes per page, 500 pages
|
||||
Serial.print("first two bytes: "); Serial.print("0x"); Serial.print(buffer[0], HEX); Serial.print("0x"); Serial.println(buffer[1], HEX); |
||||
Serial.print("Number of bytes = "); Serial.println(numbytes); // print number of bytes read
|
||||
M24512DFMwriteBytes(M24512DFM_DATA_ADDRESS, MSadd, 0x80, 128, buffer); // write data starting at 128th byte of page MSadd
|
||||
delay(100); |
||||
|
||||
if (numbytes < 128) { break; } |
||||
totnum++; |
||||
Serial.print("totnum"); Serial.println(totnum); |
||||
Serial.print("MSadd 0x"); Serial.println(MSadd, HEX); |
||||
} |
||||
|
||||
|
||||
// Read first page of EEPROM
|
||||
M24512DFMreadBytes(M24512DFM_DATA_ADDRESS, 0x00, 0x00, 128, data); |
||||
Serial.println("EEPROM first page");
|
||||
|
||||
for (int i = 0; i < 16; i++) { |
||||
Serial.println(" "); |
||||
|
||||
for (int j = 0; j < 8; j++) { |
||||
Serial.print(data[i*8 + j], HEX); Serial.print(" "); |
||||
} |
||||
} |
||||
|
||||
// Read second page of EEPROM
|
||||
M24512DFMreadBytes(M24512DFM_DATA_ADDRESS, 0x00, 0x80, 128, data); |
||||
Serial.println("");Serial.println("EEPROM second page");
|
||||
|
||||
for (int i = 0; i < 16; i++) { |
||||
Serial.println(" "); |
||||
|
||||
for (int j = 0; j < 8; j++) { |
||||
Serial.print(data[i*8 + j], HEX); Serial.print(" "); |
||||
} |
||||
} |
||||
|
||||
// Read third page of EEPROM
|
||||
M24512DFMreadBytes(M24512DFM_DATA_ADDRESS, 0x01, 0x00, 128, data); |
||||
Serial.println("");Serial.println("EEPROM third page");
|
||||
|
||||
for (int i = 0; i < 16; i++) { |
||||
Serial.println(" "); |
||||
|
||||
for (int j = 0; j < 8; j++) { |
||||
Serial.print(data[i*8 + j], HEX); Serial.print(" "); |
||||
} |
||||
} |
||||
|
||||
|
||||
} |
||||
|
||||
void loop() { |
||||
} |
||||
|
||||
// I2C read/write functions for the MPU9250 and AK8963 sensors
|
||||
|
||||
void 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 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(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
|
||||
// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
|
||||
// Wire.requestFrom(address, 1); // Read one byte from slave register address
|
||||
Wire.requestFrom(address, (size_t) 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 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(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
|
||||
// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
|
||||
uint8_t i = 0; |
||||
// Wire.requestFrom(address, count); // Read bytes from slave register address
|
||||
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
|
||||
} |
||||
|
||||
|
||||
void SENtralPassThroughMode() |
||||
{ |
||||
// First put SENtral in standby mode
|
||||
uint8_t c = readByte(EM7180_ADDRESS, EM7180_AlgorithmControl); |
||||
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
|
||||
writeByte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
|
||||
if(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 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 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 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(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
|
||||
// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
|
||||
// Wire.requestFrom(address, 1); // Read one byte from slave register address
|
||||
Wire.requestFrom(device_address, (size_t) 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 M24512DFMreadBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest) |
||||
{
|
||||
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(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
|
||||
// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
|
||||
uint8_t i = 0; |
||||
// Wire.requestFrom(address, count); // Read bytes from slave register address
|
||||
Wire.requestFrom(device_address, (size_t) count); // Read bytes from slave register address
|
||||
while (Wire.available()) { |
||||
dest[i++] = Wire.read(); } // Put read results in the Rx buffer
|
||||
} |
||||
|
||||
// simple function to scan for I2C devices on the bus
|
||||
void 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.endTransmisstion 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("Unknow 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"); |
||||
} |
||||
@ -1,20 +1,2 @@
@@ -1,20 +1,2 @@
|
||||
EM7180_SENtral_sensor_hub |
||||
========================= |
||||
|
||||
 |
||||
|
||||
*EM7180 sensor hub with Invensense's MPU9250 gyro/accelerometer with embedded Asahi Kasei AK8963C magnetometer, Measurement Specialties' MS5637 Barometer, and ST's M24512DFC I2C EEPROM on a board directly mountable onto the Teensy Cortex ARM M4 microcontroller.* |
||||
|
||||
There are two kinds of files in this repository. |
||||
|
||||
The FirmwareUpload.ino file is a sketch that takes the firmware file xxx.fw (~22 kbyte) generated by the EM7180 [SENtral Tool Kit Configuration](http://www.emdeveloper.com/?page_id=105) tool and writes it to an ST Microelectronics M24512DFM/C EEPROM from an SD card. Both the SD card and the [SENtral breakout board](https://www.tindie.com/products/onehorse/em7180-sentral-sensor-hub-with-bmx055-motion-sensor/) need to be connected to a microcontroller; I use the Teensy 3.1. The SENtral breakout board is connected to the Teensy 3.1 I2C port on pins 16 and 17 and the SD card reader is connected to the SPI port on pins 10-13. Once the firmware is loaded onto the EEPROM it doesn't have to be done again unless the firmware changes or is updated; the SENtral reads the firmware upon power on and gets the information it needs about the particular sensors on the board. |
||||
|
||||
The other files are sketches that further configure the SENtral for either normal mode, where it manages the BMX055 or LSM9DS0 or MPU6500+AK8963C sensors as slaves providing scaled sensor output and quaternions,or pass-through mode, where the Teensy microcontroller can directly communicate with the BMX055 or LSM9DS0 or MPU6500+AK8963C motion sensors and the MS5637/BMP280 pressure sensor. |
||||
|
||||
These are the three major motion sensor inputs I am planning to implement in the short term. These will allow me to test the dependence of the quality of the motion sensor input data on the resulting sensor fusion solution using the same fusion algorithms and fusion engine. |
||||
|
||||
The SENtral is configurable and the firmware, including the sensor fusion algorithms, can be programmed by the (sophisticated) user. It's just not easy. |
||||
|
||||
Later, I will add altimetry to the sensor fusion algorithm as well as some other refinements. This is a great platform for testing sensor fusion algorithms and new motion sensors. |
||||
|
||||
This project is a work in progress... |
||||
STM32 Driver for the newest Ultimate Sensor Fusion Solution using the latest ST motion sensors: combination accel/gyro LSM6DSM, magnetometer LIS2MDL, and barometer LPS22HB. Now sold on [Tindie](https://www.tindie.com/products/onehorse/ultimate-sensor-fusion-solution-lsm6dsm--lis2md/). |
||||
 |
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,524 +0,0 @@
@@ -1,524 +0,0 @@
|
||||
#ifndef Globals_h |
||||
#define Globals_h |
||||
|
||||
/*************************************************************************************************/ |
||||
/************* ***************/ |
||||
/************* Parameter Definitions ***************/ |
||||
/************* ***************/ |
||||
/*************************************************************************************************/ |
||||
|
||||
// These are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
|
||||
#define Kp 2.0f * 5.0f |
||||
#define Ki 0.0f |
||||
|
||||
// BMP280 registers
|
||||
#define BMP280_TEMP_XLSB 0xFC |
||||
#define BMP280_TEMP_LSB 0xFB |
||||
#define BMP280_TEMP_MSB 0xFA |
||||
#define BMP280_PRESS_XLSB 0xF9 |
||||
#define BMP280_PRESS_LSB 0xF8 |
||||
#define BMP280_PRESS_MSB 0xF7 |
||||
#define BMP280_CONFIG 0xF5 |
||||
#define BMP280_CTRL_MEAS 0xF4 |
||||
#define BMP280_STATUS 0xF3 |
||||
#define BMP280_RESET 0xE0 |
||||
#define BMP280_ID 0xD0 // should be 0x58
|
||||
#define BMP280_CALIB00 0x88 |
||||
|
||||
// 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_GP36 0x5B |
||||
#define EM7180_GP37 0x5C |
||||
#define EM7180_GP38 0x5D |
||||
#define EM7180_GP39 0x5E |
||||
#define EM7180_GP40 0x5F |
||||
#define EM7180_GP50 0x69 |
||||
#define EM7180_GP51 0x6A |
||||
#define EM7180_GP52 0x6B |
||||
#define EM7180_GP53 0x6C |
||||
#define EM7180_GP54 0x6D |
||||
#define EM7180_GP55 0x6E |
||||
#define EM7180_GP56 0x6F |
||||
|
||||
#define EM7180_ADDRESS 0x28 // Address of the EM7180 SENtral sensor hub
|
||||
#define M24512DFM_DATA_ADDRESS 0x50 // Address of the 500 page M24512DRC EEPROM data buffer, 1024 bits (128 8-bit bytes) per page
|
||||
#define M24512DFM_IDPAGE_ADDRESS 0x58 // Address of the single M24512DRC lockable EEPROM ID page
|
||||
#define MPU9250_ADDRESS 0x68 // Device address of MPU9250 when ADO = 0
|
||||
#define AK8963_ADDRESS 0x0C // Address of magnetometer
|
||||
#define BMP280_ADDRESS 0x76 // Address of BMP280 altimeter when ADO = 0
|
||||
|
||||
/*************************************************************************************************/ |
||||
/************* ***************/ |
||||
/************* Enumerators and Structure Variables ***************/ |
||||
/************* ***************/ |
||||
/*************************************************************************************************/ |
||||
|
||||
// Set initial input parameters
|
||||
enum Ascale { |
||||
AFS_2G = 0, |
||||
AFS_4G, |
||||
AFS_8G, |
||||
AFS_16G |
||||
}; |
||||
|
||||
enum Gscale { |
||||
GFS_250DPS = 0, |
||||
GFS_500DPS, |
||||
GFS_1000DPS, |
||||
GFS_2000DPS |
||||
}; |
||||
|
||||
enum Mscale { |
||||
MFS_14BITS = 0, // 0.6 mG per LSB
|
||||
MFS_16BITS // 0.15 mG per LSB
|
||||
}; |
||||
|
||||
enum Posr { |
||||
P_OSR_00 = 0, // no op
|
||||
P_OSR_01, |
||||
P_OSR_02, |
||||
P_OSR_04, |
||||
P_OSR_08, |
||||
P_OSR_16 |
||||
}; |
||||
|
||||
enum Tosr { |
||||
T_OSR_00 = 0, // no op
|
||||
T_OSR_01, |
||||
T_OSR_02, |
||||
T_OSR_04, |
||||
T_OSR_08, |
||||
T_OSR_16 |
||||
}; |
||||
|
||||
enum IIRFilter { |
||||
full = 0, // bandwidth at full sample rate
|
||||
BW0_223ODR, |
||||
BW0_092ODR, |
||||
BW0_042ODR, |
||||
BW0_021ODR // bandwidth at 0.021 x sample rate
|
||||
}; |
||||
|
||||
enum Mode { |
||||
BMP280Sleep = 0, |
||||
forced, |
||||
forced2, |
||||
normal |
||||
}; |
||||
|
||||
enum SBy { |
||||
t_00_5ms = 0, |
||||
t_62_5ms, |
||||
t_125ms, |
||||
t_250ms, |
||||
t_500ms, |
||||
t_1000ms, |
||||
t_2000ms, |
||||
t_4000ms, |
||||
}; |
||||
|
||||
struct global_conf_t |
||||
{ |
||||
uint8_t currentSet; |
||||
int16_t accZero_max[3]; |
||||
int16_t accZero_min[3]; |
||||
int16_t magZero[3]; |
||||
int16_t grav; |
||||
uint8_t checksum; // Last position in structure
|
||||
}; |
||||
|
||||
struct Sentral_WS_params |
||||
{ |
||||
uint8_t Sen_param[35][4]; |
||||
}; |
||||
|
||||
/*************************************************************************************************/ |
||||
/************* ***************/ |
||||
/************* Global Scope Variables ***************/ |
||||
/************* ***************/ |
||||
/*************************************************************************************************/ |
||||
|
||||
// General purpose variables
|
||||
int16_t serial_input; |
||||
static int16_t warm_start = 0; |
||||
static int16_t warm_start_saved = 0; |
||||
|
||||
// Specify BMP280 configuration
|
||||
uint8_t Posr = P_OSR_16; |
||||
uint8_t Tosr = T_OSR_02; |
||||
uint8_t Mode = normal; |
||||
uint8_t IIRFilter = BW0_042ODR; |
||||
uint8_t SBy = t_62_5ms; |
||||
|
||||
// t_fine carries fine temperature as global value for BMP280
|
||||
int32_t t_fine; |
||||
|
||||
// Specify sensor full scale
|
||||
uint8_t Gscale = GFS_250DPS; |
||||
uint8_t Ascale = AFS_2G; |
||||
|
||||
// Choose either 14-bit or 16-bit magnetometer resolution
|
||||
uint8_t Mscale = MFS_16BITS; |
||||
|
||||
// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
|
||||
uint8_t Mmode = 0x02; |
||||
|
||||
// scale resolutions per LSB for the sensors
|
||||
float aRes; |
||||
float gRes; |
||||
float mRes; |
||||
|
||||
// Pin definitions
|
||||
// These can be changed, 2 and 3 are the Arduinos ext int pins
|
||||
int intPin = 8; |
||||
|
||||
// LED on the Teensy 3.1
|
||||
int myLed = 13; |
||||
|
||||
// BMP280 compensation parameters
|
||||
uint16_t dig_T1; |
||||
uint16_t dig_P1; |
||||
int16_t dig_T2; |
||||
int16_t dig_T3; |
||||
int16_t dig_P2; |
||||
int16_t dig_P3; |
||||
int16_t dig_P4; |
||||
int16_t dig_P5; |
||||
int16_t dig_P6; |
||||
int16_t dig_P7; |
||||
int16_t dig_P8; |
||||
int16_t dig_P9; |
||||
|
||||
// stores BMP280 pressures sensor pressure and temperature
|
||||
double Temperature; |
||||
double Pressure; |
||||
|
||||
// pressure and temperature raw count output for BMP280
|
||||
int32_t rawPress; |
||||
int32_t rawTemp; |
||||
|
||||
// MPU9250 variables
|
||||
// Stores the 16-bit signed accelerometer sensor output
|
||||
int16_t accelCount[3]; |
||||
|
||||
// Stores the 16-bit signed gyro sensor output
|
||||
int16_t gyroCount[3]; |
||||
|
||||
// Stores the 16-bit signed magnetometer sensor output
|
||||
int16_t magCount[3]; |
||||
|
||||
// quaternion data register
|
||||
float Quat[4] = {0, 0, 0, 0}; |
||||
|
||||
// Factory mag calibration and mag bias
|
||||
float magCalibration[3] = {0, 0, 0}; |
||||
|
||||
// Bias corrections for gyro, accelerometer, mag
|
||||
float gyroBias[3] = {0, 0, 0}; |
||||
float accelBias[3] = {0, 0, 0}; |
||||
float magBias[3] = {0, 0, 0}; |
||||
float magScale[3] = {0, 0, 0}; |
||||
|
||||
// Pressure, temperature raw count output
|
||||
int16_t tempCount; |
||||
int16_t rawPressure; |
||||
int16_t rawTemperature; |
||||
|
||||
// Stores the MPU9250 internal chip temperature in degrees Celsius
|
||||
float temperature; |
||||
float pressure; |
||||
float altitude; |
||||
|
||||
// holds results of gyro and accelerometer self test
|
||||
float SelfTest[6]; |
||||
|
||||
// Global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
|
||||
// Gyroscope measurement error in rads/s (start at 40 deg/s)
|
||||
float GyroMeasError = PI * (40.0f / 180.0f); |
||||
|
||||
// Gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
|
||||
float GyroMeasDrift = PI * (0.0f / 180.0f); |
||||
|
||||
// There is a tradeoff in the beta parameter between accuracy and response speed.
|
||||
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
|
||||
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
|
||||
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
|
||||
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
|
||||
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
|
||||
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
|
||||
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
|
||||
|
||||
// Compute beta
|
||||
float beta = sqrt(3.0f / 4.0f) * GyroMeasError; |
||||
|
||||
// Compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
|
||||
float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; |
||||
|
||||
// Used to control display output rate
|
||||
uint32_t delt_t = 0; |
||||
uint32_t count = 0; |
||||
uint32_t sumCount = 0; |
||||
float pitch; |
||||
float yaw; |
||||
float roll; |
||||
float Yaw; |
||||
float Pitch; |
||||
float Roll; |
||||
|
||||
// Integration interval for both filter schemes
|
||||
float deltat = 0.0f; |
||||
float sum = 0.0f; |
||||
|
||||
// used to calculate integration interval
|
||||
uint32_t lastUpdate = 0; |
||||
uint32_t firstUpdate = 0; |
||||
|
||||
// used to calculate integration interval
|
||||
uint32_t Now = 0; |
||||
|
||||
// used for param transfer
|
||||
uint8_t param[4]; |
||||
|
||||
// EM7180 sensor full scale ranges
|
||||
uint16_t EM7180_mag_fs; |
||||
uint16_t EM7180_acc_fs; |
||||
uint16_t EM7180_gyro_fs; |
||||
|
||||
// variables to hold latest sensor data values
|
||||
float ax; |
||||
float ay; |
||||
float az; |
||||
float gx; |
||||
float gy; |
||||
float gz; |
||||
float mx; |
||||
float my; |
||||
float mz; |
||||
|
||||
// Vector to hold quaternion
|
||||
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; |
||||
|
||||
// Vector to hold integral error for Mahony method
|
||||
float eInt[3] = {0.0f, 0.0f, 0.0f}; |
||||
|
||||
global_conf_t global_conf; |
||||
Sentral_WS_params WS_params; |
||||
|
||||
#endif // Globals_h
|
||||
@ -1 +0,0 @@
@@ -1 +0,0 @@
|
||||
These are the most recent host-side programs for the EM7180 reference board AKA Ultimate Sensor Fusion Solution available at [Tindie](https://www.tindie.com/products/onehorse/ultimate-sensor-fusion-solution/) and updated with the warm-start function which stores the dynamic calibration parameters on the on-board EEPROM so the device starts well-calibrated (given similar environmental conditions) upon subsequent power on. The code is designed to be used with the Teensy 3.Xmicrcontroller but with few changes any I2C-compatible micrcontroller will do. |
||||
@ -1,195 +0,0 @@
@@ -1,195 +0,0 @@
|
||||
#include "Arduino.h" |
||||
|
||||
// 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 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 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 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; |
||||
|
||||
} |
||||
File diff suppressed because it is too large
Load Diff
@ -1,523 +0,0 @@
@@ -1,523 +0,0 @@
|
||||
#ifndef Globals_h |
||||
#define Globals_h |
||||
|
||||
/*************************************************************************************************/ |
||||
/************* ***************/ |
||||
/************* Parameter Definitions ***************/ |
||||
/************* ***************/ |
||||
/*************************************************************************************************/ |
||||
|
||||
// These are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
|
||||
#define Kp 2.0f * 5.0f |
||||
#define Ki 0.0f |
||||
|
||||
// BMP280 registers
|
||||
#define BMP280_TEMP_XLSB 0xFC |
||||
#define BMP280_TEMP_LSB 0xFB |
||||
#define BMP280_TEMP_MSB 0xFA |
||||
#define BMP280_PRESS_XLSB 0xF9 |
||||
#define BMP280_PRESS_LSB 0xF8 |
||||
#define BMP280_PRESS_MSB 0xF7 |
||||
#define BMP280_CONFIG 0xF5 |
||||
#define BMP280_CTRL_MEAS 0xF4 |
||||
#define BMP280_STATUS 0xF3 |
||||
#define BMP280_RESET 0xE0 |
||||
#define BMP280_ID 0xD0 // should be 0x58
|
||||
#define BMP280_CALIB00 0x88 |
||||
|
||||
// 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_GP36 0x5B |
||||
#define EM7180_GP37 0x5C |
||||
#define EM7180_GP38 0x5D |
||||
#define EM7180_GP39 0x5E |
||||
#define EM7180_GP40 0x5F |
||||
#define EM7180_GP50 0x69 |
||||
#define EM7180_GP51 0x6A |
||||
#define EM7180_GP52 0x6B |
||||
#define EM7180_GP53 0x6C |
||||
#define EM7180_GP54 0x6D |
||||
#define EM7180_GP55 0x6E |
||||
#define EM7180_GP56 0x6F |
||||
|
||||
#define EM7180_ADDRESS 0x28 // Address of the EM7180 SENtral sensor hub
|
||||
#define M24512DFM_DATA_ADDRESS 0x50 // Address of the 500 page M24512DRC EEPROM data buffer, 1024 bits (128 8-bit bytes) per page
|
||||
#define M24512DFM_IDPAGE_ADDRESS 0x58 // Address of the single M24512DRC lockable EEPROM ID page
|
||||
#define MPU9250_ADDRESS 0x68 // Device address of MPU9250 when ADO = 0
|
||||
#define AK8963_ADDRESS 0x0C // Address of magnetometer
|
||||
#define BMP280_ADDRESS 0x76 // Address of BMP280 altimeter when ADO = 0
|
||||
|
||||
/*************************************************************************************************/ |
||||
/************* ***************/ |
||||
/************* Enumerators and Structure Variables ***************/ |
||||
/************* ***************/ |
||||
/*************************************************************************************************/ |
||||
|
||||
// Set initial input parameters
|
||||
enum Ascale { |
||||
AFS_2G = 0, |
||||
AFS_4G, |
||||
AFS_8G, |
||||
AFS_16G |
||||
}; |
||||
|
||||
enum Gscale { |
||||
GFS_250DPS = 0, |
||||
GFS_500DPS, |
||||
GFS_1000DPS, |
||||
GFS_2000DPS |
||||
}; |
||||
|
||||
enum Mscale { |
||||
MFS_14BITS = 0, // 0.6 mG per LSB
|
||||
MFS_16BITS // 0.15 mG per LSB
|
||||
}; |
||||
|
||||
enum Posr { |
||||
P_OSR_00 = 0, // no op
|
||||
P_OSR_01, |
||||
P_OSR_02, |
||||
P_OSR_04, |
||||
P_OSR_08, |
||||
P_OSR_16 |
||||
}; |
||||
|
||||
enum Tosr { |
||||
T_OSR_00 = 0, // no op
|
||||
T_OSR_01, |
||||
T_OSR_02, |
||||
T_OSR_04, |
||||
T_OSR_08, |
||||
T_OSR_16 |
||||
}; |
||||
|
||||
enum IIRFilter { |
||||
full = 0, // bandwidth at full sample rate
|
||||
BW0_223ODR, |
||||
BW0_092ODR, |
||||
BW0_042ODR, |
||||
BW0_021ODR // bandwidth at 0.021 x sample rate
|
||||
}; |
||||
|
||||
enum Mode { |
||||
BMP280Sleep = 0, |
||||
forced, |
||||
forced2, |
||||
normal |
||||
}; |
||||
|
||||
enum SBy { |
||||
t_00_5ms = 0, |
||||
t_62_5ms, |
||||
t_125ms, |
||||
t_250ms, |
||||
t_500ms, |
||||
t_1000ms, |
||||
t_2000ms, |
||||
t_4000ms, |
||||
}; |
||||
|
||||
struct acc_cal |
||||
{ |
||||
int16_t accZero_max[3]; |
||||
int16_t accZero_min[3]; |
||||
}; |
||||
|
||||
struct Sentral_WS_params |
||||
{ |
||||
uint8_t Sen_param[35][4]; |
||||
}; |
||||
|
||||
/*************************************************************************************************/ |
||||
/************* ***************/ |
||||
/************* Global Scope Variables ***************/ |
||||
/************* ***************/ |
||||
/*************************************************************************************************/ |
||||
|
||||
// General purpose variables
|
||||
int16_t serial_input; |
||||
static int16_t warm_start = 0; |
||||
static int16_t accel_cal = 0; |
||||
static int16_t warm_start_saved = 0; |
||||
static int16_t accel_cal_saved = 0; |
||||
static uint16_t calibratingA = 0; |
||||
|
||||
// Specify BMP280 configuration
|
||||
uint8_t Posr = P_OSR_16; |
||||
uint8_t Tosr = T_OSR_02; |
||||
uint8_t Mode = normal; |
||||
uint8_t IIRFilter = BW0_042ODR; |
||||
uint8_t SBy = t_62_5ms; |
||||
|
||||
// t_fine carries fine temperature as global value for BMP280
|
||||
int32_t t_fine; |
||||
|
||||
// Specify sensor full scale
|
||||
uint8_t Gscale = GFS_250DPS; |
||||
uint8_t Ascale = AFS_2G; |
||||
|
||||
// Choose either 14-bit or 16-bit magnetometer resolution
|
||||
uint8_t Mscale = MFS_16BITS; |
||||
|
||||
// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
|
||||
uint8_t Mmode = 0x02; |
||||
|
||||
// scale resolutions per LSB for the sensors
|
||||
float aRes; |
||||
float gRes; |
||||
float mRes; |
||||
|
||||
// Pin definitions
|
||||
// These can be changed, 2 and 3 are the Arduinos ext int pins
|
||||
int intPin = 8; |
||||
|
||||
// LED on the Teensy 3.1
|
||||
int myLed = 13; |
||||
|
||||
// BMP280 compensation parameters
|
||||
uint16_t dig_T1; |
||||
uint16_t dig_P1; |
||||
int16_t dig_T2; |
||||
int16_t dig_T3; |
||||
int16_t dig_P2; |
||||
int16_t dig_P3; |
||||
int16_t dig_P4; |
||||
int16_t dig_P5; |
||||
int16_t dig_P6; |
||||
int16_t dig_P7; |
||||
int16_t dig_P8; |
||||
int16_t dig_P9; |
||||
|
||||
// stores BMP280 pressures sensor pressure and temperature
|
||||
double Temperature; |
||||
double Pressure; |
||||
|
||||
// pressure and temperature raw count output for BMP280
|
||||
int32_t rawPress; |
||||
int32_t rawTemp; |
||||
|
||||
// MPU9250 variables
|
||||
// Stores the 16-bit signed accelerometer sensor output
|
||||
int16_t accelCount[3]; |
||||
|
||||
// Stores the 16-bit signed gyro sensor output
|
||||
int16_t gyroCount[3]; |
||||
|
||||
// Stores the 16-bit signed magnetometer sensor output
|
||||
int16_t magCount[3]; |
||||
|
||||
// quaternion data register
|
||||
float Quat[4] = {0, 0, 0, 0}; |
||||
|
||||
// Factory mag calibration and mag bias
|
||||
float magCalibration[3] = {0, 0, 0}; |
||||
|
||||
// Bias corrections for gyro, accelerometer, mag
|
||||
float gyroBias[3] = {0, 0, 0}; |
||||
float accelBias[3] = {0, 0, 0}; |
||||
float magBias[3] = {0, 0, 0}; |
||||
float magScale[3] = {0, 0, 0}; |
||||
|
||||
// Pressure, temperature raw count output
|
||||
int16_t tempCount; |
||||
int16_t rawPressure; |
||||
int16_t rawTemperature; |
||||
|
||||
// Stores the MPU9250 internal chip temperature in degrees Celsius
|
||||
float temperature; |
||||
float pressure; |
||||
float altitude; |
||||
|
||||
// holds results of gyro and accelerometer self test
|
||||
float SelfTest[6]; |
||||
|
||||
// Global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
|
||||
// Gyroscope measurement error in rads/s (start at 40 deg/s)
|
||||
float GyroMeasError = PI * (40.0f / 180.0f); |
||||
|
||||
// Gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
|
||||
float GyroMeasDrift = PI * (0.0f / 180.0f); |
||||
|
||||
// There is a tradeoff in the beta parameter between accuracy and response speed.
|
||||
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
|
||||
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
|
||||
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
|
||||
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
|
||||
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
|
||||
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
|
||||
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
|
||||
|
||||
// Compute beta
|
||||
float beta = sqrt(3.0f / 4.0f) * GyroMeasError; |
||||
|
||||
// Compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
|
||||
float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; |
||||
|
||||
// Used to control display output rate
|
||||
uint32_t delt_t = 0; |
||||
uint32_t count = 0; |
||||
uint32_t sumCount = 0; |
||||
float pitch; |
||||
float yaw; |
||||
float roll; |
||||
float Yaw; |
||||
float Pitch; |
||||
float Roll; |
||||
|
||||
// Integration interval for both filter schemes
|
||||
float deltat = 0.0f; |
||||
float sum = 0.0f; |
||||
|
||||
// used to calculate integration interval
|
||||
uint32_t lastUpdate = 0; |
||||
uint32_t firstUpdate = 0; |
||||
|
||||
// used to calculate integration interval
|
||||
uint32_t Now = 0; |
||||
|
||||
// used for param transfer
|
||||
uint8_t param[4]; |
||||
|
||||
// EM7180 sensor full scale ranges
|
||||
uint16_t EM7180_mag_fs; |
||||
uint16_t EM7180_acc_fs; |
||||
uint16_t EM7180_gyro_fs; |
||||
|
||||
// variables to hold latest sensor data values
|
||||
float ax; |
||||
float ay; |
||||
float az; |
||||
float gx; |
||||
float gy; |
||||
float gz; |
||||
float mx; |
||||
float my; |
||||
float mz; |
||||
|
||||
// Vector to hold quaternion
|
||||
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; |
||||
|
||||
// Vector to hold integral error for Mahony method
|
||||
float eInt[3] = {0.0f, 0.0f, 0.0f}; |
||||
|
||||
acc_cal global_conf; |
||||
Sentral_WS_params WS_params; |
||||
|
||||
#endif // Globals_h
|
||||
@ -1,195 +0,0 @@
@@ -1,195 +0,0 @@
|
||||
#include "Arduino.h" |
||||
|
||||
// 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 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 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 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; |
||||
|
||||
} |
||||
@ -1,194 +0,0 @@
@@ -1,194 +0,0 @@
|
||||
|
||||
// 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 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 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 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; |
||||
|
||||
} |
||||
Loading…
Reference in new issue