1 /* 2 * Copyright (C) 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include <string.h> 18 #include "bosch_bmm150_slave.h" 19 20 #define kScale_mag 0.0625f // 1.0f / 16.0f; 21 22 void bmm150SaveDigData(struct MagTask *magTask, uint8_t *data, size_t offset) 23 { 24 // magnetometer temperature calibration data is read in 3 bursts of 8 byte 25 // length each. 26 memcpy(&magTask->raw_dig_data[offset], data, 8); 27 28 if (offset == 16) { 29 // we have all the raw data. 30 31 static const size_t first_reg = BMM150_REG_DIG_X1; 32 magTask->dig_x1 = magTask->raw_dig_data[BMM150_REG_DIG_X1 - first_reg]; 33 magTask->dig_y1 = magTask->raw_dig_data[BMM150_REG_DIG_Y1 - first_reg]; 34 magTask->dig_x2 = magTask->raw_dig_data[BMM150_REG_DIG_X2 - first_reg]; 35 magTask->dig_y2 = magTask->raw_dig_data[BMM150_REG_DIG_Y2 - first_reg]; 36 magTask->dig_xy2 = magTask->raw_dig_data[BMM150_REG_DIG_XY2 - first_reg]; 37 magTask->dig_xy1 = magTask->raw_dig_data[BMM150_REG_DIG_XY1 - first_reg]; 38 39 magTask->dig_z1 = *(uint16_t *)(&magTask->raw_dig_data[BMM150_REG_DIG_Z1_LSB - first_reg]); 40 magTask->dig_z2 = *(int16_t *)(&magTask->raw_dig_data[BMM150_REG_DIG_Z2_LSB - first_reg]); 41 magTask->dig_z3 = *(int16_t *)(&magTask->raw_dig_data[BMM150_REG_DIG_Z3_LSB - first_reg]); 42 magTask->dig_z4 = *(int16_t *)(&magTask->raw_dig_data[BMM150_REG_DIG_Z4_LSB - first_reg]); 43 44 magTask->dig_xyz1 = *(uint16_t *)(&magTask->raw_dig_data[BMM150_REG_DIG_XYZ1_LSB - first_reg]); 45 } 46 } 47 48 static int32_t bmm150TempCompensateX(struct MagTask *magTask, int16_t mag_x, uint16_t rhall) 49 { 50 int32_t inter_retval = 0; 51 52 // some temp var to made the long calculation easier to read 53 int32_t temp_1, temp_2, temp_3, temp_4; 54 55 // no overflow 56 if (mag_x != BMM150_MAG_FLIP_OVERFLOW_ADCVAL) { 57 if ((rhall != 0) && (magTask->dig_xyz1 != 0)) { 58 59 inter_retval = ((int32_t)(((uint16_t) ((((int32_t)magTask->dig_xyz1) << 14) 60 / (rhall != 0 ? rhall : magTask->dig_xyz1))) - ((uint16_t)0x4000))); 61 62 } else { 63 inter_retval = BMM150_MAG_OVERFLOW_OUTPUT; 64 return inter_retval; 65 } 66 67 temp_1 = ((int32_t)magTask->dig_xy2) * ((((int32_t)inter_retval) * ((int32_t)inter_retval)) >> 7); 68 temp_2 = ((int32_t)inter_retval) * ((int32_t)(((int16_t)magTask->dig_xy1) << 7)); 69 temp_3 = ((temp_1 + temp_2) >> 9) + ((int32_t)BMM150_CALIB_HEX_LACKS); 70 temp_4 = ((int32_t)mag_x) * ((temp_3 * ((int32_t)(((int16_t)magTask->dig_x2) + ((int16_t)0xa0)))) >> 12); 71 72 inter_retval = ((int32_t)(temp_4 >> 13)) + (((int16_t)magTask->dig_x1) << 3); 73 74 // check the overflow output 75 if (inter_retval == (int32_t)BMM150_MAG_OVERFLOW_OUTPUT) 76 inter_retval = BMM150_MAG_OVERFLOW_OUTPUT_S32; 77 } else { 78 // overflow 79 inter_retval = BMM150_MAG_OVERFLOW_OUTPUT; 80 } 81 return inter_retval; 82 } 83 84 static int32_t bmm150TempCompensateY(struct MagTask *magTask, int16_t mag_y, uint16_t rhall) 85 { 86 int32_t inter_retval = 0; 87 88 // some temp var to made the long calculation easier to read 89 int32_t temp_1, temp_2, temp_3, temp_4; 90 91 // no overflow 92 if (mag_y != BMM150_MAG_FLIP_OVERFLOW_ADCVAL) { 93 if ((rhall != 0) && (magTask->dig_xyz1 != 0)) { 94 95 inter_retval = ((int32_t)(((uint16_t)((( (int32_t)magTask->dig_xyz1) << 14) 96 / (rhall != 0 ? rhall : magTask->dig_xyz1))) - ((uint16_t)0x4000))); 97 98 } else { 99 inter_retval = BMM150_MAG_OVERFLOW_OUTPUT; 100 return inter_retval; 101 } 102 103 temp_1 = ((int32_t)magTask->dig_xy2) * ((((int32_t) inter_retval) * ((int32_t)inter_retval)) >> 7); 104 temp_2 = ((int32_t)inter_retval) * ((int32_t)(((int16_t)magTask->dig_xy1) << 7)); 105 temp_3 = ((temp_1 + temp_2) >> 9) + ((int32_t)BMM150_CALIB_HEX_LACKS); 106 temp_4 = ((int32_t)mag_y) * ((temp_3 * ((int32_t)(((int16_t)magTask->dig_y2) + ((int16_t)0xa0)))) >> 12); 107 108 inter_retval = ((int32_t)(temp_4 >> 13)) + (((int16_t)magTask->dig_y1) << 3); 109 110 // check the overflow output 111 if (inter_retval == (int32_t)BMM150_MAG_OVERFLOW_OUTPUT) 112 inter_retval = BMM150_MAG_OVERFLOW_OUTPUT_S32; 113 } else { 114 // overflow 115 inter_retval = BMM150_MAG_OVERFLOW_OUTPUT; 116 } 117 return inter_retval; 118 } 119 120 static int32_t bmm150TempCompensateZ(struct MagTask *magTask, int16_t mag_z, uint16_t rhall) 121 { 122 int32_t retval = 0; 123 if (mag_z != BMM150_MAG_HALL_OVERFLOW_ADCVAL) { 124 if ((rhall != 0) && (magTask->dig_z2 != 0) && (magTask->dig_z1 != 0)) { 125 126 retval = ((((int32_t)(mag_z - magTask->dig_z4)) << 15) 127 - ((((int32_t)magTask->dig_z3) * ((int32_t)(((int16_t)rhall) - ((int16_t)magTask->dig_xyz1)))) >> 2)); 128 129 retval /= (magTask->dig_z2 130 + ((int16_t)(((((int32_t)magTask->dig_z1) * ((((int16_t)rhall) << 1))) + (1 << 15)) >> 16))); 131 } 132 } else { 133 retval = BMM150_MAG_OVERFLOW_OUTPUT; 134 } 135 return retval; 136 } 137 138 void parseMagData(struct MagTask *magTask, uint8_t *buf, float *x, float *y, float *z) { 139 int32_t mag_x = (*(int16_t *)&buf[0]) >> 3; 140 int32_t mag_y = (*(int16_t *)&buf[2]) >> 3; 141 int32_t mag_z = (*(int16_t *)&buf[4]) >> 1; 142 uint32_t mag_rhall = (*(uint16_t *)&buf[6]) >> 2; 143 144 int32_t raw_x = bmm150TempCompensateX(magTask, mag_x, mag_rhall); 145 int32_t raw_y = bmm150TempCompensateY(magTask, mag_y, mag_rhall); 146 int32_t raw_z = bmm150TempCompensateZ(magTask, mag_z, mag_rhall); 147 148 *x = (float)raw_x * kScale_mag; 149 *y = (float)raw_y * kScale_mag; 150 *z = (float)raw_z * kScale_mag; 151 } 152
