Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : #include "AP_Baro_BMP388.h"
16 :
17 : #if AP_BARO_BMP388_ENABLED
18 :
19 : #include <AP_Math/AP_Math.h>
20 :
21 : extern const AP_HAL::HAL &hal;
22 :
23 : #define BMP388_MODE_SLEEP 0
24 : #define BMP388_MODE_FORCED 1
25 : #define BMP388_MODE_NORMAL 3
26 : #define BMP388_MODE BMP388_MODE_NORMAL
27 :
28 : #define BMP388_ID 0x50
29 : #define BMP390_ID 0x60
30 :
31 : #define BMP388_REG_ID 0x00
32 : #define BMP388_REV_ID_ADDR 0x01
33 : #define BMP388_REG_ERR 0x02
34 : #define BMP388_REG_STATUS 0x03
35 : #define BMP388_REG_PRESS 0x04 // 24 bit
36 : #define BMP388_REG_TEMP 0x07 // 24 bit
37 : #define BMP388_REG_TIME 0x0C // 24 bit
38 : #define BMP388_REG_EVENT 0x10
39 : #define BMP388_REG_INT_STS 0x11
40 : #define BMP388_REG_FIFO_LEN 0x12 // 9 bit
41 : #define BMP388_REG_FIFO_DATA 0x14
42 : #define BMP388_REG_FIFO_WTMK 0x15 // 9 bit
43 : #define BMP388_REG_FIFO_CNF1 0x17
44 : #define BMP388_REG_FIFO_CNF2 0x18
45 : #define BMP388_REG_INT_CTRL 0x19
46 : #define BMP388_REG_PWR_CTRL 0x1B
47 : #define BMP388_REG_OSR 0x1C
48 : #define BMP388_REG_ODR 0x1D
49 : #define BMP388_REG_CONFIG 0x1F
50 : #define BMP388_REG_CMD 0x7E
51 :
52 : #define BMP388_REG_CAL_P 0x36
53 : #define BMP388_REG_CAL_T 0x31
54 :
55 : #define BMP388_SOFT_RESET 0xB6
56 :
57 0 : AP_Baro_BMP388::AP_Baro_BMP388(AP_Baro &baro, AP_HAL::Device &_dev)
58 : : AP_Baro_Backend(baro)
59 0 : , dev(&_dev)
60 : {
61 0 : }
62 :
63 0 : AP_Baro_Backend *AP_Baro_BMP388::probe(AP_Baro &baro, AP_HAL::Device &_dev)
64 : {
65 0 : AP_Baro_BMP388 *sensor = NEW_NOTHROW AP_Baro_BMP388(baro, _dev);
66 0 : if (!sensor || !sensor->init()) {
67 0 : delete sensor;
68 0 : return nullptr;
69 : }
70 0 : return sensor;
71 : }
72 :
73 0 : bool AP_Baro_BMP388::init()
74 : {
75 0 : if (!dev) {
76 0 : return false;
77 : }
78 0 : WITH_SEMAPHORE(dev->get_semaphore());
79 :
80 0 : dev->set_speed(AP_HAL::Device::SPEED_HIGH);
81 :
82 : // setup to allow reads on SPI
83 0 : if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
84 0 : dev->set_read_flag(0x80);
85 : }
86 : uint8_t whoami;
87 0 : if (!read_registers(BMP388_REG_ID, &whoami, 1)) {
88 0 : return false;
89 : }
90 :
91 0 : switch (whoami) {
92 0 : case BMP388_ID:
93 0 : dev->set_device_type(DEVTYPE_BARO_BMP388);
94 0 : break;
95 0 : case BMP390_ID:
96 0 : dev->set_device_type(DEVTYPE_BARO_BMP390);
97 0 : break;
98 0 : default:
99 0 : return false;
100 : }
101 :
102 : // BMP390 needs a soft reset to start converting
103 0 : if (!dev->write_register(BMP388_REG_CMD, BMP388_SOFT_RESET)) {
104 0 : return false;
105 : }
106 0 : hal.scheduler->delay(10);
107 :
108 : // normal mode, temp and pressure
109 0 : dev->write_register(BMP388_REG_PWR_CTRL, 0x33, true);
110 :
111 : // read the calibration data
112 0 : read_registers(BMP388_REG_CAL_P, (uint8_t *)&calib_p, sizeof(calib_p));
113 0 : read_registers(BMP388_REG_CAL_T, (uint8_t *)&calib_t, sizeof(calib_t));
114 :
115 0 : scale_calibration_data();
116 :
117 0 : dev->setup_checked_registers(4);
118 :
119 : // normal mode, temp and pressure
120 0 : dev->write_register(BMP388_REG_PWR_CTRL, 0x33, true);
121 :
122 0 : instance = _frontend.register_sensor();
123 :
124 0 : set_bus_id(instance, dev->get_bus_id());
125 :
126 : // request 50Hz update
127 0 : dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP388::timer, void));
128 :
129 0 : return true;
130 0 : }
131 :
132 : // accumulate a new sensor reading
133 0 : void AP_Baro_BMP388::timer(void)
134 : {
135 : uint8_t buf[7];
136 :
137 0 : if (!read_registers(BMP388_REG_STATUS, buf, sizeof(buf))) {
138 0 : return;
139 : }
140 :
141 : // inexplicably, some versions of GCC believe buf dies after a
142 : // call. Retrieve what we need from it beforehand to avoid a
143 : // warning.
144 0 : const uint8_t status = buf[0];
145 0 : const uint32_t pressure_data = (buf[3] << 16) | (buf[2] << 8) | buf[1];
146 0 : const uint32_t temperature_data = (buf[6] << 16) | (buf[5] << 8) | buf[4];
147 0 : if ((status & 0x20) != 0) {
148 : // we have pressure data
149 0 : update_pressure(pressure_data);
150 : }
151 0 : if ((status & 0x40) != 0) {
152 : // we have temperature data
153 0 : update_temperature(temperature_data);
154 : }
155 :
156 0 : dev->check_next_register();
157 : }
158 :
159 : // transfer data to the frontend
160 0 : void AP_Baro_BMP388::update(void)
161 : {
162 0 : WITH_SEMAPHORE(_sem);
163 :
164 0 : if (pressure_count == 0) {
165 0 : return;
166 : }
167 :
168 0 : _copy_to_frontend(instance,
169 0 : pressure_sum/pressure_count,
170 : temperature);
171 :
172 0 : pressure_sum = 0;
173 0 : pressure_count = 0;
174 0 : }
175 :
176 : /*
177 : convert calibration data from NVM values to values ready for
178 : compensation calculations
179 : */
180 0 : void AP_Baro_BMP388::scale_calibration_data(void)
181 : {
182 : // note that this assumes little-endian MCU
183 0 : calib.par_t1 = calib_t.nvm_par_t1 * 256.0;
184 0 : calib.par_t2 = calib_t.nvm_par_t2 / 1073741824.0f;
185 0 : calib.par_t3 = calib_t.nvm_par_t3 / 281474976710656.0f;
186 :
187 0 : calib.par_p1 = (calib_p.nvm_par_p1 - 16384) / 1048576.0f;
188 0 : calib.par_p2 = (calib_p.nvm_par_p2 - 16384) / 536870912.0f;
189 0 : calib.par_p3 = calib_p.nvm_par_p3 / 4294967296.0f;
190 0 : calib.par_p4 = calib_p.nvm_par_p4 / 137438953472.0;
191 0 : calib.par_p5 = calib_p.nvm_par_p5 * 8.0f;
192 0 : calib.par_p6 = calib_p.nvm_par_p6 / 64.0;
193 0 : calib.par_p7 = calib_p.nvm_par_p7 / 256.0f;
194 0 : calib.par_p8 = calib_p.nvm_par_p8 / 32768.0f;
195 0 : calib.par_p9 = calib_p.nvm_par_p9 / 281474976710656.0f;
196 0 : calib.par_p10 = calib_p.nvm_par_p10 / 281474976710656.0f;
197 0 : calib.par_p11 = calib_p.nvm_par_p11 / 36893488147419103232.0f;
198 0 : }
199 :
200 : /*
201 : update temperature from raw sample
202 : */
203 0 : void AP_Baro_BMP388::update_temperature(uint32_t data)
204 : {
205 0 : float partial1 = data - calib.par_t1;
206 0 : float partial2 = partial1 * calib.par_t2;
207 :
208 0 : WITH_SEMAPHORE(_sem);
209 0 : temperature = partial2 + sq(partial1) * calib.par_t3;
210 0 : }
211 :
212 : /*
213 : update pressure from raw pressure data
214 : */
215 0 : void AP_Baro_BMP388::update_pressure(uint32_t data)
216 : {
217 0 : float partial1 = calib.par_p6 * temperature;
218 0 : float partial2 = calib.par_p7 * powf(temperature, 2);
219 0 : float partial3 = calib.par_p8 * powf(temperature, 3);
220 0 : float partial_out1 = calib.par_p5 + partial1 + partial2 + partial3;
221 :
222 0 : partial1 = calib.par_p2 * temperature;
223 0 : partial2 = calib.par_p3 * powf(temperature, 2);
224 0 : partial3 = calib.par_p4 * powf(temperature, 3);
225 0 : float partial_out2 = data * (calib.par_p1 + partial1 + partial2 + partial3);
226 :
227 0 : partial1 = powf(data, 2);
228 0 : partial2 = calib.par_p9 + calib.par_p10 * temperature;
229 0 : partial3 = partial1 * partial2;
230 0 : float partial4 = partial3 + powf(data, 3) * calib.par_p11;
231 0 : float press = partial_out1 + partial_out2 + partial4;
232 :
233 0 : WITH_SEMAPHORE(_sem);
234 :
235 0 : pressure_sum += press;
236 0 : pressure_count++;
237 0 : }
238 :
239 : /*
240 : read registers, special SPI handling needed
241 : */
242 0 : bool AP_Baro_BMP388::read_registers(uint8_t reg, uint8_t *data, uint8_t len)
243 : {
244 : // when on I2C we just read normally
245 0 : if (dev->bus_type() != AP_HAL::Device::BUS_TYPE_SPI) {
246 0 : return dev->read_registers(reg, data, len);
247 : }
248 : // for SPI we need to discard the first returned byte. See
249 : // datasheet for explanation
250 0 : uint8_t b[len+2];
251 0 : b[0] = reg | 0x80;
252 0 : memset(&b[1], 0, len+1);
253 0 : if (!dev->transfer_fullduplex(b, len+2)) {
254 0 : return false;
255 : }
256 0 : memcpy(data, &b[2], len);
257 0 : return true;
258 0 : }
259 :
260 : #endif // AP_BARO_BMP388_ENABLED
|
