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_SPL06.h"
16 :
17 : #if AP_BARO_SPL06_ENABLED
18 :
19 : #include <strings.h>
20 : #include <AP_Math/definitions.h>
21 :
22 : extern const AP_HAL::HAL &hal;
23 :
24 : #define SPL06_CHIP_ID 0x10
25 : #define SPA06_CHIP_ID 0x11
26 :
27 : #define SPL06_REG_PRESSURE_B2 0x00 // Pressure MSB Register
28 : #define SPL06_REG_PRESSURE_B1 0x01 // Pressure middle byte Register
29 : #define SPL06_REG_PRESSURE_B0 0x02 // Pressure LSB Register
30 : #define SPL06_REG_PRESSURE_START SPL06_REG_PRESSURE_B2
31 : #define SPL06_PRESSURE_LEN 3 // 24 bits, 3 bytes
32 : #define SPL06_REG_TEMPERATURE_B2 0x03 // Temperature MSB Register
33 : #define SPL06_REG_TEMPERATURE_B1 0x04 // Temperature middle byte Register
34 : #define SPL06_REG_TEMPERATURE_B0 0x05 // Temperature LSB Register
35 : #define SPL06_REG_TEMPERATURE_START SPL06_REG_TEMPERATURE_B2
36 : #define SPL06_TEMPERATURE_LEN 3 // 24 bits, 3 bytes
37 : #define SPL06_REG_PRESSURE_CFG 0x06 // Pressure config
38 : #define SPL06_REG_TEMPERATURE_CFG 0x07 // Temperature config
39 : #define SPL06_REG_MODE_AND_STATUS 0x08 // Mode and status
40 : #define SPL06_REG_INT_AND_FIFO_CFG 0x09 // Interrupt and FIFO config
41 : #define SPL06_REG_INT_STATUS 0x0A // Interrupt and FIFO config
42 : #define SPL06_REG_FIFO_STATUS 0x0B // Interrupt and FIFO config
43 : #define SPL06_REG_RST 0x0C // Softreset Register
44 : #define SPL06_REG_CHIP_ID 0x0D // Chip ID Register
45 : #define SPL06_REG_CALIB_COEFFS_START 0x10
46 : #define SPL06_REG_CALIB_COEFFS_END 0x21
47 : #define SPA06_REG_CALIB_COEFFS_END 0x24
48 :
49 : // number of calibration coefficient bytes for each device
50 : #define SPL06_NUM_CALIB_COEFFS (SPL06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1)
51 : #define SPA06_NUM_CALIB_COEFFS (SPA06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1)
52 :
53 : // MAX_NUM_CALIB_COEFFS is the largest of the per-device counts:
54 : #define MAX_NUM_CALIB_COEFFS SPA06_NUM_CALIB_COEFFS
55 :
56 : // PRESSURE_CFG_REG
57 : #define SPL06_PRES_RATE_32HZ (0x05 << 4)
58 :
59 : // TEMPERATURE_CFG_REG
60 : #define SPL06_TEMP_USE_EXT_SENSOR (1<<7)
61 : #define SPL06_TEMP_RATE_32HZ (0x05 << 4)
62 :
63 : // MODE_AND_STATUS_REG
64 : #define SPL06_MEAS_PRESSURE (1<<0) // measure pressure
65 : #define SPL06_MEAS_TEMPERATURE (1<<1) // measure temperature
66 : #define SPL06_MEAS_CON_PRE_TEM 0x07
67 :
68 : #define SPL06_MEAS_CFG_CONTINUOUS (1<<2)
69 : #define SPL06_MEAS_CFG_PRESSURE_RDY (1<<4)
70 : #define SPL06_MEAS_CFG_TEMPERATURE_RDY (1<<5)
71 : #define SPL06_MEAS_CFG_SENSOR_RDY (1<<6)
72 : #define SPL06_MEAS_CFG_COEFFS_RDY (1<<7)
73 :
74 : // INT_AND_FIFO_CFG_REG
75 : #define SPL06_PRESSURE_RESULT_BIT_SHIFT (1<<2) // necessary for pressure oversampling > 8
76 : #define SPL06_TEMPERATURE_RESULT_BIT_SHIFT (1<<3) // necessary for temperature oversampling > 8
77 :
78 : // Don't set oversampling higher than 8 or the measurement time will be higher than 20ms (timer period)
79 : #define SPL06_PRESSURE_OVERSAMPLING 8
80 : #define SPL06_TEMPERATURE_OVERSAMPLING 8
81 :
82 : #define SPL06_OVERSAMPLING_TO_REG_VALUE(n) (ffs(n)-1)
83 :
84 : #define SPL06_BACKGROUND_SAMPLE_RATE 32
85 :
86 : // enable Background Mode for continuous measurement
87 : #ifndef AP_BARO_SPL06_BACKGROUND_ENABLE
88 : #define AP_BARO_SPL06_BACKGROUND_ENABLE 1
89 : #endif
90 :
91 0 : AP_Baro_SPL06::AP_Baro_SPL06(AP_Baro &baro, AP_HAL::Device &dev)
92 : : AP_Baro_Backend(baro)
93 0 : , _dev(&dev)
94 : {
95 0 : }
96 :
97 0 : AP_Baro_Backend *AP_Baro_SPL06::probe(AP_Baro &baro, AP_HAL::Device &dev)
98 : {
99 0 : if (dev.bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
100 0 : dev.set_read_flag(0x80);
101 : }
102 :
103 0 : AP_Baro_SPL06 *sensor = NEW_NOTHROW AP_Baro_SPL06(baro, dev);
104 0 : if (!sensor || !sensor->_init()) {
105 0 : delete sensor;
106 0 : return nullptr;
107 : }
108 0 : return sensor;
109 : }
110 :
111 0 : bool AP_Baro_SPL06::_init()
112 : {
113 0 : if (!_dev) {
114 0 : return false;
115 : }
116 0 : WITH_SEMAPHORE(_dev->get_semaphore());
117 :
118 0 : _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
119 :
120 : uint8_t whoami;
121 :
122 : // Sometimes SPL06 has init problems, that's due to failure of reading using SPI for the first time. The SPL06 is a dual
123 : // protocol sensor(I2C and SPI), sometimes it takes one SPI operation to convert it to SPI mode after it starts up.
124 :
125 0 : for (uint8_t i=0; i<5; i++) {
126 0 : if (_dev->read_registers(SPL06_REG_CHIP_ID, &whoami, 1)) {
127 0 : switch(whoami) {
128 0 : case SPL06_CHIP_ID:
129 0 : type = Type::SPL06;
130 0 : break;
131 0 : case SPA06_CHIP_ID:
132 0 : type = Type::SPA06;
133 0 : break;
134 0 : default:
135 0 : type = Type::UNKNOWN;
136 0 : break;
137 : }
138 : }
139 :
140 0 : if (type != Type::UNKNOWN)
141 0 : break;
142 : }
143 :
144 0 : if (type == Type::UNKNOWN) {
145 0 : return false;
146 : }
147 :
148 : // number of calibration coefficient bytes to read for this device
149 0 : uint8_t calib_coeffs_len = SPL06_NUM_CALIB_COEFFS;
150 0 : switch(type) {
151 0 : case Type::SPL06:
152 0 : calib_coeffs_len = SPL06_NUM_CALIB_COEFFS;
153 0 : break;
154 0 : case Type::SPA06:
155 0 : calib_coeffs_len = SPA06_NUM_CALIB_COEFFS;
156 0 : break;
157 0 : default:
158 0 : break;
159 : }
160 :
161 0 : bool ready = false;
162 0 : for (uint8_t i=0; i<5; i++) {
163 0 : uint8_t status = 0;
164 0 : if (_dev->read_registers(SPL06_REG_MODE_AND_STATUS, &status, 1)) {
165 0 : if ((status & 1<<7U) && (status & 1<<6U)) {
166 0 : ready = true;
167 0 : break;
168 : }
169 : }
170 0 : hal.scheduler->delay_microseconds(100);
171 : }
172 :
173 0 : if (!ready) {
174 0 : return false;
175 : }
176 :
177 : // fixed-size buffer holding the largest device's coefficients; a
178 : // compile-time constant rather than a VLA sized by calib_coeffs_len
179 : uint8_t buf[MAX_NUM_CALIB_COEFFS];
180 :
181 : // fail safe if a device's length exceeds the buffer, e.g. a new device
182 : // type was added without growing MAX_NUM_CALIB_COEFFS above
183 0 : if (calib_coeffs_len > sizeof(buf)) {
184 0 : return false;
185 : }
186 :
187 : #define READ_LENGTH 9
188 :
189 : // only read this device's coefficients (calib_coeffs_len <= buffer size)
190 0 : for (uint8_t i = 0; i < calib_coeffs_len; ) {
191 0 : ssize_t chunk = MIN(READ_LENGTH, calib_coeffs_len - i);
192 0 : if (!_dev->read_registers(SPL06_REG_CALIB_COEFFS_START + i, buf + i, chunk)) {
193 0 : return false;
194 : }
195 0 : i += chunk;
196 : }
197 :
198 : // 0x11 c0 [3:0] + 0x10 c0 [11:4]
199 0 : _c0 = get_twos_complement(((uint32_t)buf[0] << 4) | (((uint32_t)buf[1] >> 4) & 0x0F), 12);
200 : // 0x11 c1 [11:8] + 0x12 c1 [7:0]
201 0 : _c1 = get_twos_complement((((uint32_t)buf[1] & 0x0F) << 8) | (uint32_t)buf[2], 12);
202 : // 0x13 c00 [19:12] + 0x14 c00 [11:4] + 0x15 c00 [3:0]
203 0 : _c00 = get_twos_complement(((uint32_t)buf[3] << 12) | ((uint32_t)buf[4] << 4) | (((uint32_t)buf[5] >> 4) & 0x0F), 20);
204 : // 0x15 c10 [19:16] + 0x16 c10 [15:8] + 0x17 c10 [7:0]
205 0 : _c10 = get_twos_complement((((uint32_t)buf[5] & 0x0F) << 16) | ((uint32_t)buf[6] << 8) | (uint32_t)buf[7], 20);
206 : // 0x18 c01 [15:8] + 0x19 c01 [7:0]
207 0 : _c01 = get_twos_complement(((uint32_t)buf[8] << 8) | (uint32_t)buf[9], 16);
208 : // 0x1A c11 [15:8] + 0x1B c11 [7:0]
209 0 : _c11 = get_twos_complement(((uint32_t)buf[10] << 8) | (uint32_t)buf[11], 16);
210 : // 0x1C c20 [15:8] + 0x1D c20 [7:0]
211 0 : _c20 = get_twos_complement(((uint32_t)buf[12] << 8) | (uint32_t)buf[13], 16);
212 : // 0x1E c21 [15:8] + 0x1F c21 [7:0]
213 0 : _c21 = get_twos_complement(((uint32_t)buf[14] << 8) | (uint32_t)buf[15], 16);
214 : // 0x20 c30 [15:8] + 0x21 c30 [7:0]
215 0 : _c30 = get_twos_complement(((uint32_t)buf[16] << 8) | (uint32_t)buf[17], 16);
216 :
217 0 : if(type == Type::SPA06) {
218 : // 0x23 c31 [3:0] + 0x22 c31 [11:4]
219 0 : _c31 = get_twos_complement(((uint32_t)buf[18] << 4) | (((uint32_t)buf[19] >> 4) & 0x0F), 12);
220 : // 0x23 c40 [11:8] + 0x24 c40 [7:0]
221 0 : _c40 = get_twos_complement((((uint32_t)buf[19] & 0x0F) << 8) | (uint32_t)buf[20], 12);
222 : }
223 :
224 0 : const uint8_t tmp_sensor = (type == Type::SPA06 ? 0 : SPL06_TEMP_USE_EXT_SENSOR);
225 : #if AP_BARO_SPL06_BACKGROUND_ENABLE
226 : // setup temperature and pressure measurements
227 0 : _dev->setup_checked_registers(4, 20);
228 :
229 : //set rate and oversampling
230 0 : _dev->write_register(SPL06_REG_TEMPERATURE_CFG, tmp_sensor | SPL06_TEMP_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true);
231 0 : _dev->write_register(SPL06_REG_PRESSURE_CFG, SPL06_PRES_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_PRESSURE_OVERSAMPLING), true);
232 :
233 : //enable background mode
234 0 : _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_CON_PRE_TEM, true);
235 : #else
236 : // setup temperature and pressure measurements
237 : _dev->setup_checked_registers(3, 20);
238 :
239 : _dev->write_register(SPL06_REG_TEMPERATURE_CFG, tmp_sensor | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true);
240 : _dev->write_register(SPL06_REG_PRESSURE_CFG, SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_PRESSURE_OVERSAMPLING), true);
241 : #endif //AP_BARO_SPL06_BACKGROUND_ENABLE
242 :
243 0 : uint8_t int_and_fifo_reg_value = 0;
244 : if (SPL06_TEMPERATURE_OVERSAMPLING > 8) {
245 : int_and_fifo_reg_value |= SPL06_TEMPERATURE_RESULT_BIT_SHIFT;
246 : }
247 : if (SPL06_PRESSURE_OVERSAMPLING > 8) {
248 : int_and_fifo_reg_value |= SPL06_PRESSURE_RESULT_BIT_SHIFT;
249 : }
250 0 : _dev->write_register(SPL06_REG_INT_AND_FIFO_CFG, int_and_fifo_reg_value, true);
251 :
252 0 : _instance = _frontend.register_sensor();
253 :
254 0 : if(type == Type::SPA06) {
255 0 : _dev->set_device_type(DEVTYPE_BARO_SPA06);
256 : } else {
257 0 : _dev->set_device_type(DEVTYPE_BARO_SPL06);
258 : }
259 :
260 0 : set_bus_id(_instance, _dev->get_bus_id());
261 :
262 : // request 50Hz update
263 0 : _timer_counter = -1;
264 : #if AP_BARO_SPL06_BACKGROUND_ENABLE
265 0 : _dev->register_periodic_callback(1000000/SPL06_BACKGROUND_SAMPLE_RATE, FUNCTOR_BIND_MEMBER(&AP_Baro_SPL06::_timer, void));
266 : #else
267 : _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_SPL06::_timer, void));
268 : #endif //AP_BARO_SPL06_BACKGROUND_ENABLE
269 :
270 0 : return true;
271 0 : }
272 :
273 0 : int32_t AP_Baro_SPL06::raw_value_scale_factor(uint8_t oversampling)
274 : {
275 : // From the datasheet page 13
276 0 : switch(oversampling)
277 : {
278 0 : case 1: return 524288;
279 0 : case 2: return 1572864;
280 0 : case 4: return 3670016;
281 0 : case 8: return 7864320;
282 0 : case 16: return 253952;
283 0 : case 32: return 516096;
284 0 : case 64: return 1040384;
285 0 : case 128: return 2088960;
286 0 : default: return -1; // invalid
287 : }
288 : }
289 :
290 : // accumulate a new sensor reading
291 0 : void AP_Baro_SPL06::_timer(void)
292 : {
293 : uint8_t buf[3];
294 :
295 : #if AP_BARO_SPL06_BACKGROUND_ENABLE
296 0 : _dev->read_registers(SPL06_REG_TEMPERATURE_START, buf, sizeof(buf));
297 0 : _update_temperature((int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2]));
298 :
299 0 : _dev->read_registers(SPL06_REG_PRESSURE_START, buf, sizeof(buf));
300 0 : _update_pressure((int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2]));
301 : #else
302 : //command mode
303 : if ((_timer_counter == -1) || (_timer_counter == 49)) {
304 : // First call and every second start a temperature measurement (50Hz call)
305 : _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_TEMPERATURE, false);
306 : _timer_counter = 0; // Next cycle we are reading the temperature
307 : } else if (_timer_counter == 0) {
308 : // A temperature measurement had been started during the previous call
309 : _dev->read_registers(SPL06_REG_TEMPERATURE_START, buf, sizeof(buf));
310 : _update_temperature((int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2]));
311 : _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_PRESSURE, false);
312 : _timer_counter += 1;
313 : } else {
314 : // The rest of the time read the latest pressure and start a new measurement
315 : _dev->read_registers(SPL06_REG_PRESSURE_START, buf, sizeof(buf));
316 : _update_pressure((int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2]));
317 : _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_PRESSURE, false);
318 : _timer_counter += 1;
319 : }
320 : #endif //AP_BARO_SPL06_BACKGROUND_ENABLE
321 :
322 0 : _dev->check_next_register();
323 0 : }
324 :
325 : // transfer data to the frontend
326 0 : void AP_Baro_SPL06::update(void)
327 : {
328 0 : WITH_SEMAPHORE(_sem);
329 :
330 0 : if (_pressure_count == 0) {
331 0 : return;
332 : }
333 :
334 0 : _copy_to_frontend(_instance, _pressure_sum/_pressure_count, _temperature);
335 :
336 0 : _pressure_sum = 0;
337 0 : _pressure_count = 0;
338 0 : }
339 :
340 : // calculate temperature
341 0 : void AP_Baro_SPL06::_update_temperature(int32_t temp_raw)
342 : {
343 0 : _temp_raw = (float)temp_raw / raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING);
344 0 : const float temp_comp = (float)_c0 / 2 + _temp_raw * _c1;
345 :
346 0 : WITH_SEMAPHORE(_sem);
347 :
348 0 : _temperature = temp_comp;
349 0 : }
350 :
351 : // calculate pressure
352 0 : void AP_Baro_SPL06::_update_pressure(int32_t press_raw)
353 : {
354 0 : const float press_raw_sc = (float)press_raw / raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING);
355 0 : float pressure_cal = 0;
356 0 : float press_temp_comp = 0;
357 :
358 0 : switch(type) {
359 0 : case Type::SPL06:
360 0 : pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * _c30));
361 0 : press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * _c21));
362 0 : break;
363 0 : case Type::SPA06:
364 0 : pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * ((float)_c30 + press_raw_sc * _c40)));
365 0 : press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * ((float)_c21) + press_raw_sc * _c31));
366 0 : break;
367 0 : default:
368 0 : break;
369 : }
370 :
371 0 : const float press_comp = pressure_cal + press_temp_comp;
372 :
373 0 : if (!pressure_ok(press_comp)) {
374 0 : return;
375 : }
376 :
377 0 : WITH_SEMAPHORE(_sem);
378 :
379 0 : _pressure_sum += press_comp;
380 0 : _pressure_count++;
381 : }
382 :
383 : #endif // AP_BARO_SPL06_ENABLED
|
