MPU9250, good news:
Posted: Thu Apr 04, 2019 5:47 pm
I wrote this code for the MPU9250. I been testing it for the last 4 days. It's been working wonderfully.
Thanks to Koolban, though he does need to update the SPI portion of his notes. Some names have been changed.
The testing unit takes the roll and pitch to maintain a X/Y platform on level. The calibration code, which I can post, rotates the X/Y/Z axis takes measurements, and sums them together for offset bias for the accelerometers, gyros, and magnetometer.
I, also, wrote the code for an LSM9DS1, which I will post when I am done with that project.
It is my understanding that if a magnetometer is mounted 180 degrees out of phase with another magnetometer, a self calibrating magnetometer can be realized. I am going to see if that's possible.
BTW. Feel free to use the code and modify it in any way you wish.
Thanks to Koolban, though he does need to update the SPI portion of his notes. Some names have been changed.
Code: Select all
#include <driver/spi_master.h>
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "freertos/event_groups.h"
#include "sdkconfig.h"
////////////////////////////////////////////////////
#define evtGetIMU ( 1 << 1 ) // 10
///////////////////////////////////////////////////
EventGroupHandle_t eg;
///////////////////////////////////////////////////
#define TaskCore1 1
#define TaskCore0 0
#define SerialDataBits 115200
#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
#define TaskStack30K 30000
#define Priority4 4
#define MPU_int_Pin 34
////////////////////////////////////////
spi_device_handle_t hAG;
////////////////////////////////////////
const uint8_t SPI_READ = 0x80;
const uint8_t WHO_I_AMa = 0x73;
const uint8_t WHO_I_AMb = 0x71;
const uint8_t AK8963_IS = 0x48;
const uint8_t ACCELX_OUT = 0x3B;
const uint8_t ACCELY_OUT = 0x3D;
const uint8_t ACCELZ_OUT = 0x3F;
const uint8_t GYRO_OUTX = 0x43;
const uint8_t GYRO_OUTY = 0x45;
const uint8_t GYRO_OUTZ = 0x47;
// transformation matrix
/* transform the accel and gyro axes to match the magnetometer axes */
// constants
const uint8_t EXT_SENS_DATA_00 = 0x49;
const float G = 9.807f;
const float _d2r = 3.14159265359f / 180.0f;
const int16_t tX[3] = {0, 1, 0};
const int16_t tY[3] = {1, 0, 0};
const int16_t tZ[3] = {0, 0, -1};
const uint8_t ACCEL_CONFIG = 0x1C;
const uint8_t ACCEL_FS_SEL_2G = 0x00;
const uint8_t ACCEL_FS_SEL_4G = 0x08;
const uint8_t ACCEL_FS_SEL_8G = 0x10;
const uint8_t ACCEL_FS_SEL_16G = 0x18;
const uint8_t GYRO_CONFIG = 0x1B;
const uint8_t GYRO_FS_SEL_250DPS = 0x00;
const uint8_t GYRO_FS_SEL_500DPS = 0x08;
const uint8_t GYRO_FS_SEL_1000DPS = 0x10;
const uint8_t GYRO_FS_SEL_2000DPS = 0x18;
const uint8_t ACCEL_CONFIG2 = 0x1D;
const uint8_t ACCEL_DLPF_184 = 0x01;
const uint8_t ACCEL_DLPF_92 = 0x02;
const uint8_t ACCEL_DLPF_41 = 0x03;
const uint8_t ACCEL_DLPF_20 = 0x04;
const uint8_t ACCEL_DLPF_10 = 0x05;
const uint8_t ACCEL_DLPF_5 = 0x06;
const uint8_t CONFIG = 0x1A;
const uint8_t GYRO_DLPF_184 = 0x01;
const uint8_t GYRO_DLPF_92 = 0x02;
const uint8_t GYRO_DLPF_41 = 0x03;
const uint8_t GYRO_DLPF_20 = 0x04;
const uint8_t GYRO_DLPF_10 = 0x05;
const uint8_t GYRO_DLPF_5 = 0x06;
const uint8_t SMPDIV = 0x19;
const uint8_t INT_PIN_CFG = 0x37;
const uint8_t INT_ENABLE = 0x38;
const uint8_t INT_DISABLE = 0x00;
const uint8_t INT_PULSE_50US = 0x00;
const uint8_t INT_WOM_EN = 0x40;
const uint8_t INT_RAW_RDY_EN = 0x01;
const uint8_t PWR_MGMNT_1 = 0x6B;
const uint8_t PWR_CYCLE = 0x20;
const uint8_t PWR_RESET = 0x80;
const uint8_t CLOCK_SEL_PLL = 0x01;
const uint8_t PWR_MGMNT_2 = 0x6C;
const uint8_t SEN_ENABLE = 0x00;
const uint8_t DIS_GYRO = 0x07;
const uint8_t USER_CTRL = 0x6A;
const uint8_t I2C_MST_EN = 0x20;
const uint8_t I2C_MST_CLK = 0x0D;
const uint8_t I2C_MST_CTRL = 0x24;
const uint8_t I2C_SLV0_ADDR = 0x25;
const uint8_t I2C_SLV0_REG = 0x26;
const uint8_t I2C_SLV0_DO = 0x63;
const uint8_t I2C_SLV0_CTRL = 0x27;
const uint8_t I2C_SLV0_EN = 0x80;
const uint8_t I2C_READ_FLAG = 0x80;
const uint8_t MOT_DETECT_CTRL = 0x69;
const uint8_t ACCEL_INTEL_EN = 0x80;
const uint8_t ACCEL_INTEL_MODE = 0x40;
const uint8_t LP_ACCEL_ODR = 0x1E;
const uint8_t WOM_THR = 0x1F;
const uint8_t WHO_AM_I = 0x75;
const uint8_t FIFO_EN = 0x23;
const uint8_t FIFO_TEMP = 0x80;
const uint8_t FIFO_GYRO = 0x70;
const uint8_t FIFO_ACCEL = 0x08;
const uint8_t FIFO_MAG = 0x01;
const uint8_t FIFO_COUNT = 0x72;
const uint8_t FIFO_READ = 0x74;
// AK8963 registers
const uint8_t AK8963_I2C_ADDR = 0x0C;
const uint8_t AK8963_HXL = 0x03;
const uint8_t AK8963_CNTL1 = 0x0A;
const uint8_t AK8963_PWR_DOWN = 0x00;
const uint8_t AK8963_CNT_MEAS1 = 0x12;
const uint8_t AK8963_CNT_MEAS2 = 0x16;
const uint8_t AK8963_FUSE_ROM = 0x0F;
const uint8_t AK8963_CNTL2 = 0x0B;
const uint8_t AK8963_RESET = 0x01;
const uint8_t AK8963_ASA = 0x10;
const uint8_t AK8963_WHO_AM_I = 0x00;
////////////////////////////////////////////////
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
float deltat = 0.0f; // integration interval for both filter schemes
#define Kp 7.50f
// #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 1.7f
uint8_t txData[2] = { };
uint8_t rxData[21] = { };
////////////////////////////////////////////////
void triggerGet_IMU()
{
BaseType_t xHigherPriorityTaskWoken;
xEventGroupSetBitsFromISR(eg, evtGetIMU, &xHigherPriorityTaskWoken);
}
///////////////////////////////////////////////
void setup()
{
eg = xEventGroupCreate();
Serial.begin( SerialDataBits );
xTaskCreatePinnedToCore ( fGetIMU, "v_getIMU", TaskStack30K, NULL, Priority4, NULL, TaskCore0 );
}
////
void loop() {}
////
void fGetIMU( void *pvParameters )
{
bool MPU9250_OK = false;
bool AK8963_OK = false;
esp_err_t intError;
// data counts
int16_t _axcounts, _aycounts, _azcounts;
int16_t _gxcounts, _gycounts, _gzcounts;
int16_t _hxcounts, _hycounts, _hzcounts;
int16_t _tcounts;
// data buffer
float _ax, _ay, _az;
float _gx, _gy, _gz;
float _hx, _hy, _hz;
float _t;
float _accelScale;
float _gyroScale;
float _magScaleX, _magScaleY, _magScaleZ;
float _axb, _ayb, _azb;
float _gxb, _gyb, _gzb;
float _axs = 1.0f;
float _ays = 1.0f;
float _azs = 1.0f;
float _hxb, _hyb, _hzb;
float _hxs = 1.0f;
float _hys = 1.0f;
float _hzs = 1.0f;
////
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
Serial.print(" Initializing bus error = ");
intError = spi_bus_initialize(HSPI_HOST, &bus_config, 1) ;
Serial.println ( 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 = SPI_MODE3 ;
dev_config.duty_cycle_pos = 0;
dev_config.cs_ena_posttrans = 0;
dev_config.cs_ena_pretrans = 0;
dev_config.clock_speed_hz = 1000000; // mpu 9250 spi read registers safe up to 1Mhz.
dev_config.spics_io_num = csPinAG;
dev_config.flags = 0;
dev_config.queue_size = 1;
dev_config.pre_cb = NULL;
dev_config.post_cb = NULL;
Serial.print (" Adding device bus error = ");
intError = spi_bus_add_device(HSPI_HOST, &dev_config, &hAG);
Serial.println ( intError );
////
////
// spi_transaction_t trans_desc = { };
fWrite_AK8963( AK8963_CNTL1, AK8963_PWR_DOWN );
vTaskDelay ( 100 );
// fWriteSPIdata8bits( PWR_MGMNT_1, PWR_RESET );
// wait for MPU-9250 to come back up
// vTaskDelay(100);
// select clock source to gyro
// PWR_MGMNT_1,CLOCK_SEL_PLL
fWriteSPIdata8bits( PWR_MGMNT_1, CLOCK_SEL_PLL );
vTaskDelay(1);
// Do who am I MPU9250
fReadMPU9250 ( 2 , WHO_AM_I );
if ( (WHO_I_AMa == rxData[1]) || (WHO_I_AMb == rxData[1]) )
{
MPU9250_OK = true;
}
else
{
Serial.print( " I am not MPU9250! I am: ");
Serial.println ( rxData[1] );
}
if ( MPU9250_OK )
{
// enable I2C master mode
// USER_CTRL,I2C_MST_EN
fWriteSPIdata8bits( USER_CTRL, I2C_MST_EN );
vTaskDelay(1);
// set the I2C bus speed to 400 kHz
// I2C_MST_CTRL,I2C_MST_CLK
fWriteSPIdata8bits( I2C_MST_CTRL, I2C_MST_CLK );
vTaskDelay(1);
fWriteSPIdata8bits( SMPDIV, 0x04 );
vTaskDelay(1);
// DLPF ACCEL_CONFIG2,ACCEL_DLPF_184
fWriteSPIdata8bits( ACCEL_CONFIG2, ACCEL_DLPF_184 );
// CONFIG,GYRO_DLPF_184
fWriteSPIdata8bits( CONFIG, GYRO_DLPF_184 );
vTaskDelay(1);
// check AK8963_WHO_AM_I
fReadAK8963( AK8963_WHO_AM_I, 1 );
vTaskDelay(500); // giving the AK8963 lots of time to recover from reset
fReadMPU9250( 2 , EXT_SENS_DATA_00 );
if ( AK8963_IS == rxData[1] )
{
AK8963_OK = true;
}
else
{
Serial.print ( " AK8963_OK data return = " );
Serial.print ( rxData[0] );
Serial.println ( rxData[1] );
}
if ( AK8963_OK )
{
// set AK8963 to FUSE ROM access
// AK8963_CNTL1,AK8963_FUSE_ROM
fWrite_AK8963 ( AK8963_CNTL1, AK8963_FUSE_ROM );
vTaskDelay ( 100 ); // delay for mode change
// // setting the accel range to 16G
fWriteSPIdata8bits( ACCEL_CONFIG, ACCEL_FS_SEL_16G );
_accelScale = 16.0f / 32768.0f; // setting the accel scale to 16G
// ACCEL_CONFIG,ACCEL_FS_SEL_8G
// fWriteSPIdata8bits( ACCEL_CONFIG, ACCEL_FS_SEL_8G );
// _accelScale = 8.0f/32768.0f; // setting the accel scale to 8G
// ACCEL_CONFIG,ACCEL_FS_SEL_4G
// fWriteSPIdata8bits( ACCEL_CONFIG, ACCEL_FS_SEL_4G );
// _accelScale = 4.0f/32768.0f; // setting the accel scale to 4G
// ACCEL_CONFIG,ACCEL_FS_SEL_2G
// fWriteSPIdata8bits( ACCEL_CONFIG, ACCEL_FS_SEL_2G );
// _accelScale = 2.0f/32768.0f;
// // _accelScale = G * 2.0f / 32767.5f; // setting the accel scale to 2G
// set gyro scale
// GYRO_CONFIG,GYRO_FS_SEL_1000DPS
fWriteSPIdata8bits( GYRO_CONFIG, GYRO_FS_SEL_1000DPS );
// 250.0f/32768.0f;
// 500.0f/32768.0f;
// 2000.0f/32768.0f;
_gyroScale = 1000.0f / 32768.0f;
// read the AK8963 ASA registers and compute magnetometer scale factors
// set accel scale
fReadAK8963(AK8963_ASA, 3 );
fReadMPU9250 ( 3 , EXT_SENS_DATA_00 );
// convert to mG multiply by 10
_magScaleX = ((((float)rxData[0]) - 128.0f) / (256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
_magScaleY = ((((float)rxData[1]) - 128.0f) / (256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
_magScaleZ = ((((float)rxData[2]) - 128.0f) / (256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
Serial.print( " _magScaleX " );
Serial.print( _magScaleX );
Serial.print( " , " );
Serial.print( " _magScaleY " );
Serial.print( _magScaleY );
Serial.print( " , " );
Serial.print( " _magScaleZ " );
Serial.print( _magScaleZ );
Serial.print( " , " );
Serial.println();
// set AK8963 to 16 bit resolution, 100 Hz update rate
// AK8963_CNTL1,AK8963_CNT_MEAS2
fWrite_AK8963( AK8963_CNTL1, AK8963_CNT_MEAS2 );
// delay for mode change
vTaskDelay ( 100 );
// AK8963_HXL,7 ;
fReadAK8963(AK8963_HXL, 7 );
////
/* setting the interrupt */
// INT_PIN_CFG,INT_PULSE_50US setup interrupt, 50 us pulse
fWriteSPIdata8bits( INT_PIN_CFG, INT_PULSE_50US );
// INT_ENABLE,INT_RAW_RDY_EN set to data ready
fWriteSPIdata8bits( INT_ENABLE, INT_RAW_RDY_EN );
pinMode( MPU_int_Pin, INPUT );
attachInterrupt( MPU_int_Pin, triggerGet_IMU, RISING );
}
}
////////////////////////////////////////////////////////////////
TickType_t TimePast = xTaskGetTickCount();
TickType_t TimeNow = xTaskGetTickCount();
TickType_t xLastWakeTime;
// const TickType_t xFrequency = pdMS_TO_TICKS( 100 );
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
////
while (1)
{
xEventGroupWaitBits (eg, evtGetIMU, pdTRUE, pdTRUE, portMAX_DELAY);
if ( MPU9250_OK && AK8963_OK )
{
TimeNow = xTaskGetTickCount();
deltat = ( (float)TimeNow - (float)TimePast) / 1000.0f;
////
// Serial.println ( " doing a loop MPU OK" );
fReadMPU9250 ( 8 , 0X3A );
_axcounts = (int16_t)(((int16_t)rxData[2] << 8) | rxData[3]) ;
_aycounts = (int16_t)(((int16_t)rxData[4] << 8) | rxData[5]) ;
_azcounts = (int16_t)(((int16_t)rxData[6] << 8) | rxData[7]) ;
fReadMPU9250 ( 6 , GYRO_OUTX );
_gxcounts = (int16_t)(((int16_t)rxData[0]) << 8) | rxData[1];
_gycounts = (int16_t)(((int16_t)rxData[2]) << 8) | rxData[3];
_gzcounts = (int16_t)(((int16_t)rxData[4]) << 8) | rxData[5];
// EXT_SENS_DATA_00
fReadMPU9250 ( 6 , EXT_SENS_DATA_00 );
_hxcounts = ((int16_t)rxData[1] << 8) | rxData[0] ;
_hycounts = ((int16_t)rxData[3] << 8) | rxData[2] ;;
_hzcounts = ((int16_t)rxData[5] << 8) | rxData[4] ;;
//
/// no biases applied just scale factor
_ax = (float)_axcounts * _accelScale;
_ay = (float)_aycounts * _accelScale;
_az = (float)_azcounts * _accelScale;
//
_gx = (float)_gxcounts * _gyroScale;
_gy = (float)_gycounts * _gyroScale;
_gz = (float)_gzcounts * _gyroScale;
//
_hx = (float)_hxcounts * _magScaleX;
_hy = (float)_hycounts * _magScaleY;
_hz = (float)_hzcounts * _magScaleZ;
////
// Serial.print ( "ax = " );
// Serial.print ( _ax * 1000.0f );
// Serial.print ( " ay = " );
// Serial.print ( _ay * 1000.0f );
// Serial.print ( " az = " );
// Serial.print ( _az * 1000.0f );
// Serial.print ( " gx = " );
// Serial.print ( _gx * 1000.0f );
// Serial.print ( " gy = " );
// Serial.print ( _gy * 1000.0f );
// Serial.print ( " gz = " );
// Serial.print ( _gz * 1000.0f );
// Serial.print( " deg/s" );
// Serial.print ( " hx = " );
// Serial.print ( _hx );
// Serial.print ( " hy = " );
// Serial.print ( _hy );
// Serial.print ( " hz = " );
// Serial.print ( _hz );
// Serial.print ( " uT" );
// Serial.println();
// h value multiplied by 10 converts to mG from uT
MahonyQuaternionUpdate ( _ax, _ay, _az, _gx, _gy, _gz, _hx * 10.0f, _hy * 10.0f, _hz * 10.0f );
// 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.
float 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]);
float pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
float 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]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw += 13.13f; // Declination
roll *= 180.0f / PI;
TimePast = TimeNow;
Serial.print ( " Roll: ");
Serial.print ( roll, 6 );
Serial.print ( " Pitch: " );
Serial.print ( pitch, 6 );
Serial.print ( " Yaw: " );
Serial.println ( yaw, 6 );
}
else
{
Serial.print ( "WHO AM I FAILED!! ");
Serial.print ( "MPU9250 OK = ");
Serial.print ( MPU9250_OK );
Serial.print ( " AK8963 OK = " );
Serial.println ( AK8963_OK );
}
// xLastWakeTime = xTaskGetTickCount();
}
vTaskDelete(NULL);
} // void fGetIMU( void *pvParameters )
///////////////////////////////////////////
void fReadMPU9250 ( uint8_t byteReadSize, uint8_t addressToRead )
{
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) + (8 * byteReadSize) ; // total data bits
trans_desc.tx_buffer = txData;
trans_desc.rxlength = byteReadSize * 8 ; // Number of bits NOT number of bytes
trans_desc.rx_buffer = rxData;
txData[0] = addressToRead | SPI_READ;
txData[1] = 0x0;
intError = spi_device_transmit( hAG, &trans_desc);
if ( intError != 0 )
{
Serial.print( " WHO I am MPU9250. Transmitting error = ");
Serial.println ( intError );
}
} // void fReadMPU9250 ( uint8_t byteReadSize, uint8_t addressToRead )
void fWriteSPIdata8bits( uint8_t address, uint8_t DataToSend)
{
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;
txData[0] = address;
txData[1] = DataToSend;
intError = spi_device_transmit( hAG, &trans_desc);
if ( intError != 0 )
{
Serial.print( " Transmitting error = ");
Serial.println ( intError );
}
} // void fSendSPI( uint8_t count, uint8_t address, uint8_t DataToSend)
///////////////////////////////////////////////////////////////////////
void fReadAK8963( uint8_t subAddress, uint8_t count )
{
// set slave 0 to the AK8963 and set for read
fWriteSPIdata8bits ( I2C_SLV0_ADDR, AK8963_I2C_ADDR | I2C_READ_FLAG );
// set the register to the desired AK8963 sub address
fWriteSPIdata8bits ( I2C_SLV0_REG, subAddress );
// enable I2C and request the bytes
fWriteSPIdata8bits ( I2C_SLV0_CTRL, I2C_SLV0_EN | count );
// fwriteMUP9250register ( I2C_SLV0_CTRL, 0x80 );
vTaskDelay ( 1 );
} // fReadAK8963(uint8_t subAddress, uint8_t count, uint8_t* dest)
////////////////////////////////////////////////////////////////////////////////////////////
void fWrite_AK8963 ( uint8_t subAddress, uint8_t dataAK8963 )
{
fWriteSPIdata8bits ( I2C_SLV0_ADDR, AK8963_I2C_ADDR );
fWriteSPIdata8bits ( I2C_SLV0_REG, subAddress );
fWriteSPIdata8bits ( I2C_SLV0_DO, dataAK8963 );
fWriteSPIdata8bits ( I2C_SLV0_CTRL, I2C_SLV0_EN | (uint8_t)1) ;
//
vTaskDelay ( 1 );
} // void fWrite_AK8963 ( uint8_t subAddress, uint8_t dataAK8963 )
/////////////////////////////////////////////////////////////////////////////////////////////
void fwriteMUP9250register ( uint8_t addr, uint8_t sendData )
{
esp_err_t intError;
spi_transaction_t trans_desc = {};
trans_desc.addr = addr;
trans_desc.cmd = 0;
trans_desc.flags = 0;
trans_desc.length = 8 * 1 ; // total data bits
trans_desc.tx_buffer = txData;
txData[0] = sendData;
Serial.print( " write mpu register " );
Serial.print ( addr, HEX );
Serial.print ( " data " );
Serial.print ( sendData, HEX );
Serial.print ( ". Transmitting error = ");
intError = spi_device_transmit( hAG, &trans_desc);
Serial.println ( intError );
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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;
} // void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
//////////////////////////////////////////////I, also, wrote the code for an LSM9DS1, which I will post when I am done with that project.
It is my understanding that if a magnetometer is mounted 180 degrees out of phase with another magnetometer, a self calibrating magnetometer can be realized. I am going to see if that's possible.
BTW. Feel free to use the code and modify it in any way you wish.