MPU9250, good news:

idahowalker
Posts: 154
Joined: Wed Aug 01, 2018 12:06 pm

MPU9250, good news:

Postby idahowalker » 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.

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)
//////////////////////////////////////////////
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.

Who is online

Users browsing this forum: No registered users and 9 guests