1 /*
2 * Copyright (C) 2015-2021 Alibaba Group Holding Limited
3 */
4
5 #include <math.h>
6 #include "drv_baro_qst_qmp6988.h"
7 #include "aos/hal/i2c.h"
8 #include "ulog/ulog.h"
9 #include <stdio.h>
10 #include <stdlib.h>
11
12 #define QMP6988_LOG
13 #define QMP6988_ERR printf
14
15 #if !defined(QMP6988_CALC_INT)
16 static float const Conv_A_S[10][2] = {
17 {-6.30E-03, 4.30E-04},
18 {-1.90E-11, 1.20E-10},
19 {1.00E-01, 9.10E-02},
20 {1.20E-08, 1.20E-06},
21 {3.30E-02, 1.90E-02},
22 {2.10E-07, 1.40E-07},
23 {-6.30E-10, 3.50E-10},
24 {2.90E-13, 7.60E-13},
25 {2.10E-15, 1.20E-14},
26 {1.30E-16, 7.90E-17},
27 };
28 #endif
29
qmp6988_delay(unsigned int delay)30 static void qmp6988_delay(unsigned int delay)
31 {
32 int i, j;
33 for (i = 0; i < delay; i++) {
34 for (j = 0; j < 1000; j++) {
35 ;
36 }
37 }
38 }
39
qmp6988_WriteReg(uint8_t slave,uint8_t reg_add,uint8_t reg_dat)40 uint8_t qmp6988_WriteReg(uint8_t slave, uint8_t reg_add, uint8_t reg_dat)
41 {
42 #if defined(QST_USE_SPI)
43 return qmp6988_spi_write(reg_add, reg_dat);
44 #elif defined(QST_USE_SW_I2C)
45 return qst_sw_writereg(slave, reg_add, reg_dat);
46 #else
47 uint8_t write_buf[2];
48 write_buf[0] = reg_add;
49 write_buf[1] = reg_dat;
50 sensor_i2c_master_send(1, slave, write_buf, 2, 1000);
51 #endif
52 }
53
qmp6988_ReadData(uint8_t slave,uint8_t reg_add,unsigned char * Read,uint8_t num)54 uint8_t qmp6988_ReadData(uint8_t slave, uint8_t reg_add, unsigned char *Read, uint8_t num)
55 {
56 #if defined(QST_USE_SPI)
57 return qmp6988_spi_read(reg_add, Read, num);
58 #elif defined(QST_USE_SW_I2C)
59 return qst_sw_readreg(slave, reg_add, Read, num);
60 #else
61 uint8_t data;
62 sensor_i2c_master_send(1, slave, ®_add, 1, 1000);
63 aos_msleep(2);
64 sensor_i2c_master_recv(1, slave, Read, num, 1000);
65 return 1;
66 #endif
67 }
68
qmp6988_device_check(qmp6988_data * qmp6988)69 static uint8_t qmp6988_device_check(qmp6988_data *qmp6988)
70 {
71 uint8_t slave_addr[1] = {QMP6988_SLAVE_ADDRESS_H};
72 uint8_t ret = 0;
73 uint8_t i;
74
75 for (i = 0; i < 1; i++) {
76 qmp6988->slave = slave_addr[i];
77 ret = qmp6988_ReadData(qmp6988->slave, QMP6988_CHIP_ID_REG, &(qmp6988->chip_id), 1);
78 if (ret == 0) {
79 QMP6988_LOG("%s: read 0xD1 failed\n", __func__);
80 continue;
81 }
82 QMP6988_LOG("qmp6988 read chip id = 0x%x\n", qmp6988->chip_id);
83 if (qmp6988->chip_id == QMP6988_CHIP_ID) {
84 return 1;
85 }
86 }
87
88 return 0;
89 }
90
qmp6988_get_calibration_data(qmp6988_data * qmp6988)91 static int qmp6988_get_calibration_data(qmp6988_data *qmp6988)
92 {
93 int status = 0;
94 // BITFIELDS temp_COE;
95 uint8_t a_data_u8r[QMP6988_CALIBRATION_DATA_LENGTH] = {0};
96 int len;
97
98 for (len = 0; len < QMP6988_CALIBRATION_DATA_LENGTH; len += 1) {
99 // if((qmp6988_cali_data_LENGTH-len) >= 8 )
100 // status = qmp6988_ReadData(qmp6988_cali_data_START+len,&a_data_u8r[len],8);
101 // else
102 // status = qmp6988_ReadData(qmp6988_cali_data_START+len,&a_data_u8r[len],(qmp6988_cali_data_LENGTH-len));
103 status = qmp6988_ReadData(qmp6988->slave, QMP6988_CALIBRATION_DATA_START + len, &a_data_u8r[len], 1);
104 if (status == 0) {
105 QMP6988_LOG("qmp6988 read 0xA0 error!");
106 return status;
107 }
108 }
109
110 qmp6988->qmp6988_cali.COE_a0 = (QMP6988_S32_t)(((a_data_u8r[18] << SHIFT_LEFT_12_POSITION) \
111 | (a_data_u8r[19] << SHIFT_LEFT_4_POSITION) \
112 | (a_data_u8r[24] & 0x0f)) << 12);
113 qmp6988->qmp6988_cali.COE_a0 = qmp6988->qmp6988_cali.COE_a0 >> 12;
114
115 qmp6988->qmp6988_cali.COE_a1 = (QMP6988_S16_t)(((a_data_u8r[20]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[21]);
116 qmp6988->qmp6988_cali.COE_a2 = (QMP6988_S16_t)(((a_data_u8r[22]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[23]);
117
118 qmp6988->qmp6988_cali.COE_b00 = (QMP6988_S32_t)(((a_data_u8r[0] << SHIFT_LEFT_12_POSITION) \
119 | (a_data_u8r[1] << SHIFT_LEFT_4_POSITION) \
120 | ((a_data_u8r[24] & 0xf0) >> SHIFT_RIGHT_4_POSITION)) << 12);
121 qmp6988->qmp6988_cali.COE_b00 = qmp6988->qmp6988_cali.COE_b00 >> 12;
122
123 qmp6988->qmp6988_cali.COE_bt1 = (QMP6988_S16_t)(((a_data_u8r[2]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[3]);
124 qmp6988->qmp6988_cali.COE_bt2 = (QMP6988_S16_t)(((a_data_u8r[4]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[5]);
125 qmp6988->qmp6988_cali.COE_bp1 = (QMP6988_S16_t)(((a_data_u8r[6]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[7]);
126 qmp6988->qmp6988_cali.COE_b11 = (QMP6988_S16_t)(((a_data_u8r[8]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[9]);
127 qmp6988->qmp6988_cali.COE_bp2 = (QMP6988_S16_t)(((a_data_u8r[10]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[11]);
128 qmp6988->qmp6988_cali.COE_b12 = (QMP6988_S16_t)(((a_data_u8r[12]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[13]);
129 qmp6988->qmp6988_cali.COE_b21 = (QMP6988_S16_t)(((a_data_u8r[14]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[15]);
130 qmp6988->qmp6988_cali.COE_bp3 = (QMP6988_S16_t)(((a_data_u8r[16]) << SHIFT_LEFT_8_POSITION) | a_data_u8r[17]);
131
132 QMP6988_LOG("<-----------calibration data-------------->\n");
133 QMP6988_LOG("COE_a0[%d] COE_a1[%d] COE_a2[%d] COE_b00[%d]\n", \
134 qmp6988->qmp6988_cali.COE_a0, qmp6988->qmp6988_cali.COE_a1, qmp6988->qmp6988_cali.COE_a2, qmp6988->qmp6988_cali.COE_b00);
135 QMP6988_LOG("COE_bt1[%d] COE_bt2[%d] COE_bp1[%d] COE_b11[%d]\n", \
136 qmp6988->qmp6988_cali.COE_bt1, qmp6988->qmp6988_cali.COE_bt2, qmp6988->qmp6988_cali.COE_bp1, qmp6988->qmp6988_cali.COE_b11);
137 QMP6988_LOG("COE_bp2[%d] COE_b12[%d] COE_b21[%d] COE_bp3[%d]\n", \
138 qmp6988->qmp6988_cali.COE_bp2, qmp6988->qmp6988_cali.COE_b12, qmp6988->qmp6988_cali.COE_b21, qmp6988->qmp6988_cali.COE_bp3);
139 QMP6988_LOG("<-----------calibration data-------------->\n");
140
141 #if defined(QMP6988_CALC_INT)
142 qmp6988->ik.a0 = qmp6988->qmp6988_cali.COE_a0; // 20Q4
143 qmp6988->ik.b00 = qmp6988->qmp6988_cali.COE_b00; // 20Q4
144
145 qmp6988->ik.a1 = 3608L * (QMP6988_S32_t)qmp6988->qmp6988_cali.COE_a1 - 1731677965L; // 31Q23
146 qmp6988->ik.a2 = 16889L * (QMP6988_S32_t) qmp6988->qmp6988_cali.COE_a2 - 87619360L; // 30Q47
147
148 qmp6988->ik.bt1 = 2982L * (QMP6988_S64_t)qmp6988->qmp6988_cali.COE_bt1 + 107370906L; // 28Q15
149 qmp6988->ik.bt2 = 329854L * (QMP6988_S64_t)qmp6988->qmp6988_cali.COE_bt2 + 108083093L; // 34Q38
150 qmp6988->ik.bp1 = 19923L * (QMP6988_S64_t)qmp6988->qmp6988_cali.COE_bp1 + 1133836764L; // 31Q20
151 qmp6988->ik.b11 = 2406L * (QMP6988_S64_t)qmp6988->qmp6988_cali.COE_b11 + 118215883L; // 28Q34
152 qmp6988->ik.bp2 = 3079L * (QMP6988_S64_t)qmp6988->qmp6988_cali.COE_bp2 - 181579595L; // 29Q43
153 qmp6988->ik.b12 = 6846L * (QMP6988_S64_t)qmp6988->qmp6988_cali.COE_b12 + 85590281L; // 29Q53
154 qmp6988->ik.b21 = 13836L * (QMP6988_S64_t)qmp6988->qmp6988_cali.COE_b21 + 79333336L; // 29Q60
155 qmp6988->ik.bp3 = 2915L * (QMP6988_S64_t)qmp6988->qmp6988_cali.COE_bp3 + 157155561L; // 28Q65
156 /*
157 QMP6988_LOG("<----------- int calibration data -------------->\n");
158 QMP6988_LOG("a0[%d] a1[%d] a2[%d] b00[%d]\n", qmp6988->ik.a0, qmp6988->ik.a1, qmp6988->ik.a2, qmp6988->ik.b00);
159 QMP6988_LOG("bt1[%lld] bt2[%lld] bp1[%lld] b11[%lld]\n", qmp6988->ik.bt1, qmp6988->ik.bt2, qmp6988->ik.bp1, qmp6988->ik.b11);
160 QMP6988_LOG("bp2[%lld] b12[%lld] b21[%lld] bp3[%lld]\n", qmp6988->ik.bp2, qmp6988->ik.b12, qmp6988->ik.b21, qmp6988->ik.bp3);
161 QMP6988_LOG("<----------- int calibration data -------------->\n");
162 */
163 #else
164 qmp6988->fk.a0 = qmp6988->qmp6988_cali.COE_a0 / 16.0f;
165 qmp6988->fk.b00 = qmp6988->qmp6988_cali.COE_b00 / 16.0f;
166
167 qmp6988->fk.a1 = Conv_A_S[0][0] + Conv_A_S[0][1] * qmp6988->qmp6988_cali.COE_a1 / 32767.0f;
168 qmp6988->fk.a2 = Conv_A_S[1][0] + Conv_A_S[1][1] * qmp6988->qmp6988_cali.COE_a2 / 32767.0f;
169 qmp6988->fk.bt1 = Conv_A_S[2][0] + Conv_A_S[2][1] * qmp6988->qmp6988_cali.COE_bt1 / 32767.0f;
170 qmp6988->fk.bt2 = Conv_A_S[3][0] + Conv_A_S[3][1] * qmp6988->qmp6988_cali.COE_bt2 / 32767.0f;
171 qmp6988->fk.bp1 = Conv_A_S[4][0] + Conv_A_S[4][1] * qmp6988->qmp6988_cali.COE_bp1 / 32767.0f;
172 qmp6988->fk.b11 = Conv_A_S[5][0] + Conv_A_S[5][1] * qmp6988->qmp6988_cali.COE_b11 / 32767.0f;
173 qmp6988->fk.bp2 = Conv_A_S[6][0] + Conv_A_S[6][1] * qmp6988->qmp6988_cali.COE_bp2 / 32767.0f;
174 qmp6988->fk.b12 = Conv_A_S[7][0] + Conv_A_S[7][1] * qmp6988->qmp6988_cali.COE_b12 / 32767.0f;
175 qmp6988->fk.b21 = Conv_A_S[8][0] + Conv_A_S[8][1] * qmp6988->qmp6988_cali.COE_b21 / 32767.0f;
176 qmp6988->fk.bp3 = Conv_A_S[9][0] + Conv_A_S[9][1] * qmp6988->qmp6988_cali.COE_bp3 / 32767.0f;
177 /*
178 QMP6988_LOG("<----------- float calibration data -------------->\n");
179 QMP6988_LOG("a0[%lle] a1[%lle] a2[%lle] b00[%lle]\n", qmp6988->fk.a0, qmp6988->fk.a1, qmp6988->fk.a2, qmp6988->fk.b00);
180 QMP6988_LOG("bt1[%lle] bt2[%lle] bp1[%lle] b11[%lle]\n", qmp6988->fk.bt1, qmp6988->fk.bt2, qmp6988->fk.bp1, qmp6988->fk.b11);
181 QMP6988_LOG("bp2[%lle] b12[%lle] b21[%lle] bp3[%lle]\n", qmp6988->fk.bp2, qmp6988->fk.b12, qmp6988->fk.b21, qmp6988->fk.bp3);
182 QMP6988_LOG("<----------- float calibration data -------------->\n");
183 */
184 #endif
185
186 return 1;
187 }
188
189 #if defined(QMP6988_CALC_INT)
qmp6988_convTx_02e(qmp6988_ik_data * ik,QMP6988_S32_t dt)190 static QMP6988_S16_t qmp6988_convTx_02e(qmp6988_ik_data *ik, QMP6988_S32_t dt)
191 {
192 QMP6988_S16_t ret;
193 QMP6988_S64_t wk1, wk2;
194
195 // wk1: 60Q4 // bit size
196 wk1 = ((QMP6988_S64_t)ik->a1 * (QMP6988_S64_t)dt); // 31Q23+24-1=54 (54Q23)
197 wk2 = ((QMP6988_S64_t)ik->a2 * (QMP6988_S64_t)dt) >> 14; // 30Q47+24-1=53 (39Q33)
198 wk2 = (wk2 * (QMP6988_S64_t)dt) >> 10; // 39Q33+24-1=62 (52Q23)
199 wk2 = ((wk1 + wk2) / 32767) >> 19; // 54,52->55Q23 (20Q04)
200 ret = (QMP6988_S16_t)((ik->a0 + wk2) >> 4); // 21Q4 -> 17Q0
201 return ret;
202 }
203
qmp6988_get_pressure_02e(qmp6988_ik_data * ik,QMP6988_S32_t dp,QMP6988_S16_t tx)204 static QMP6988_S32_t qmp6988_get_pressure_02e(qmp6988_ik_data *ik, QMP6988_S32_t dp, QMP6988_S16_t tx)
205 {
206 QMP6988_S32_t ret;
207 QMP6988_S64_t wk1, wk2, wk3;
208
209 // wk1 = 48Q16 // bit size
210 wk1 = ((QMP6988_S64_t)ik->bt1 * (QMP6988_S64_t)tx); // 28Q15+16-1=43 (43Q15)
211 wk2 = ((QMP6988_S64_t)ik->bp1 * (QMP6988_S64_t)dp) >> 5; // 31Q20+24-1=54 (49Q15)
212 wk1 += wk2; // 43,49->50Q15
213 wk2 = ((QMP6988_S64_t)ik->bt2 * (QMP6988_S64_t)tx) >> 1; // 34Q38+16-1=49 (48Q37)
214 wk2 = (wk2 * (QMP6988_S64_t)tx) >> 8; // 48Q37+16-1=63 (55Q29)
215 wk3 = wk2; // 55Q29
216 wk2 = ((QMP6988_S64_t)ik->b11 * (QMP6988_S64_t)tx) >> 4; // 28Q34+16-1=43 (39Q30)
217 wk2 = (wk2 * (QMP6988_S64_t)dp) >> 1; // 39Q30+24-1=62 (61Q29)
218 wk3 += wk2; // 55,61->62Q29
219 wk2 = ((QMP6988_S64_t)ik->bp2 * (QMP6988_S64_t)dp) >> 13; // 29Q43+24-1=52 (39Q30)
220 wk2 = (wk2 * (QMP6988_S64_t)dp) >> 1; // 39Q30+24-1=62 (61Q29)
221 wk3 += wk2; // 62,61->63Q29
222 wk1 += wk3 >> 14; // Q29 >> 14 -> Q15
223 wk2 = ((QMP6988_S64_t)ik->b12 * (QMP6988_S64_t)tx); // 29Q53+16-1=45 (45Q53)
224 wk2 = (wk2 * (QMP6988_S64_t)tx) >> 22; // 45Q53+16-1=61 (39Q31)
225 wk2 = (wk2 * (QMP6988_S64_t)dp) >> 1; // 39Q31+24-1=62 (61Q30)
226 wk3 = wk2; // 61Q30
227 wk2 = ((QMP6988_S64_t)ik->b21 * (QMP6988_S64_t)tx) >> 6; // 29Q60+16-1=45 (39Q54)
228 wk2 = (wk2 * (QMP6988_S64_t)dp) >> 23; // 39Q54+24-1=62 (39Q31)
229 wk2 = (wk2 * (QMP6988_S64_t)dp) >> 1; // 39Q31+24-1=62 (61Q20)
230 wk3 += wk2; // 61,61->62Q30
231 wk2 = ((QMP6988_S64_t)ik->bp3 * (QMP6988_S64_t)dp) >> 12; // 28Q65+24-1=51 (39Q53)
232 wk2 = (wk2 * (QMP6988_S64_t)dp) >> 23; // 39Q53+24-1=62 (39Q30)
233 wk2 = (wk2 * (QMP6988_S64_t)dp); // 39Q30+24-1=62 (62Q30)
234 wk3 += wk2; // 62,62->63Q30
235 wk1 += wk3 >> 15; // Q30 >> 15 = Q15
236 wk1 /= 32767L;
237 wk1 >>= 11; // Q15 >> 7 = Q4
238 wk1 += ik->b00; // Q4 + 20Q4
239 // wk1 >>= 4; // 28Q4 -> 24Q0
240 ret = (QMP6988_S32_t)wk1;
241 return ret;
242 }
243 #endif
244
qmp6988_software_reset(qmp6988_data * qmp6988)245 static void qmp6988_software_reset(qmp6988_data *qmp6988)
246 {
247 /*
248 uint8_t ret = 0;
249
250 ret = qmp6988_WriteReg(qmp6988->slave, QMP6988_RESET_REG, 0xe6);
251 if (ret == 0) {
252 QMP6988_LOG("qmp6988_software_reset fail!!!\n");
253 }
254 qmp6988_delay(20);
255 ret = qmp6988_WriteReg(qmp6988->slave, QMP6988_RESET_REG, 0x00);
256 */
257 }
258
qmp6988_set_powermode(qmp6988_data * qmp6988,int power_mode)259 static void qmp6988_set_powermode(qmp6988_data *qmp6988, int power_mode)
260 {
261 uint8_t data;
262
263 QMP6988_LOG("qmp_set_powermode %d\n", power_mode);
264 // if(power_mode == qmp6988->power_mode)
265 // return;
266
267 qmp6988->power_mode = power_mode;
268 qmp6988_ReadData(qmp6988->slave, QMP6988_CTRLMEAS_REG, &data, 1);
269 data = data & 0xfc;
270 if (power_mode == QMP6988_SLEEP_MODE) {
271 data |= 0x00;
272 } else if (power_mode == QMP6988_FORCED_MODE) {
273 data |= 0x01;
274 } else if (power_mode == QMP6988_NORMAL_MODE) {
275 data |= 0x03;
276 }
277 qmp6988_WriteReg(qmp6988->slave, QMP6988_CTRLMEAS_REG, data);
278
279 QMP6988_LOG("qmp_set_powermode 0xf4=0x%x\n", data);
280
281 qmp6988_delay(20);
282 }
283
284
qmp6988_set_filter(qmp6988_data * qmp6988,unsigned char filter)285 static void qmp6988_set_filter(qmp6988_data *qmp6988, unsigned char filter)
286 {
287 uint8_t data;
288
289 data = (filter & 0x03);
290 qmp6988_WriteReg(qmp6988->slave, QMP6988_CONFIG_REG, data);
291
292 qmp6988_delay(20);
293 }
294
qmp6988_set_oversampling_p(qmp6988_data * qmp6988,unsigned char oversampling_p)295 static void qmp6988_set_oversampling_p(qmp6988_data *qmp6988, unsigned char oversampling_p)
296 {
297 uint8_t data;
298
299 qmp6988_ReadData(qmp6988->slave, QMP6988_CTRLMEAS_REG, &data, 1);
300 data &= 0xe3;
301 data |= (oversampling_p << 2);
302 qmp6988_WriteReg(qmp6988->slave, QMP6988_CTRLMEAS_REG, data);
303 qmp6988_delay(20);
304 }
305
qmp6988_set_oversampling_t(qmp6988_data * qmp6988,unsigned char oversampling_t)306 static void qmp6988_set_oversampling_t(qmp6988_data *qmp6988, unsigned char oversampling_t)
307 {
308 uint8_t data;
309
310 qmp6988_ReadData(qmp6988->slave, QMP6988_CTRLMEAS_REG, &data, 1);
311 data &= 0x1f;
312 data |= (oversampling_t << 5);
313 qmp6988_WriteReg(qmp6988->slave, QMP6988_CTRLMEAS_REG, data);
314 qmp6988_delay(20);
315 }
316
qmp6988_calc_altitude(float pressure,float temp)317 float qmp6988_calc_altitude(float pressure, float temp)
318 {
319 float altitude;
320
321 altitude = (pow((101325 / pressure), 1 / 5.257) - 1) * (temp + 273.15) / 0.0065;
322 // QMP6988_LOG("altitude = %f\n" ,altitude);
323 return altitude;
324 }
325
qmp6988_calc_pressure(qmp6988_data * qmp6988)326 float qmp6988_calc_pressure(qmp6988_data *qmp6988)
327 {
328 uint8_t err = 0;
329 // uint8_t retry_count = 0;
330 QMP6988_U32_t P_read, T_read;
331 QMP6988_S32_t P_raw, T_raw;
332 uint8_t a_data_u8r[6] = {0};
333 #if defined(QMP6988_CALC_INT)
334 QMP6988_S32_t T_int, P_int;
335 #else
336 float a0, b00;
337 float a1, a2, bt1, bt2, bp1, b11, bp2, b12, b21, bp3;
338 double Tr;
339 #endif
340
341 /*
342 a_data_u8r[0] = 0x08;
343 retry_count = 0;
344
345 while (a_data_u8r[0] & 0x08) {
346 err = qmp6988_ReadData(QMP6988_DEVICE_STAT_REG, a_data_u8r, 1);
347 if (err == 0) {
348 QMP6988_LOG("qmp6988 read status reg error!\n");
349 return;
350 }
351 QMP6988_LOG("qmp6988 read status 0xf3 = 0x%02x\n", a_data_u8r[0]);
352 qmp6988_delay(10);
353 retry_count++;
354 if (retry_count > 5)
355 return;
356 }
357 */
358 // press
359 err = qmp6988_ReadData(qmp6988->slave, QMP6988_PRESSURE_MSB_REG, a_data_u8r, 6);
360 if (err == 0) {
361 QMP6988_LOG("qmp6988 read press raw error!\n");
362 return 0.0f;
363 }
364 P_read = (QMP6988_U32_t)(
365 (((QMP6988_U32_t)(a_data_u8r[0])) << SHIFT_LEFT_16_POSITION) |
366 (((QMP6988_U16_t)(a_data_u8r[1])) << SHIFT_LEFT_8_POSITION) | (a_data_u8r[2]));
367 P_raw = (QMP6988_S32_t)(P_read - SUBTRACTOR);
368
369 /*
370 // temp
371 err = qmp6988_ReadData(QMP6988_TEMPERATURE_MSB_REG, a_data_u8r, 3);
372 if (err == 0) {
373 QMP6988_LOG("qmp6988 read temp raw error!\n");
374 }
375 */
376
377 T_read = (QMP6988_U32_t)(
378 (((QMP6988_U32_t)(a_data_u8r[3])) << SHIFT_LEFT_16_POSITION) |
379 (((QMP6988_U16_t)(a_data_u8r[4])) << SHIFT_LEFT_8_POSITION) | (a_data_u8r[5]));
380 T_raw = (QMP6988_S32_t)(T_read - SUBTRACTOR);
381
382 #if defined(QMP6988_CALC_INT)
383 T_int = qmp6988_convTx_02e(&(qmp6988->ik), T_raw);
384 P_int = qmp6988_get_pressure_02e(&(qmp6988->ik), P_raw, T_int);
385 qmp6988->temperature = (float)T_int / 256.0f;
386 qmp6988->pressure = (float)P_int / 16.0f;
387 // QMP6988_LOG("int temp=%f Pressure = %f\n",qmp6988->temperature, qmp6988->pressure);
388 #else
389 a0 = qmp6988->fk.a0;
390 b00 = qmp6988->fk.b00;
391 a1 = qmp6988->fk.a1;
392 a2 = qmp6988->fk.a2;
393 bt1 = qmp6988->fk.bt1;
394 bt2 = qmp6988->fk.bt2;
395 bp1 = qmp6988->fk.bp1;
396 b11 = qmp6988->fk.b11;
397 bp2 = qmp6988->fk.bp2;
398 b12 = qmp6988->fk.b12;
399 b21 = qmp6988->fk.b21;
400 bp3 = qmp6988->fk.bp3;
401
402 Tr = a0 + (a1 * T_raw) + (a2 * T_raw) *T_raw;
403 // Unit centigrade
404 qmp6988->temperature = Tr / 256.0f;
405 // compensation pressure, Unit Pa
406 qmp6988->pressure = b00 + (bt1 * Tr) + (bp1 * P_raw) + (b11 * Tr) *P_raw + (bt2 * Tr) *Tr + (bp2 * P_raw) *P_raw + (b12 * P_raw) *(Tr *Tr) + (b21 * P_raw) *(P_raw *Tr) + (bp3 * P_raw) *(P_raw *P_raw);
407 QMP6988_LOG("float temp=%f Pressure = %f\n", qmp6988->temperature, qmp6988->pressure);
408 #endif
409 qmp6988->altitude = qmp6988_calc_altitude(qmp6988->pressure, qmp6988->temperature);
410 // QMP6988_LOG("altitude = %f\n",qmp6988->altitude);
411 return qmp6988->pressure;
412 }
413
qmp6988_init(qmp6988_data * qmp6988)414 uint8_t qmp6988_init(qmp6988_data *qmp6988)
415 {
416 int32_t ret = sensor_i2c_open(1, QMP6988_SLAVE_ADDRESS_H, I2C_BUS_BIT_RATES_100K, 0);
417 if (ret) {
418 printf("sensor i2c open failed, ret:%d\n", ret);
419 return;
420 }
421
422 ret = qmp6988_device_check(qmp6988);
423 if (ret == 0) {
424 return 0;
425 }
426
427 qmp6988_software_reset(qmp6988);
428 qmp6988_get_calibration_data(qmp6988);
429 qmp6988_set_powermode(qmp6988, QMP6988_NORMAL_MODE);
430 qmp6988_set_filter(qmp6988, QMP6988_FILTERCOEFF_OFF);
431 qmp6988_set_oversampling_p(qmp6988, QMP6988_OVERSAMPLING_2X);
432 qmp6988_set_oversampling_t(qmp6988, QMP6988_OVERSAMPLING_1X);
433
434 return 1;
435 }
436
qmp6988_deinit(void)437 void qmp6988_deinit(void)
438 {
439 int32_t ret = sensor_i2c_close(1);
440 if (ret) {
441 LOGE("SENSOR", "sensor i2c close failed, ret:%d\n", ret);
442 }
443 return;
444 }
445