/*
* em7180.c
*
* 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.
*/
/* Includes */
#include <stdint.h>
#include <stdbool.h>
#include "em7180.h"
/* Private Global Variables */
static uint8_t _intPin;
static bool _passThru;
static float _aRes;
static float _gRes;
static float _mRes;
static uint8_t _Mmode;
static float _fuseROMx;
static float _fuseROMy;
static float _fuseROMz;
static float _q[4];
static float _beta;
static float _deltat;
static float _Kp;
static float _Ki;
/* Function Prototypes */
static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t data);
static void m24512dfm_write(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t count, uint8_t *dest);
static uint8_t m24512dfm_read_byte(uint8_t device_address,
uint8_t data_address1, uint8_t data_address2);
static void m24512dfm_read(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t count, uint8_t *dest);
static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress);
static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest);
/* Function Definitions */
em7180_new(uint8_t pin, bool passthru)
{
pinMode(pin, INPUT);
_intPin = pin;
_passThru = passthru;
}
void em7180_chip_id_get()
{
// Read SENtral device information
uint16_t ROM1 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ROMVersion1);
uint16_t ROM2 = lsm6dsm_read_byte(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 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion1);
uint16_t RAM2 = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RAMVersion2);
Serial.print("EM7180 RAM Version: 0x");
Serial.print(RAM1);
Serial.println(RAM2);
uint8_t PID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ProductID);
Serial.print("EM7180 ProductID: 0x");
Serial.print(PID, HEX);
Serial.println(" Should be: 0x80");
uint8_t RID = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RevisionID);
Serial.print("EM7180 RevisionID: 0x");
Serial.print(RID, HEX);
Serial.println(" Should be: 0x02");
}
void em7180_load_fw_from_eeprom()
{
// Check which sensors can be detected by the EM7180
uint8_t featureflag = lsm6dsm_read_byte(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 = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
Serial.println("EEPROM detected on the sensor bus!");
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
Serial.println("EEPROM uploaded config file!");
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
Serial.println("EEPROM CRC incorrect!");
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
Serial.println("EM7180 in initialized state!");
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
Serial.println("No EEPROM detected!");
int count = 0;
while(!STAT)
{
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
delay(500);
count++;
STAT = (lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)
Serial.println("EEPROM detected on the sensor bus!");
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)
Serial.println("EEPROM uploaded config file!");
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)
Serial.println("EEPROM CRC incorrect!");
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)
Serial.println("EM7180 in initialized state!");
if(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)
Serial.println("No EEPROM detected!");
if(count > 10)
break;
}
if(!(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04))
Serial.println("EEPROM upload successful!");
}
uint8_t em7180_status()
{
// Check event status register, way to check data ready by polling rather than interrupt
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
return c;
}
uint8_t em7180_errors()
{
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ErrorRegister); // check error register
return c;
}
void em7180_init(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
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
//Setup LPF bandwidth (BEFORE setting ODR's)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW); // accBW = 3 = 41Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz
// Set accel/gyro/mag desired ODR rates
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_QRateDivisor, QRtDiv); // quat rate = gyroRt/(1 QRTDiv)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_MagRate, magRt); // 0x64 = 100 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AccelRate, accRt); // 200/10 Hz, 0x14 = 200 Hz
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_GyroRate, gyroRt); // 200/10 Hz, 0x14 = 200 Hz
lsm6dsm_write_byte(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
lsm6dsm_write_byte(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)
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
// Enable EM7180 run mode
lsm6dsm_write_byte(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
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
byte param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4A))
{
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(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");
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4B))
{
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(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");
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
lsm6dsm_write_byte(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
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4A))
{
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(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");
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read parameter 75
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
while(!(param_xfer == 0x4B))
{
param_xfer = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
param[0] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte0);
param[1] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte1);
param[2] = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_SavedParamByte2);
param[3] = lsm6dsm_read_byte(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");
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm
// Read EM7180 status
uint8_t runStatus = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_RunStatus);
if(runStatus & 0x01)
Serial.println(" EM7180 run status = normal mode");
uint8_t algoStatus = lsm6dsm_read_byte(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 = lsm6dsm_read_byte(EM7180_ADDRESS,
EM7180_PassThruStatus);
if(passthruStatus & 0x01)
Serial.print(" EM7180 in passthru mode!");
uint8_t eventStatus = lsm6dsm_read_byte(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 = lsm6dsm_read_byte(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(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualMagRate));
Serial.println(" Hz");
Serial.print("Actual AccelRate = ");
Serial.print(
10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualAccelRate));
Serial.println(" Hz");
Serial.print("Actual GyroRate = ");
Serial.print(10 * lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualGyroRate));
Serial.println(" Hz");
Serial.print("Actual BaroRate = ");
Serial.print(lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ActualBaroRate));
Serial.println(" Hz");
}
float em7180_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 em7180_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 em7180_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 em7180_gyro_set_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;
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Gyro LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Gyro MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Unused
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Unused
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); //Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == 0xCB))
{
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void em7180_mag_acc_set_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);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Mag LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Mag MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Acc LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Acc MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); //Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == 0xCA))
{
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void 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
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == param))
{
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void em7180_param_set_float(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
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, param);
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
while(!(STAT == param))
{
STAT = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
}
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}
void em7180_quatdata_get(float *destination)
{
uint8_t rawData[16]; // x/y/z quaternion register data stored here
em7180_read(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 em7180_acceldata_get(int16_t *destination)
{
uint8_t rawData[6]; // x/y/z accel register data stored here
em7180_read(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 em7180_gyrodata_get(int16_t *destination)
{
uint8_t rawData[6]; // x/y/z gyro register data stored here
em7180_read(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 em7180_magdata_get(int16_t *destination)
{
uint8_t rawData[6]; // x/y/z gyro register data stored here
em7180_read(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]);
}
float em7180_mres_get(uint8_t Mscale)
{
switch(Mscale)
{
// Possible magnetometer scales (and their register bit settings) are:
// 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS:
_mRes = 10. * 4912. / 8190.; // Proper scale to return milliGauss
return _mRes;
break;
case MFS_16BITS:
_mRes = 10. * 4912. / 32760.0; // Proper scale to return milliGauss
return _mRes;
break;
}
}
float em7180_gres_get(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).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case GFS_250DPS:
_gRes = 250.0 / 32768.0;
return _gRes;
break;
case GFS_500DPS:
_gRes = 500.0 / 32768.0;
return _gRes;
break;
case GFS_1000DPS:
_gRes = 1000.0 / 32768.0;
return _gRes;
break;
case GFS_2000DPS:
_gRes = 2000.0 / 32768.0;
return _gRes;
break;
}
}
float em7180_ares_get(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 algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G:
_aRes = 2.0 / 32768.0;
return _aRes;
break;
case AFS_4G:
_aRes = 4.0 / 32768.0;
return _aRes;
break;
case AFS_8G:
_aRes = 8.0 / 32768.0;
return _aRes;
break;
case AFS_16G:
_aRes = 16.0 / 32768.0;
return _aRes;
break;
}
}
int16_t em7180_baro_get()
{
uint8_t rawData[2]; // x/y/z gyro register data stored here
em7180_read(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 em7180_temp_get()
{
uint8_t rawData[2]; // x/y/z gyro register data stored here
em7180_read(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 em7180_passthrough()
{
// First put SENtral in standby mode
uint8_t c = lsm6dsm_read_byte(EM7180_ADDRESS, EM7180_AlgorithmControl);
lsm6dsm_write_byte(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
lsm6dsm_write_byte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01);
if(lsm6dsm_read_byte(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
static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
uint8_t data_address2, uint8_t data)
{
uint8_t temp[2] = { data_address1, data_address2 };
Wire.transfer(device_address, &temp[0], 2, NULL, 0);
Wire.transfer(device_address, &data, 1, NULL, 0);
}
static void m24512dfm_write(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!");
}
uint8_t temp[2] = { data_address1, data_address2 };
Wire.transfer(device_address, &temp[0], 2, NULL, 0);
Wire.transfer(device_address, &dest[0], count, NULL, 0);
}
static uint8_t m24512dfm_read_byte(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
}
static void m24512dfm_read(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.transfer(device_address, &temp[0], 2, dest, count);
}
// I2C read/write functions for the EM7180
void em7180_write_byte(uint8_t address, uint8_t subAddress, uint8_t data)
{
uint8_t temp[2];
temp[0] = subAddress;
temp[1] = data;
Wire.transfer(address, &temp[0], 2, NULL, 0);
}
static uint8_t em7180_read_byte(uint8_t address, uint8_t subAddress)
{
uint8_t temp[1];
Wire.transfer(address, &subAddress, 1, &temp[0], 1);
return temp[0];
}
static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t *dest)
{
Wire.transfer(address, &subAddress, 1, dest, count);
}
// 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 em7180_update_quat_madgwick(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;
// Normalize 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;
// Normalize 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); // normalize 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); // normalize 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 em7180_update_quat_mahony(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;
// Normalize 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;
// Normalize 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);
// Normalize 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;
}