yikestone/Embedded_codes
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Embedded_codes/libraries/RTIMULib/utility/RTIMUBMX055.cpp
yikestone c932efe7ff initial
2018-02-18 14:02:33 +05:30

907 lines
29 KiB
C++
Raw Blame History

////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
// Some code from Bosch BMM150 API driver:
/*
****************************************************************************
* Copyright (C) 2011 - 2014 Bosch Sensortec GmbH
*
* bmm050.c
* Date: 2014/12/12
* Revision: 2.0.3 $
*
* Usage: Sensor Driver for BMM050 and BMM150 sensor
*
****************************************************************************
* License:
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* Neither the name of the copyright holder nor the names of the
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER
* OR CONTRIBUTORS BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
* OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
*
* The information provided is believed to be accurate and reliable.
* The copyright holder assumes no responsibility
* for the consequences of use
* of such information nor for any infringement of patents or
* other rights of third parties which may result from its use.
* No license is granted by implication or otherwise under any patent or
* patent rights of the copyright holder.
**************************************************************************/
/****************************************************************************/
#include "RTIMUBMX055.h"
#include "RTIMUSettings.h"
// this sets the learning rate for compass running average calculation
#define COMPASS_ALPHA 0.2f
RTIMUBMX055::RTIMUBMX055(RTIMUSettings *settings) : RTIMU(settings)
{
m_sampleRate = 100;
m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
}
RTIMUBMX055::~RTIMUBMX055()
{
}
bool RTIMUBMX055::IMUInit()
{
unsigned char result;
m_firstTime = true;
// set validity flags
m_imuData.fusionPoseValid = false;
m_imuData.fusionQPoseValid = false;
m_imuData.gyroValid = true;
m_imuData.accelValid = true;
m_imuData.compassValid = true;
m_imuData.pressureValid = false;
m_imuData.temperatureValid = false;
m_imuData.humidityValid = false;
// configure IMU
m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_WHO_AM_I, 1, &result, "Failed to read BMX055 gyro id"))
return false;
if (result != BMX055_GYRO_ID) {
HAL_ERROR1("Incorrect BMX055 id %d\n", result);
return false;
}
// work out accel address
if (m_settings->HALRead(BMX055_ACCEL_ADDRESS0, BMX055_ACCEL_WHO_AM_I, 1, &result, "")) {
if (result == BMX055_ACCEL_ID) {
m_accelSlaveAddr = BMX055_ACCEL_ADDRESS0;
} else {
m_accelSlaveAddr = BMX055_ACCEL_ADDRESS1;
}
}
// work out mag address
int magAddr;
for (magAddr = BMX055_MAG_ADDRESS0; magAddr <= BMX055_MAG_ADDRESS3; magAddr++) {
// have to enable chip to get id...
m_settings->HALWrite(magAddr, BMX055_MAG_POWER, 1, "");
m_settings->delayMs(50);
if (m_settings->HALRead(magAddr, BMX055_MAG_WHO_AM_I, 1, &result, "")) {
if (result == BMX055_MAG_ID) {
m_magSlaveAddr = magAddr;
break;
}
}
}
if (magAddr > BMX055_MAG_ADDRESS3) {
HAL_ERROR("Failed to find BMX055 mag\n");
return false;
}
setCalibrationData();
// enable the I2C bus
if (!m_settings->HALOpen())
return false;
// Set up the gyro
if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config"))
return false;
if (!setGyroSampleRate())
return false;
if (!setGyroFSR())
return false;
gyroBiasInit();
// set up the accel
if (!setAccelSampleRate())
return false;
if (!setAccelFSR())
return false;
// set up the mag
magInitTrimRegisters();
setMagPreset();
HAL_INFO("BMX055 init complete\n");
return true;
}
bool RTIMUBMX055::setGyroSampleRate()
{
unsigned char bw;
switch (m_settings->m_BMX055GyroSampleRate) {
case BMX055_GYRO_SAMPLERATE_2000_523:
bw = 0;
m_sampleRate = 2000;
break;
case BMX055_GYRO_SAMPLERATE_2000_230:
bw = 1;
m_sampleRate = 2000;
break;
case BMX055_GYRO_SAMPLERATE_1000_116:
bw = 2;
m_sampleRate = 1000;
break;
case BMX055_GYRO_SAMPLERATE_400_47:
bw = 3;
m_sampleRate = 400;
break;
case BMX055_GYRO_SAMPLERATE_200_23:
bw = 4;
m_sampleRate = 200;
break;
case BMX055_GYRO_SAMPLERATE_100_12:
bw = 5;
m_sampleRate = 100;
break;
case BMX055_GYRO_SAMPLERATE_200_64:
bw = 6;
m_sampleRate = 200;
break;
case BMX055_GYRO_SAMPLERATE_100_32:
bw = 7;
m_sampleRate = 100;
break;
default:
HAL_ERROR1("Illegal BMX055 gyro sample rate code %d\n", m_settings->m_BMX055GyroSampleRate);
return false;
}
m_sampleInterval = (uint64_t)1000000 / m_sampleRate;
return (m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_BW, bw, "Failed to set BMX055 gyro rate"));
}
bool RTIMUBMX055::setGyroFSR()
{
switch(m_settings->m_BMX055GyroFsr) {
case BMX055_GYRO_FSR_2000:
m_gyroScale = 0.061 * RTMATH_DEGREE_TO_RAD;
break;
case BMX055_GYRO_FSR_1000:
m_gyroScale = 0.0305 * RTMATH_DEGREE_TO_RAD;
break;
case BMX055_GYRO_FSR_500:
m_gyroScale = 0.0153 * RTMATH_DEGREE_TO_RAD;
break;
case BMX055_GYRO_FSR_250:
m_gyroScale = 0.0076 * RTMATH_DEGREE_TO_RAD;
break;
case BMX055_GYRO_FSR_125:
m_gyroScale = 0.0038 * RTMATH_DEGREE_TO_RAD;
break;
default:
HAL_ERROR1("Illegal BMX055 gyro FSR code %d\n", m_settings->m_BMX055GyroFsr);
return false;
}
return (m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_RANGE, m_settings->m_BMX055GyroFsr, "Failed to set BMX055 gyro rate"));
}
bool RTIMUBMX055::setAccelSampleRate()
{
unsigned char reg;
switch(m_settings->m_BMX055AccelSampleRate) {
case BMX055_ACCEL_SAMPLERATE_15:
reg = 0x08;
break;
case BMX055_ACCEL_SAMPLERATE_31:
reg = 0x09;
break;
case BMX055_ACCEL_SAMPLERATE_62:
reg = 0x0a;
break;
case BMX055_ACCEL_SAMPLERATE_125:
reg = 0x0b;
break;
case BMX055_ACCEL_SAMPLERATE_250:
reg = 0x0c;
break;
case BMX055_ACCEL_SAMPLERATE_500:
reg = 0x0d;
break;
case BMX055_ACCEL_SAMPLERATE_1000:
reg = 0x0e;
break;
case BMX055_ACCEL_SAMPLERATE_2000:
reg = 0x0f;
break;
default:
HAL_ERROR1("Illegal BMX055 accel FSR code %d\n", m_settings->m_BMX055AccelSampleRate);
return false;
}
return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_BW, reg, "Failed to set BMX055 accel rate"));
}
bool RTIMUBMX055::setAccelFSR()
{
unsigned char reg;
switch(m_settings->m_BMX055AccelFsr) {
case BMX055_ACCEL_FSR_2:
reg = 0x03;
m_accelScale = 0.00098 / 16.0;
break;
case BMX055_ACCEL_FSR_4:
reg = 0x05;
m_accelScale = 0.00195 / 16.0;
break;
case BMX055_ACCEL_FSR_8:
reg = 0x08;
m_accelScale = 0.00391 / 16.0;
break;
case BMX055_ACCEL_FSR_16:
reg = 0x0c;
m_accelScale = 0.00781 / 16.0;
break;
default:
HAL_ERROR1("Illegal BMX055 accel FSR code %d\n", m_settings->m_BMX055AccelFsr);
return false;
}
return (m_settings->HALWrite(m_accelSlaveAddr, BMX055_ACCEL_PMU_RANGE, reg, "Failed to set BMX055 accel rate"));
}
bool RTIMUBMX055::setMagPreset()
{
unsigned char mode, repXY, repZ;
switch (m_settings->m_BMX055MagPreset) {
case BMX055_MAG_LOW_POWER: // ODR=10, RepXY=3, RepZ=3
mode = 0;
repXY = 1;
repZ = 2;
break;
case BMX055_MAG_REGULAR: // ODR=10, RepXY=9, RepZ=15
mode = 0;
repXY = 4;
repZ = 14;
break;
case BMX055_MAG_ENHANCED: // ODR=10, RepXY=15, RepZ=27
mode = 0;
repXY = 7;
repZ = 26;
break;
case BMX055_MAG_HIGH_ACCURACY: // ODR=10, RepXY=47, RepZ=83
mode = 0;
repXY = 23;
repZ = 82;
break;
default:
HAL_ERROR1("Illegal BMX055 mag preset code %d\n", m_settings->m_BMX055MagPreset);
return false;
}
if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_MODE, mode, "Failed to set BMX055 mag mode"))
return false;
if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_REPXY, repXY, "Failed to set BMX055 mag repXY"))
return false;
if (!m_settings->HALWrite(m_magSlaveAddr, BMX055_MAG_REPZ, repZ, "Failed to set BMX055 mag repZ"))
return false;
return true;
}
int RTIMUBMX055::IMUGetPollInterval()
{
if (m_sampleRate > 400)
return 1;
else
return (400 / m_sampleRate);
}
bool RTIMUBMX055::IMURead()
{
unsigned char status;
unsigned char gyroData[6];
unsigned char accelData[6];
unsigned char magData[8];
if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_STATUS, 1, &status, "Failed to read BMX055 gyro fifo status"))
return false;
if (status & 0x80) {
// fifo overflowed
HAL_ERROR("BMX055 fifo overflow\n");
// this should clear it
if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config"))
return false;
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); // try to fix timestamp
return false;
}
if (status == 0)
return false;
if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_DATA, 6, gyroData, "Failed to read BMX055 gyro data"))
return false;
if (!m_settings->HALRead(m_accelSlaveAddr, BMX055_ACCEL_X_LSB, 6, accelData, "Failed to read BMX055 accel data"))
return false;
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_X_LSB, 8, magData, "Failed to read BMX055 mag data"))
return false;
RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
// need to prepare accel data
accelData[0] &= 0xf0;
accelData[2] &= 0xf0;
accelData[4] &= 0xf0;
RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
float mx, my, mz;
processMagData(magData, mx, my, mz);
m_imuData.compass.setX(mx);
m_imuData.compass.setY(my);
m_imuData.compass.setZ(mz);
// sort out gyro axes
m_imuData.gyro.setY(-m_imuData.gyro.y());
m_imuData.gyro.setZ(-m_imuData.gyro.z());
// sort out accel axes
m_imuData.accel.setX(-m_imuData.accel.x());
// sort out mag axes
m_imuData.compass.setY(-m_imuData.compass.y());
m_imuData.compass.setZ(-m_imuData.compass.z());
// now do standard processing
handleGyroBias();
calibrateAverageCompass();
calibrateAccel();
if (m_firstTime)
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
else
m_imuData.timestamp += m_sampleInterval;
m_firstTime = false;
// now update the filter
updateFusion();
return true;
}
/*!
* @brief This API reads remapped compensated Magnetometer
* data of X,Y,Z values
* @note The output value of compensated X, Y, Z as float
*
* @note In this function X and Y axis is remapped
* @note X is read from the address 0x44 & 0x45
* @note Y is read from the address 0x42 & 0x43
* @note this API is only applicable for BMX055 sensor
* *
*
*/
/****************************************************/
/**\name ARRAY PARAMETERS */
/***************************************************/
#define LSB_ZERO 0
#define MSB_ONE 1
#define LSB_TWO 2
#define MSB_THREE 3
#define LSB_FOUR 4
#define MSB_FIVE 5
#define LSB_SIX 6
#define MSB_SEVEN 7
/********************************************/
/**\name BIT MASK, LENGTH AND POSITION OF REMAPPED DATA REGISTERS */
/********************************************/
/* Data X LSB Remapped Register only applicable for BMX055 */
#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__POS 3
#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__LEN 5
#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__MSK 0xF8
#define BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX__REG\
BMM050_BMX055_REMAPPED_DATAX_LSB
/* Data Y LSB Remapped Register only applicable for BMX055 */
#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__POS 3
#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__LEN 5
#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__MSK 0xF8
#define BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY__REG\
BMM050_BMX055_REMAPPED_DATAY_LSB
/********************************************/
/**\name BIT MASK, LENGTH AND POSITION OF DATA REGISTERS */
/********************************************/
/* Data X LSB Register */
#define BMM050_DATAX_LSB_VALUEX__POS 3
#define BMM050_DATAX_LSB_VALUEX__LEN 5
#define BMM050_DATAX_LSB_VALUEX__MSK 0xF8
#define BMM050_DATAX_LSB_VALUEX__REG BMM050_DATAX_LSB
/* Data X SelfTest Register */
#define BMM050_DATAX_LSB_TESTX__POS 0
#define BMM050_DATAX_LSB_TESTX__LEN 1
#define BMM050_DATAX_LSB_TESTX__MSK 0x01
#define BMM050_DATAX_LSB_TESTX__REG BMM050_DATAX_LSB
/* Data Y LSB Register */
#define BMM050_DATAY_LSB_VALUEY__POS 3
#define BMM050_DATAY_LSB_VALUEY__LEN 5
#define BMM050_DATAY_LSB_VALUEY__MSK 0xF8
#define BMM050_DATAY_LSB_VALUEY__REG BMM050_DATAY_LSB
/* Data Y SelfTest Register */
#define BMM050_DATAY_LSB_TESTY__POS 0
#define BMM050_DATAY_LSB_TESTY__LEN 1
#define BMM050_DATAY_LSB_TESTY__MSK 0x01
#define BMM050_DATAY_LSB_TESTY__REG BMM050_DATAY_LSB
/* Data Z LSB Register */
#define BMM050_DATAZ_LSB_VALUEZ__POS 1
#define BMM050_DATAZ_LSB_VALUEZ__LEN 7
#define BMM050_DATAZ_LSB_VALUEZ__MSK 0xFE
#define BMM050_DATAZ_LSB_VALUEZ__REG BMM050_DATAZ_LSB
/* Data Z SelfTest Register */
#define BMM050_DATAZ_LSB_TESTZ__POS 0
#define BMM050_DATAZ_LSB_TESTZ__LEN 1
#define BMM050_DATAZ_LSB_TESTZ__MSK 0x01
#define BMM050_DATAZ_LSB_TESTZ__REG BMM050_DATAZ_LSB
/* Hall Resistance LSB Register */
#define BMM050_R_LSB_VALUE__POS 2
#define BMM050_R_LSB_VALUE__LEN 6
#define BMM050_R_LSB_VALUE__MSK 0xFC
#define BMM050_R_LSB_VALUE__REG BMM050_R_LSB
#define BMM050_DATA_RDYSTAT__POS 0
#define BMM050_DATA_RDYSTAT__LEN 1
#define BMM050_DATA_RDYSTAT__MSK 0x01
#define BMM050_DATA_RDYSTAT__REG BMM050_R_LSB
/********************************************/
/**\name GET AND SET BITSLICE FUNCTIONS */
/********************************************/
/* get bit slice */
#define BMM050_GET_BITSLICE(regvar, bitname)\
((regvar & bitname##__MSK) >> bitname##__POS)
/* Set bit slice */
#define BMM050_SET_BITSLICE(regvar, bitname, val)\
((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
/********************************************/
/**\name BIT SHIFTING DEFINITIONS */
/********************************************/
/*Shifting Constants*/
#define SHIFT_RIGHT_1_POSITION 1
#define SHIFT_RIGHT_2_POSITION 2
#define SHIFT_RIGHT_3_POSITION 3
#define SHIFT_RIGHT_4_POSITION 4
#define SHIFT_RIGHT_5_POSITION 5
#define SHIFT_RIGHT_6_POSITION 6
#define SHIFT_RIGHT_7_POSITION 7
#define SHIFT_RIGHT_8_POSITION 8
#define SHIFT_RIGHT_9_POSITION 9
#define SHIFT_RIGHT_12_POSITION 12
#define SHIFT_RIGHT_13_POSITION 13
#define SHIFT_RIGHT_16_POSITION 16
#define SHIFT_LEFT_1_POSITION 1
#define SHIFT_LEFT_2_POSITION 2
#define SHIFT_LEFT_3_POSITION 3
#define SHIFT_LEFT_4_POSITION 4
#define SHIFT_LEFT_5_POSITION 5
#define SHIFT_LEFT_6_POSITION 6
#define SHIFT_LEFT_7_POSITION 7
#define SHIFT_LEFT_8_POSITION 8
#define SHIFT_LEFT_14_POSITION 14
#define SHIFT_LEFT_15_POSITION 15
#define SHIFT_LEFT_16_POSITION 16
/****************************************************/
/**\name COMPENSATED FORMULA DEFINITIONS */
/***************************************************/
#define BMM050_HEX_FOUR_THOUSAND 0x4000
#define BMM050_HEX_ONE_LACK 0x100000
#define BMM050_HEX_A_ZERO 0xA0
#define BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR 16384.0f
#define BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA 268435456.0f
#define BMM050_FLOAT_1_6_3_8_4_DATA 16384.0f
#define BMM050_FLOAT_2_5_6_DATA 256.0f
#define BMM050_FLOAT_1_6_0_DATA 160.0f
#define BMM050_FLOAT_8_1_9_2_DATA 8192.0f
#define BMM050_FLOAT_EIGHT_DATA 8.0f
#define BMM050_FLOAT_SIXTEEN_DATA 16.0f
#define BMM050_FLOAT_1_3_1_0_7_2_DATA 131072.0f
#define BMM050_FLOAT_3_2_7_6_8_DATA 32768.0
#define BMM050_FLOAT_4_DATA 4.0
/********************************************/
/**\name ENABLE/DISABLE DEFINITIONS */
/********************************************/
#define BMM050_ZERO_U8X 0
#define BMM050_DISABLE 0
#define BMM050_ENABLE 1
#define BMM050_CHANNEL_DISABLE 1
#define BMM050_CHANNEL_ENABLE 0
#define BMM050_INTPIN_LATCH_ENABLE 1
#define BMM050_INTPIN_LATCH_DISABLE 0
#define BMM050_OFF 0
#define BMM050_ON 1
/********************************************/
/**\name OVERFLOW DEFINITIONS */
/********************************************/
/* compensated output value returned if sensor had overflow */
#define BMM050_OVERFLOW_OUTPUT -32768
#define BMM050_OVERFLOW_OUTPUT_S32 ((int32_t)(-2147483647-1))
#define BMM050_OVERFLOW_OUTPUT_FLOAT 0.0f
#define BMM050_FLIP_OVERFLOW_ADCVAL -4096
#define BMM050_HALL_OVERFLOW_ADCVAL -16384
/********************************************/
/**\name NUMERIC DEFINITIONS */
/********************************************/
#define C_BMM050_ZERO_U8X ((uint8_t)0)
#define C_BMM050_ONE_U8X ((uint8_t)1)
#define C_BMM050_TWO_U8X ((uint8_t)2)
#define C_BMM050_FOUR_U8X ((uint8_t)4)
#define C_BMM050_FIVE_U8X ((uint8_t)5)
#define C_BMM050_EIGHT_U8X ((uint8_t)8)
#define BMM0505_HEX_ZERO_ZERO 0x00
/* Conversion factors*/
#define BMM050_CONVFACTOR_LSB_UT 6
/********************************************/
/**\name BIT MASK, LENGTH AND POSITION OF TRIM REGISTER */
/********************************************/
/* Register 6D */
#define BMM050_DIG_XYZ1_MSB__POS 0
#define BMM050_DIG_XYZ1_MSB__LEN 7
#define BMM050_DIG_XYZ1_MSB__MSK 0x7F
#define BMM050_DIG_XYZ1_MSB__REG BMM050_DIG_XYZ1_MSB
void RTIMUBMX055::processMagData(unsigned char *v_data_uint8_t, float& magX, float& magY, float& magZ)
{
/* structure used to store the mag raw xyz and r data */
struct {
int16_t raw_data_x;
int16_t raw_data_y;
int16_t raw_data_z;
uint16_t raw_data_r;
} raw_data_xyz_t;
/* Reading data for Y axis */
v_data_uint8_t[LSB_ZERO] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_ZERO],
BMM050_BMX055_REMAPPED_DATAY_LSB_VALUEY);
raw_data_xyz_t.raw_data_y = (int16_t)((((int32_t)
((int8_t)v_data_uint8_t[MSB_ONE])) <<
SHIFT_LEFT_5_POSITION) | v_data_uint8_t[LSB_ZERO]);
/* Reading data for X axis */
v_data_uint8_t[LSB_TWO] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_TWO],
BMM050_BMX055_REMAPPED_DATAX_LSB_VALUEX);
raw_data_xyz_t.raw_data_x = (int16_t)((((int32_t)
((int8_t)v_data_uint8_t[MSB_THREE])) <<
SHIFT_LEFT_5_POSITION) | v_data_uint8_t[LSB_TWO]);
raw_data_xyz_t.raw_data_x = -raw_data_xyz_t.raw_data_x;
/* Reading data for Z axis */
v_data_uint8_t[LSB_FOUR] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_FOUR],
BMM050_DATAZ_LSB_VALUEZ);
raw_data_xyz_t.raw_data_z = (int16_t)((((int32_t)
((int8_t)v_data_uint8_t[MSB_FIVE])) <<
SHIFT_LEFT_7_POSITION) | v_data_uint8_t[LSB_FOUR]);
/* Reading data for Resistance*/
v_data_uint8_t[LSB_SIX] = BMM050_GET_BITSLICE(v_data_uint8_t[LSB_SIX],
BMM050_R_LSB_VALUE);
raw_data_xyz_t.raw_data_r = (uint16_t)((((uint32_t)
v_data_uint8_t[MSB_SEVEN]) <<
SHIFT_LEFT_6_POSITION) | v_data_uint8_t[LSB_SIX]);
/* Compensation for X axis */
magX = bmm050_compensate_X_float(
raw_data_xyz_t.raw_data_x,
raw_data_xyz_t.raw_data_r);
/* Compensation for Y axis */
magY = bmm050_compensate_Y_float(
raw_data_xyz_t.raw_data_y,
raw_data_xyz_t.raw_data_r);
/* Compensation for Z axis */
magZ = bmm050_compensate_Z_float(
raw_data_xyz_t.raw_data_z,
raw_data_xyz_t.raw_data_r);
/* Output raw resistance value */
// mag_data->resistance = raw_data_xyz_t.raw_data_r;
}
float RTIMUBMX055::bmm050_compensate_X_float(int16_t mag_data_x, uint16_t data_r)
{
float inter_retval = BMM050_ZERO_U8X;
if (mag_data_x != BMM050_FLIP_OVERFLOW_ADCVAL /* no overflow */
) {
if (data_r != C_BMM050_ZERO_U8X) {
inter_retval = ((((float)m_dig_xyz1)
* BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR
/data_r)
- BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR);
} else {
inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
return inter_retval;
}
inter_retval = (((mag_data_x * ((((((float)m_dig_xy2) *
(inter_retval*inter_retval /
BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA) +
inter_retval * ((float)m_dig_xy1)
/ BMM050_FLOAT_1_6_3_8_4_DATA))
+ BMM050_FLOAT_2_5_6_DATA) *
(((float)m_dig_x2) + BMM050_FLOAT_1_6_0_DATA)))
/ BMM050_FLOAT_8_1_9_2_DATA)
+ (((float)m_dig_x1) *
BMM050_FLOAT_EIGHT_DATA))/
BMM050_FLOAT_SIXTEEN_DATA;
} else {
inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
}
return inter_retval;
}
float RTIMUBMX055::bmm050_compensate_Y_float(int16_t mag_data_y, uint16_t data_r)
{
float inter_retval = BMM050_ZERO_U8X;
if (mag_data_y != BMM050_FLIP_OVERFLOW_ADCVAL /* no overflow */
) {
if (data_r != C_BMM050_ZERO_U8X) {
inter_retval = ((((float)m_dig_xyz1)
* BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR
/data_r) - BMM050_FLOAT_ONE_SIX_THREE_EIGHT_FOUR);
} else {
inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
return inter_retval;
}
inter_retval = (((mag_data_y * ((((((float)m_dig_xy2) *
(inter_retval*inter_retval
/ BMM050_FLOAT_2_6_8_4_3_5_4_5_6_DATA) +
inter_retval * ((float)m_dig_xy1)
/ BMM050_FLOAT_1_6_3_8_4_DATA)) +
BMM050_FLOAT_2_5_6_DATA) *
(((float)m_dig_y2) + BMM050_FLOAT_1_6_0_DATA)))
/ BMM050_FLOAT_8_1_9_2_DATA) +
(((float)m_dig_y1) * BMM050_FLOAT_EIGHT_DATA))
/ BMM050_FLOAT_SIXTEEN_DATA;
} else {
/* overflow, set output to 0.0f */
inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
}
return inter_retval;
}
float RTIMUBMX055::bmm050_compensate_Z_float (int16_t mag_data_z, uint16_t data_r)
{
float inter_retval = BMM050_ZERO_U8X;
/* no overflow */
if (mag_data_z != BMM050_HALL_OVERFLOW_ADCVAL) {
if ((m_dig_z2 != BMM050_ZERO_U8X)
&& (m_dig_z1 != BMM050_ZERO_U8X)
&& (data_r != BMM050_ZERO_U8X)) {
inter_retval = ((((((float)mag_data_z)-
((float)m_dig_z4))*
BMM050_FLOAT_1_3_1_0_7_2_DATA)-
(((float)m_dig_z3)*(((float)data_r)
-((float)m_dig_xyz1))))
/((((float)m_dig_z2)+
((float)m_dig_z1)*((float)data_r) /
BMM050_FLOAT_3_2_7_6_8_DATA)
* BMM050_FLOAT_4_DATA))
/ BMM050_FLOAT_SIXTEEN_DATA;
}
} else {
/* overflow, set output to 0.0f */
inter_retval = BMM050_OVERFLOW_OUTPUT_FLOAT;
}
return inter_retval;
}
bool RTIMUBMX055::magInitTrimRegisters()
{
unsigned char data[2];
data[0] = 0;
data[1] = 0;
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_X1, 1, (uint8_t *)&m_dig_x1, "Failed to read BMX055 mag trim x1"))
return false;
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Y1, 1, (uint8_t *)&m_dig_y1, "Failed to read BMX055 mag trim y1"))
return false;
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_X2, 1, (uint8_t *)&m_dig_x2, "Failed to read BMX055 mag trim x2"))
return false;
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Y2, 1, (uint8_t *)&m_dig_y2, "Failed to read BMX055 mag trim y2"))
return false;
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XY1, 1, &m_dig_xy1, "Failed to read BMX055 mag trim xy1"))
return false;
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XY2, 1, (uint8_t *)&m_dig_xy2, "Failed to read BMX055 mag trim xy2"))
return false;
/* shorts can not be recast into (uint8_t*)
* due to possible mix up between trim data
* arrangement and memory arrangement */
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z1_LSB, 2, data, "Failed to read BMX055 mag trim z1"))
return false;
m_dig_z1 = (uint16_t)((((uint32_t)((uint8_t)
data[MSB_ONE])) <<
SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z2_LSB, 2, data, "Failed to read BMX055 mag trim z2"))
return false;
m_dig_z2 = (int16_t)((((int32_t)(
(int8_t)data[MSB_ONE])) <<
SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z3_LSB, 2, data, "Failed to read BMX055 mag trim z3"))
return false;
m_dig_z3 = (int16_t)((((int32_t)(
(int8_t)data[MSB_ONE])) <<
SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_Z4_LSB, 2, data, "Failed to read BMX055 mag trim z4"))
return false;
m_dig_z4 = (int16_t)((((int32_t)(
(int8_t)data[MSB_ONE])) <<
SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_DIG_XYZ1_LSB, 2, data, "Failed to read BMX055 mag trim xyz1"))
return false;
data[MSB_ONE] = BMM050_GET_BITSLICE(data[MSB_ONE],
BMM050_DIG_XYZ1_MSB);
m_dig_xyz1 = (uint16_t)((((uint32_t)
((uint8_t)data[MSB_ONE])) <<
SHIFT_LEFT_8_POSITION) | data[LSB_ZERO]);
return true;
}
Reference in New Issue View Git Blame Copy Permalink