Hi,
For my new project using ESP32 I am looking for a accelerometer which did not got to the end of life (EOL) or nor recommended (NRND) for new design status. I like the MPU-9250 because it has nine-axis (gyro + accelerometer + compass) and would like to get something similar, however any sensor I am running into has either the status EOL or NRND.
Did anyone found such sensor yet? Or should I stick with the above sensor for now?
Thanks,
Marc
The most recent official Arduino board that has an IMU is the Nano 33 BLE, which uses the LSM9DS1. You might take a look at that to see whether it meets your needs (including an ESP32-compatible library if you don't feel like writting the low level code for it).
Arduino's library for it is here:
(there are some 3rd party alternatives too).
In case anyone wants to talk to the LSMDS9 using a ESP32
ESP_32_SPI_API.h
#include <driver/spi_master.h>
#include "sdkconfig.h"
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
////////////////////////////////////
//
//#define MAGTYPE true
//#define XGTYPE false
//////////////////////////
///////////////////////////
//
////////////////////////////
uint8_t GetLowBits();
int8_t GetHighBits();
int fReadSPIdata16bits( spi_device_handle_t &h, int address );
int fWriteSPIdata8bits( spi_device_handle_t &h, int address, int sendData );
int fInitializeSPI_Devices( spi_device_handle_t &h, int csPin);
// spi_device_handle_t fInitializeSPI_Devices( int csPin);
int fInitializeSPI_Channel( int spiCLK, int spiMOSI, int spiMISO, spi_host_device_t SPI_Host, bool EnableDMA);
ESP_32_SPI_API.cpp
#include "ESP32_SPI_API.h"
/////////////////////////////
///////////////////////////
uint8_t txData[2] = { };
uint8_t rxData[25] = { };
uint8_t low;
int8_t high;
//////
//////////////////////////////////
uint8_t GetLowBits()
{
return low;
}
int8_t GetHighBits()
{
return high;
}
////////////////////////////////////////
int fInitializeSPI_Channel( int spiCLK, int spiMOSI, int spiMISO, spi_host_device_t SPI_Host, bool EnableDMA)
{
esp_err_t intError;
spi_bus_config_t bus_config = { };
bus_config.sclk_io_num = spiCLK; // CLK
bus_config.mosi_io_num = spiMOSI; // MOSI
bus_config.miso_io_num = spiMISO; // MISO
bus_config.quadwp_io_num = -1; // Not used
bus_config.quadhd_io_num = -1; // Not used
intError = spi_bus_initialize( HSPI_HOST, &bus_config, EnableDMA) ;
return intError;
}
//////
int fInitializeSPI_Devices( spi_device_handle_t &h, int csPin)
{
esp_err_t intError;
spi_device_interface_config_t dev_config = { }; // initializes all field to 0
dev_config.address_bits = 0;
dev_config.command_bits = 0;
dev_config.dummy_bits = 0;
dev_config.mode = 3 ;
dev_config.duty_cycle_pos = 0;
dev_config.cs_ena_posttrans = 0;
dev_config.cs_ena_pretrans = 0;
dev_config.clock_speed_hz = 5000000;
dev_config.spics_io_num = csPin;
dev_config.flags = 0;
dev_config.queue_size = 1;
dev_config.pre_cb = NULL;
dev_config.post_cb = NULL;
spi_bus_add_device(HSPI_HOST, &dev_config, &h);
// return intError;
// return h;
} // void fInitializeSPI_Devices()
///////////////////////////////////////////////////////////////
int fReadSPIdata16bits( spi_device_handle_t &h, int _address )
{
uint8_t address = _address;
esp_err_t intError = 0;
low=0; high=0;
spi_transaction_t trans_desc;
trans_desc = { };
trans_desc.addr = 0;
trans_desc.cmd = 0;
trans_desc.flags = 0;
trans_desc.length = (8 * 3); // total data bits
trans_desc.tx_buffer = txData;
trans_desc.rxlength = 8 * 2 ; // Number of bits NOT number of bytes
trans_desc.rx_buffer = rxData;
txData[0] = address | 0x80;
intError = spi_device_transmit( h, &trans_desc);
low = rxData[0]; high = rxData[1];
// if ( intError != 0 )
// {
// Serial.print( " WHO I am LSM9DS1. Transmitting error = ");
// Serial.println ( esp_err_to_name(intError) );
// }
return intError;
} // void fSendSPI( uint8_t count, uint8_t address, uint8_t DataToSend)
////
int fWriteSPIdata8bits( spi_device_handle_t &h, int _address, int _sendData )
{
uint8_t address = _address;
uint8_t sendData = _sendData;
esp_err_t intError;
spi_transaction_t trans_desc;
trans_desc = { };
trans_desc.addr = 0;
trans_desc.cmd = 0;
trans_desc.flags = 0;
trans_desc.length = (8 * 2); // total data bits
trans_desc.tx_buffer = txData;
trans_desc.rxlength = 0 ; // Number of bits NOT number of bytes
trans_desc.rx_buffer = NULL;
txData[0] = address & 0x7F;
txData[1] = sendData;
intError = spi_device_transmit( h, &trans_desc);
return intError;
// // if ( intError != 0 )
// // {
// // Serial.print( " LSM9DS1_REGISTER_CTRL_REG6_XL. Transmitting error = ");
// // Serial.println ( esp_err_to_name(intError) );
// // }
} // void fWriteSPIdata8bits( spi_device_handle_t &h, uint8_t address, uint8_t sendData )
ESP32_LSM9DS1.h
#include "ESP32_SPI_API.h"
#include <driver/spi_master.h>
#include "sdkconfig.h"
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
#include "freertos/FreeRTOS.h"
#include "freertos/timers.h"
// Temperature: LSB per degree celsius
#define LSM9DS1_TEMP_LSB_DEGREE_CELSIUS (8) // 1°C = 8, 25° = 200, etc.
#define FIFO_OFF 0
#define FIFO_THS 1
#define FIFO_CONT_TRIGGER 3
#define FIFO_OFF_TRIGGER 4
#define FIFO_CONT 5
#define AccelerometerDataReady 0x01
#define GyroDataReady 0x02
////////////////////////////
const uint8_t LSM9DS1_REGISTER_WHO_AM_I_XG = 0xF;
const uint8_t LSM9DS1_ID = 0B01101000;
////LSM9DS1_REGISTER_CTRL_REG4 = 0x1E,
const uint8_t LSM9DS1_FIFO_CTRL_REG = 0x2E;
const uint8_t LSM9DS1_REGISTER_CTRL_REG5_XL = 0x1F;
const uint8_t LSM9DS1_REGISTER_CTRL_REG6_XL = 0x20;
const uint8_t LSM9DS1_REGISTER_CTRL_REG7_XL = 0x21;
const uint8_t LSM9DS1_REGISTER_CTRL_REG8 = 0x22;
const uint8_t LSM9DS1_REGISTER_CTRL_REG9 = 0x23;
const uint8_t LSM9DS1_REGISTER_FIFO_SRC = 0x2F;
//// LSM9DS1_REGISTER_CTRL_REG10 = 0x24,
const uint8_t LSM9DS1_REGISTER_CTRL_REG1_G = 0x10;
//// LSM9DS1_REGISTER_CTRL_REG2_G = 0x11,
const uint8_t LSM9DS1_REGISTER_CTRL_REG3_G = 0x12;
//////// Linear Acceleration: mg per LSB
const int8_t LSM9DS1_ACCELRANGE_16G = (0b01 << 3);
const int8_t LSM9DS1_ACCELRANGE_8G = (0b11 << 3);
const int8_t LSM9DS1_ACCELRANGE_4G = (0b10 << 3);
const uint8_t LSM9DS1_ACCELRANGE_2G = (0b00 << 3);
//////// Magnetic Field Strength: gauss range
const uint8_t LSM9DS1_MAGGAIN_4GAUSS = (0b00 << 5); // +/- 4 gauss
const uint8_t LSM9DS1_MAGGAIN_8GAUSS = (0b01 << 5); // +/- 8 gauss
const uint8_t LSM9DS1_MAGGAIN_12GAUSS = (0b10 << 5); // +/- 12 gauss
const uint8_t LSM9DS1_MAGGAIN_16GAUSS = (0b11 << 5); // +/- 16 gauss
////// Linear Acceleration: mg per LSB
const float LSM9DS1_ACCEL_MG_LSB_2G = ( 2.0f / 32767.0f );
const float LSM9DS1_ACCEL_MG_LSB_4G = ( 4.0f / 32767.0f );
const float LSM9DS1_ACCEL_MG_LSB_8G = ( 8.0f / 32767.0f );
const float LSM9DS1_ACCEL_MG_LSB_16G = ( 16.0f / 32767.0f );
//////// Angular Rate: dps per LSB
const float LSM9DS1_GYRO_DPS_DIGIT_245DPS = ( 245.0f / 32767.0f );
const float LSM9DS1_GYRO_DPS_DIGIT_500DPS = ( 500.0f / 32767.0f );
const float LSM9DS1_GYRO_DPS_DIGIT_2000DPS = ( 2000.0f / 32767.0f );
////// GyroScale
const uint8_t LSM9DS1_GYROSCALE_245DPS = (0b00 << 3); // +/- 245 degrees per second rotation
const uint8_t LSM9DS1_GYROSCALE_500DPS = (0b01 << 3); // +/- 500 degrees per second rotation
const uint8_t LSM9DS1_GYROSCALE_2000DPS = (0b11 << 3); // +/- 2000 degrees per second rotation
// Magnetic Field Strength: gauss range
const float LSM9DS1_MAG_MGAUSS_4GAUSS = ( 4.0f / 32767.0f );
const float LSM9DS1_MAG_MGAUSS_8GAUSS = ( 8.0f / 32767.0f );
const float LSM9DS1_MAG_MGAUSS_12GAUSS = ( 12.0f / 32767.0f );
const float LSM9DS1_MAG_MGAUSS_16GAUSS = ( 16.0f / 32767.0f );
////// accel out
const uint8_t LSM9DS1_REGISTER_OUT_X_L_XL = 0x28;
const uint8_t LSM9DS1_REGISTER_OUT_X_H_XL = 0x29;
const uint8_t LSM9DS1_REGISTER_OUT_Y_L_XL = 0x2A;
const uint8_t LSM9DS1_REGISTER_OUT_Y_H_XL = 0x2B;
const uint8_t LSM9DS1_REGISTER_OUT_Z_L_XL = 0x2C;
const uint8_t LSM9DS1_REGISTER_OUT_Z_H_XL = 0x2D;
////// gyro out
const uint8_t LSM9DS1_REGISTER_OUT_X_L_G = 0x18;
const uint8_t LSM9DS1_REGISTER_OUT_X_H_G = 0x19;
const uint8_t LSM9DS1_REGISTER_OUT_Y_L_G = 0x1A;
const uint8_t LSM9DS1_REGISTER_OUT_Y_H_G = 0x1B;
const uint8_t LSM9DS1_REGISTER_OUT_Z_L_G = 0x1C;
const uint8_t LSM9DS1_REGISTER_OUT_Z_H_G = 0x1D;
//////
const uint8_t LSM9DS1_REGISTER_OUT_X_L_M = 0x28;
const uint8_t LSM9DS1_REGISTER_OUT_X_H_M = 0x29;
const uint8_t LSM9DS1_REGISTER_OUT_Y_L_M = 0x2A;
const uint8_t LSM9DS1_REGISTER_OUT_Y_H_M = 0x2B;
const uint8_t LSM9DS1_REGISTER_OUT_Z_L_M = 0x2C;
const uint8_t LSM9DS1_REGISTER_OUT_Z_H_M = 0x2D;
//////
const uint8_t LSM9DS1_MAG_ID = 0B00111101;
const uint8_t LSM9DS1_REGISTER_WHO_AM_I_M = 0x0F;
const uint8_t STATUS_REG = 0x27;
const uint8_t M_ZYX_AXIS_READY = 0x08;
const uint8_t LSM9DS1_REGISTER_CTRL_REG1_M = 0x20;
const uint8_t LSM9DS1_REGISTER_CTRL_REG2_M = 0x21;
const uint8_t LSM9DS1_REGISTER_CTRL_REG3_M = 0x22;
const uint8_t LSM9DS1_REGISTER_CTRL_REG4_M = 0x23;
const uint8_t LSM9DS1_REGISTER_CTRL_REG5_M = 0x24;
const uint8_t LSM9DS1_REGISTER_CFG_M = 0x30;
const uint8_t LSM9DS1_REGISTER_INT_SRC_M = 0x31;
//////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
bool fInitializeDevice( );
bool fInitializeAG();
bool fInitializeM();
bool fDO_AG_ID();
bool getLSM9DS1_ID_OK();
bool fDO_M_ID();
bool getMAG_ID_OK();
bool setupAccelScale ( int range );
int16_t returnHighBits();
int8_t returnLowBits();
int getDh();
int getMh();
bool fEnableGandA();
bool fEnableM();
bool setupMagScale ( int gain );
bool setupGyroScale ( int _gscale );
void calibrate();
float get_aXbias();
float get_aYbias();
float get_aZbias();
float get_gXbias();
float get_gYbias();
float get_gZbias();
void fReadAccelerometers();
float get_aX();
float get_aY();
float get_aZ();
void fReadGyros();
float get_gX();
float get_gY();
float get_gZ();
void fReadMagnetometer();
float get_mX();
float get_mY();
float get_mZ();
void fReboot();
ESP32_LSM9DS1.cpp part 1
#include "ESP32_LSM9DS1.h"
///////////////////////////////////////////
spi_device_handle_t hAG;
spi_device_handle_t hM;
////
#define csPinAG 5
#define csPinM 32
#define spiCLK 25 // CLK module pin SCL
#define spiMOSI 26 // MOSI module pin SDA
#define spiMISO 27 // MISO module pin SDOAG tied to SDOM
//
float _accel_mg_lsb;
float _mag_mgauss_lsb;
float _gyro_dps_digit;
float aXbias = 0.0f;
float aYbias = 0.0f;
float aZbias = 0.0f;
float gXbias = 0.0f, gYbias = 0.0f, gZbias = 0.0f;
bool LSM9DS1_ID_OK = false;
bool M_ID_OK = false;
float aX, aY, aZ, gX, gY, gZ, mX, mY, mZ;
int8_t lowB;
int16_t highB;
float ACCELrange = 0.0f;
//////////////////////////////
int getMh()
{
return (int)hM;
}
int getDh()
{
return (int)hAG;
}
//////////////////////////////////////////
bool fInitializeDevice( )
{
esp_err_t intError;
intError = fInitializeSPI_Channel( spiCLK, spiMOSI, spiMISO, HSPI_HOST, true);
if ( intError == 0 )
{
return true;
}
}
bool fInitializeAG()
{
esp_err_t intError;
intError = fInitializeSPI_Devices( hAG, csPinAG );
if ( intError == 0 )
{
if ( (int)hAG != 0 )
{
return true;
}
}
} // bool fInitializeAG()
bool fInitializeM()
{
esp_err_t intError;
intError = fInitializeSPI_Devices( hM, csPinM );
if ( intError == 0 )
{
if ( (int)hM != 0 )
{
return true;
}
}
} // bool fInitializeM()
////
int16_t returnHighBits()
{
return highB;
}
////
int8_t returnLowBits()
{
return lowB;
}
////
void fReboot()
{
// soft reset & reboot accel/gyro
fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG8, 0x05 );
// soft reset & reboot magnetometer
fWriteSPIdata8bits( hM, LSM9DS1_REGISTER_CTRL_REG2_M, 0x0C );
}
////
bool fDO_AG_ID()
{
fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_WHO_AM_I_XG );
highB = GetHighBits();
lowB = GetLowBits();
if ( highB == LSM9DS1_ID )
{
LSM9DS1_ID_OK = true;
return true;
} else
{
return false;
}
}
//////
////
bool getLSM9DS1_ID_OK()
{
return LSM9DS1_ID_OK;
}
//////
bool fDO_M_ID()
{
// uint8_t temp;
fReadSPIdata16bits( hM, LSM9DS1_REGISTER_WHO_AM_I_M );
highB = GetHighBits();
lowB = GetLowBits();
if ( highB == LSM9DS1_MAG_ID )
{
M_ID_OK = true;
return true;
} else
{
return false;
}
}
////
bool getMAG_ID_OK()
{
return M_ID_OK;
}
//////
bool setupAccelScale ( int _range )
{
esp_err_t intError = 0;
uint8_t range = _range;
fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_CTRL_REG6_XL );
uint8_t reg = GetHighBits();
reg &= ~(0b00011000);
reg |= range;
fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG6_XL, reg );
switch (range)
{
case LSM9DS1_ACCELRANGE_2G:
_accel_mg_lsb = LSM9DS1_ACCEL_MG_LSB_2G;
ACCELrange = 2.0f;
break;
case LSM9DS1_ACCELRANGE_4G:
_accel_mg_lsb = LSM9DS1_ACCEL_MG_LSB_4G;
ACCELrange = 4.0f;
break;
case LSM9DS1_ACCELRANGE_8G:
_accel_mg_lsb = LSM9DS1_ACCEL_MG_LSB_8G;
ACCELrange = 8.0f;
break;
case LSM9DS1_ACCELRANGE_16G:
_accel_mg_lsb = LSM9DS1_ACCEL_MG_LSB_16G;
ACCELrange = 16.0f;
break;
}
if ( intError )
{
return true;
}
else
{
return false;
}
} // void setupAccel ( uint8_t range )
//////
bool fEnableGandA()
{
esp_err_t intError = 0;
// high pass filter cutoff gyro
intError = fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG3_G, 0x48 );
if ( intError > 0 )
{
return false;
}
// intError = fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG5_XL, 0x38 ); // enable X Y and Z axis
// if ( intError > 0 )
// {
// return false;
// }
intError = fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG7_XL, 0xE0 ); // High resolution on,
if ( intError > 0 )
{
return false;
}
// enable accelerometer continous
intError = fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG6_XL, 0x27 ); //
if ( intError > 0 )
{
return false;
}
// enable gyro continuous
intError = fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG1_G, 0xC0 );
//// intError = fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG1_G, 0xC1 );
if ( intError > 0 )
{
return false;
}
////
return true;
} // bool fEnableGandA()
////
bool fEnableM()
{
esp_err_t intError = 0;
fWriteSPIdata8bits( hM, LSM9DS1_REGISTER_CTRL_REG1_M, 0xFC ); // high perf XY, 80 Hz ODR
if ( intError > 0 )
{
return false;
}
fWriteSPIdata8bits( hM, LSM9DS1_REGISTER_CTRL_REG3_M, 0x00 ); // continuous mode
if ( intError > 0 )
{
ESP32_LSM9DS1.cpp part 2
return false;
}
fWriteSPIdata8bits( hM, LSM9DS1_REGISTER_CTRL_REG4_M, 0x0C ); // high perf Z mode
if ( intError > 0 )
{
return false;
}
return true;
}
////
bool setupMagScale ( int _gain )
{
uint8_t gain = _gain;
float AtoDscaleFactor = 32767.5f;
if ( fReadSPIdata16bits( hM, LSM9DS1_REGISTER_CTRL_REG2_M ) > 0 )
{
return true;
}
uint8_t reg = GetHighBits();
reg &= ~(0b01100000); // ~ ones compliment
reg |= gain;
if ( fWriteSPIdata8bits( hM, LSM9DS1_REGISTER_CTRL_REG2_M, reg ) > 0 )
{
return true;
}
switch (gain)
{
case LSM9DS1_MAGGAIN_4GAUSS:
// _mag_mgauss_lsb = LSM9DS1_MAG_MGAUSS_4GAUSS;
_mag_mgauss_lsb = 4.0f / AtoDscaleFactor;
break;
case LSM9DS1_MAGGAIN_8GAUSS:
// _mag_mgauss_lsb = LSM9DS1_MAG_MGAUSS_8GAUSS;
_mag_mgauss_lsb = 8.0f / AtoDscaleFactor;
break;
case LSM9DS1_MAGGAIN_12GAUSS:
// _mag_mgauss_lsb = LSM9DS1_MAG_MGAUSS_12GAUSS;
_mag_mgauss_lsb = 12.0f / AtoDscaleFactor;
break;
case LSM9DS1_MAGGAIN_16GAUSS:
_mag_mgauss_lsb = 16.0f / AtoDscaleFactor;
break;
}
return false;
} // void setupMag ( lsm9ds1MagGain_t gain )
////
bool setupGyroScale ( int _gscale )
{
// esp_err_t intError = 0;
uint8_t gscale = _gscale;
float AtoDscaleFactor = 32767.5f;
if ( fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_CTRL_REG1_G) > 0 )
{
return true;
}
uint8_t reg = GetHighBits();
reg &= ~(0b00011000); // ~ ones compliment
reg |= gscale;
if ( fWriteSPIdata8bits( hAG, LSM9DS1_REGISTER_CTRL_REG1_G, reg ) > 0 )
{
return true;
}
//
switch ( gscale )
{
case LSM9DS1_GYROSCALE_245DPS:
//_gyro_dps_digit = LSM9DS1_GYRO_DPS_DIGIT_245DPS;
_gyro_dps_digit = 245.0f / AtoDscaleFactor;;
break;
case LSM9DS1_GYROSCALE_500DPS:
// _gyro_dps_digit = LSM9DS1_GYRO_DPS_DIGIT_500DPS;
_gyro_dps_digit = 500.0f / AtoDscaleFactor;
break;
case LSM9DS1_GYROSCALE_2000DPS:
// _gyro_dps_digit = LSM9DS1_GYRO_DPS_DIGIT_2000DPS;
_gyro_dps_digit = 2000.0f / AtoDscaleFactor;
break;
}
return false;
} // void setupGyro ( lsm9ds1GyroScale_t scale )
////
void calibrate()
{
float _Xbias = 0.0f; // temp storage location
float _Ybias = 0.0f; // temp storage location
float _Zbias = 0.0f; // temp storage location
int numberOfSamples = 33;
// read 32 samples from each device and average those samples
// do accelerometer bias
for ( int i = 0; i < numberOfSamples; i++ )
{
fReadAccelerometers();
_Xbias = _Xbias + (aX * _accel_mg_lsb); // scale factor * _accel_mg_lsb
_Ybias = _Ybias + (aY * _accel_mg_lsb);
_Zbias = _Zbias + (aZ * _accel_mg_lsb);
vTaskDelay( 40 );
} //for ( int i = 0; i < numberOfSamples; i++ )
aXbias = _Xbias / (float)numberOfSamples;
aYbias = _Ybias / (float)numberOfSamples;
aZbias = _Zbias / (float)numberOfSamples;
_Xbias = 0.0f;
_Ybias = 0.0f;
_Zbias = 0.0f;
// do gyro bias
for ( int i = 0; i < numberOfSamples; i++ )
{
fReadGyros();
_Xbias = _Xbias + (gX * _gyro_dps_digit); // scale factor * _gyro_dps_digit
_Ybias = _Ybias + (gY * _gyro_dps_digit);
_Zbias = _Zbias + (gZ * _gyro_dps_digit);
vTaskDelay( 40 );
} // for ( int i = 0; i < numberOfSamples; i++ )
gXbias = _Xbias / (float)numberOfSamples;
gYbias = _Ybias / (float)numberOfSamples;
gZbias = _Zbias / (float)numberOfSamples;
} // void calibrate()
////
float get_aXbias( )
{
return -(aXbias);
}
////
float get_aYbias()
{
return -(aYbias);
}
////
float get_aZbias()
{
return -(aZbias);
}
////
float get_gXbias()
{
return -(gXbias);
}
////
float get_gYbias()
{
return -(gYbias);
}
////
float get_gZbias()
{
return -(gZbias);
}
////
void fReadAccelerometers()
{
int16_t temp = 0;
// read status register
fReadSPIdata16bits( hAG, STATUS_REG );
// is new accel data available
if ( GetHighBits() & AccelerometerDataReady )
{
fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_OUT_X_L_XL ); // read x accel
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
aX = (float)temp;
aX = (aX * _accel_mg_lsb) - aXbias;
temp = 0;
//
fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_OUT_Y_L_XL ); // read Y accel
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
aY = (float)temp;
aY = (aY * _accel_mg_lsb) - aYbias;
temp = 0;
//
fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_OUT_Z_L_XL ); // read z accel
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
aZ = (float)temp;
aZ = (aZ * _accel_mg_lsb) - aZbias;
}
}
////
float get_aX()
{
return aX;
}
float get_aY()
{
return aY;
}
float get_aZ()
{
return aZ;
}
////
void fReadGyros()
{
int16_t temp;
fReadSPIdata16bits( hAG, STATUS_REG );
// is new gyro data available
if ( GetHighBits() & GyroDataReady )
{
gX = 0.0f; gY = 0.0f; gZ = 0.0f;
fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_OUT_X_L_G ); // read x gyro
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
temp /= 1000.0f;
gX = (float)temp * _gyro_dps_digit; // scale
gX -= gXbias;
temp = 0;
fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_OUT_Y_L_G ); // read Y gyro
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
temp /= 1000.0f;
gY = (float)temp * _gyro_dps_digit; // scale
gY -= gYbias;
temp = 0;
fReadSPIdata16bits( hAG, LSM9DS1_REGISTER_OUT_Z_L_G ); // read Z gyro
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
temp /= 1000.0f;
gZ = (float)temp * _gyro_dps_digit; // scale
gZ -= gZbias;
}
}
////
float get_gX()
{
return gX;
}
////
float get_gY()
{
return gY;
}
////
float get_gZ()
{
return gZ;
}
////
void fReadMagnetometer()
{
int16_t temp;
fReadSPIdata16bits( hM, STATUS_REG );
if ( GetHighBits() & LSM9DS1_REGISTER_OUT_X_L_M )
{
fReadSPIdata16bits( hM, LSM9DS1_REGISTER_OUT_X_L_M ); // read X magnetometer
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
mX = (float)temp * _mag_mgauss_lsb; // scale
////
fReadSPIdata16bits( hM, LSM9DS1_REGISTER_OUT_Y_L_M ); // read Y magnetometer
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
mY = (float)temp * _mag_mgauss_lsb; // scale
// ////
fReadSPIdata16bits( hM, LSM9DS1_REGISTER_OUT_Z_L_M ); // read Z magnetometer
temp = GetHighBits(); temp <<= 8; temp |= GetLowBits();
mZ = (float)temp * _mag_mgauss_lsb; // scale
}
}
////
float get_mX()
{
return mX;
}
////
float get_mY()
{
return mY;
}
////
float get_mZ()
{
return mZ;
}
////
Use example
/*
A transaction on the SPI bus consists of five phases, any of which may be skipped:
The command phase. In this phase, a command (0-16 bit) is clocked out.
The address phase. In this phase, an address (0-64 bit) is clocked out.
The read phase. The slave sends data to the master.
The write phase. The master sends data to the slave.
In full duplex, the read and write phases are combined, causing the SPI host to read and write data simultaneously.
The command and address phase are optional in that not every SPI device will need to be sent a command and/or address.
Tis is reflected in the device configuration: when the command_bits or data_bits fields are set to zero, no command or address phase is done.
Something similar is true for the read and write phase: not every transaction needs both data to be written as well as data to be read.
When rx_buffer is NULL (and SPI_USE_RXDATA) is not set) the read phase is skipped.
When tx_buffer is NULL (and SPI_USE_TXDATA) is not set) the write phase is skipped.
*/
void fGetIMU( void *pvParameters )
{
if ( fInitializeDevice( ) )
{
Serial.println ( " device init " );
if ( fInitializeAG() )
{
Serial.println( "AG init" );
if ( fDO_AG_ID() )
{
Serial.println ( " AG self ID'd " );
if ( fInitializeM() )
{
Serial.println( "Init Magnetometer" );
if ( fDO_M_ID() )
{
Serial.print ( " M self ID'd: " );
Serial.println( getMAG_ID_OK() );
fReboot();
vTaskDelay( 50 );
if ( fEnableGandA() == false ) // enable gyros and accelerometers
{
Serial.print( " Fail: Enable Gyro and Accelerometer " );
}
if ( fEnableM() == false ) // enable gyros and accelerometers
{
Serial.println( " Fail: Enable Magnetometer" );
}
if ( setupAccelScale( LSM9DS1_ACCELRANGE_8G ) )
{
Serial.println( " Fail: SetupAccelScale" );
}
if ( setupGyroScale ( LSM9DS1_GYROSCALE_500DPS ) )
{
Serial.println( " Fail: setupGyroScale" );
}
if (setupMagScale( LSM9DS1_MAGGAIN_12GAUSS ) )
{
Serial.println( "FailL setupMagScale" );
}
// calibrate();
// Serial.print( " aXbias = " );
// Serial.print( get_aXbias(), 6 );
// Serial.print( " aYbias = " );
// Serial.print( get_aYbias(), 6 );
// Serial.print( " aZias = " );
// Serial.print( get_aZbias(), 6 );
// Serial.print( " gXbias ");
// Serial.print( get_gXbias(), 6 );
// Serial.print( " gYbias = " );
// Serial.print( get_gYbias(), 6 );
// Serial.print( " gZbias = " );
// Serial.println( get_gZbias(), 6 );
} // if ( fDO_M_ID )
else
{
Serial.print( "fDO_M_ID, fail: ");
Serial.print( returnHighBits(), BIN ); //each LSMDS1 may have its own ID
Serial.println( );
}
} // if ( fInitializeM() )
} // if ( fDO_AG_ID() )
else
{
Serial.print( "fDO_AG_ID, fail: ");
Serial.print( returnHighBits(), BIN ); //each LSMDS1 may have its own ID
Serial.println( );
}
} // if ( fInitializeAG() )
} // if ( fInitializeDevice( ) )
////
TickType_t xLastWakeTime;
const TickType_t xFrequency = pdMS_TO_TICKS( 35 );
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
float TimePast = esp_timer_get_time();
float TimeNow = esp_timer_get_time();
////
float Max = 0.0f;
while (1)
{
vTaskDelayUntil( &xLastWakeTime, xFrequency );
// Serial.println ( " doing a loop " );
if ( getLSM9DS1_ID_OK() && getMAG_ID_OK() ) // then do things
{
//TimeNow = xTaskGetTickCount();
TimeNow = esp_timer_get_time() / 1000000.0f;
// TimeNow = micros() / 1000000.0f;
deltat = ( TimeNow - TimePast);
// Serial.println( deltat,6 );
////
fReadAccelerometers();
fReadGyros();
fReadMagnetometer();
// Serial.print ( "aX= ");
Serial.print( get_aX(), 6 );
Serial.print ( ", ");
// Serial.print ( " aY= ");
Serial.print( get_aY(), 6 );
Serial.print ( ", ");
// Serial.print ( " aZ= ");
Serial.print( get_aZ(), 6 );
Serial.print ( ", ");
// Serial.print ( " gX= ");
Serial.print( get_gX(), 6 );
Serial.print ( ", ");
// Serial.print ( " gY= ");
Serial.print( get_gY(), 6 );
Serial.print ( ", ");
// Serial.print ( " gZ= ");
Serial.print( get_gZ(), 6 );
Serial.print ( ", ");
// Serial.print ( " mX= ");
Serial.print( get_mX(), 6 );
Serial.print ( ", ");
// Serial.print ( " mY= ");
Serial.print( get_mY(), 6 );
Serial.print ( ", ");
// Serial.print ( " mZ= ");
Serial.print( get_mZ(), 6 );
Serial.println();
// roll = atan2f( get_aX(), sqrtf( (get_aY() * get_aY()) + (get_aZ() * get_aZ())) );
// pitch = atan2f( get_aY(), sqrtf( (get_aX() * get_aX()) + (get_aZ() * get_aZ())) );
MahonyQuaternionUpdate ( get_aX(), get_aY(), get_aZ(), get_gX() * PI / 180.0f, get_gY() * PI / 180.0f, get_gZ() * PI / 180.0f, get_mX(), get_mY(), get_mZ() );
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
// yaw = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
Serial.print( roll, 6 );
Serial.print( ", " );
Serial.print( pitch, 6 );
Serial.println( );
// log_i( " interrupt Count %d", IntCount );
TimePast = TimeNow;
} // if ( LSM9DS1_ID_OK && M_ID_OK ) // then do things
else
{
Serial.print ( " LSM9DS1_ID_NOT_OK ");
Serial.print ( getLSM9DS1_ID_OK() );
Serial.print ( " or MAG_ID_OK not OK " );
Serial.println( getMAG_ID_OK() );
}
xLastWakeTime = xTaskGetTickCount();
}
vTaskDelete(NULL);
} // void fGetIMU( void *pvParameters )
This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.