//////////////////////////////////////////////////////////////////////////// // // This file is part of RTIMULib // // Copyright (c) 2014-2015, richards-tech, LLC // // 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 #ifdef BMX055_REMAP m_imuData.compass.setY(-m_imuData.compass.y()); m_imuData.compass.setZ(-m_imuData.compass.z()); #else RTFLOAT temp = m_imuData.compass.x(); m_imuData.compass.setX(-m_imuData.compass.y()); m_imuData.compass.setY(-temp); m_imuData.compass.setZ(-m_imuData.compass.z()); #endif // 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<resistance = raw_data_xyz_t.raw_data_r; } #else void RTIMUBMX055::processMagData(unsigned char *v_data_u8, 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; int16_t raw_data_r; } raw_data_xyz_t; /* Reading data for X axis */ v_data_u8[LSB_ZERO] = BMM050_GET_BITSLICE(v_data_u8[LSB_ZERO], BMM050_DATAX_LSB_VALUEX); raw_data_xyz_t.raw_data_x = (int16_t)((((int32_t) ((int8_t)v_data_u8[MSB_ONE])) << SHIFT_LEFT_5_POSITION) | v_data_u8[LSB_ZERO]); /* Reading data for Y axis */ v_data_u8[LSB_TWO] = BMM050_GET_BITSLICE(v_data_u8[LSB_TWO], BMM050_DATAY_LSB_VALUEY); raw_data_xyz_t.raw_data_y = (int16_t)((((int32_t) ((int8_t)v_data_u8[MSB_THREE])) << SHIFT_LEFT_5_POSITION) | v_data_u8[LSB_TWO]); /* Reading data for Z axis */ v_data_u8[LSB_FOUR] = BMM050_GET_BITSLICE(v_data_u8[LSB_FOUR], BMM050_DATAZ_LSB_VALUEZ); raw_data_xyz_t.raw_data_z = (int16_t)((((int32_t) ((int8_t)v_data_u8[MSB_FIVE])) << SHIFT_LEFT_7_POSITION) | v_data_u8[LSB_FOUR]); /* Reading data for Resistance*/ v_data_u8[LSB_SIX] = BMM050_GET_BITSLICE(v_data_u8[LSB_SIX], BMM050_R_LSB_VALUE); raw_data_xyz_t.raw_data_r = (uint16_t)((((uint32_t) v_data_u8[MSB_SEVEN]) << SHIFT_LEFT_6_POSITION) | v_data_u8[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; } #endif 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; }