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 
bmm150SaveDigData(struct MagTask * magTask,uint8_t * data,size_t offset)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 
bmm150TempCompensateX(struct MagTask * magTask,int16_t mag_x,uint16_t rhall)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 
bmm150TempCompensateY(struct MagTask * magTask,int16_t mag_y,uint16_t rhall)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 
bmm150TempCompensateZ(struct MagTask * magTask,int16_t mag_z,uint16_t rhall)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 
parseMagData(struct MagTask * magTask,uint8_t * buf,float * x,float * y,float * z)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