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 : /*
16 : DPS280 barometer driver
17 : */
18 :
19 : #include "AP_Baro_DPS280.h"
20 :
21 : #if AP_BARO_DPS280_ENABLED
22 :
23 : #include <stdio.h>
24 : #include <AP_Math/definitions.h>
25 :
26 : extern const AP_HAL::HAL &hal;
27 :
28 : #define DPS280_REG_PRESS 0x00
29 : #define DPS280_REG_TEMP 0x03
30 : #define DPS280_REG_PCONF 0x06
31 : #define DPS280_REG_TCONF 0x07
32 : #define DPS280_REG_MCONF 0x08
33 : #define DPS280_REG_CREG 0x09
34 : #define DPS280_REG_ISTS 0x0A
35 : #define DPS280_REG_FSTS 0x0B
36 : #define DPS280_REG_RESET 0x0C
37 : #define DPS280_REG_PID 0x0D
38 : #define DPS280_REG_COEF 0x10
39 : #define DPS280_REG_CSRC 0x28
40 :
41 : #define DPS280_WHOAMI 0x10
42 :
43 : #define TEMPERATURE_LIMIT_C 120
44 :
45 0 : AP_Baro_DPS280::AP_Baro_DPS280(AP_Baro &baro, AP_HAL::Device &_dev)
46 : : AP_Baro_Backend(baro)
47 0 : , dev(&_dev)
48 : {
49 0 : }
50 :
51 0 : AP_Baro_Backend *AP_Baro_DPS280::probe(AP_Baro &baro,
52 : AP_HAL::Device &_dev)
53 : {
54 0 : AP_Baro_DPS280 *sensor = NEW_NOTHROW AP_Baro_DPS280(baro, _dev);
55 0 : if (!sensor || !sensor->init()) {
56 0 : delete sensor;
57 0 : return nullptr;
58 : }
59 0 : return sensor;
60 : }
61 :
62 0 : AP_Baro_Backend *AP_Baro_DPS310::probe(AP_Baro &baro, AP_HAL::Device &_dev)
63 : {
64 0 : auto *sensor = NEW_NOTHROW AP_Baro_DPS310(baro, _dev);
65 0 : if (!sensor || !sensor->init()) {
66 0 : delete sensor;
67 0 : return nullptr;
68 : }
69 0 : return sensor;
70 : }
71 :
72 : /*
73 : handle bit width for 16 bit config registers
74 : */
75 0 : void AP_Baro_DPS280::fix_config_bits16(int16_t &v, uint8_t bits) const
76 : {
77 0 : if (v > int16_t((1U<<(bits-1))-1)) {
78 0 : v = v - (1U<<bits);
79 : }
80 0 : }
81 :
82 : /*
83 : handle bit width for 32 bit config registers
84 : */
85 0 : void AP_Baro_DPS280::fix_config_bits32(int32_t &v, uint8_t bits) const
86 : {
87 0 : if (v > int32_t((1U<<(bits-1))-1)) {
88 0 : v = v - (1U<<bits);
89 : }
90 0 : }
91 :
92 : /*
93 : read calibration data
94 : */
95 0 : bool AP_Baro_DPS280::read_calibration(void)
96 : {
97 : uint8_t buf[18];
98 :
99 0 : if (!dev->read_registers(DPS280_REG_COEF, buf, 18)) {
100 0 : return false;
101 : }
102 :
103 0 : calibration.C0 = (buf[0] << 4) + ((buf[1] >>4) & 0x0F);
104 0 : calibration.C1 = (buf[2] + ((buf[1] & 0x0F)<<8));
105 0 : calibration.C00 = ((buf[4]<<4) + (buf[3]<<12)) + ((buf[5]>>4) & 0x0F);
106 0 : calibration.C10 = ((buf[5] & 0x0F)<<16) + buf[7] + (buf[6]<<8);
107 0 : calibration.C01 = (buf[9] + (buf[8]<<8));
108 0 : calibration.C11 = (buf[11] + (buf[10]<<8));
109 0 : calibration.C20 = (buf[13] + (buf[12]<<8));
110 0 : calibration.C21 = (buf[15] + (buf[14]<<8));
111 0 : calibration.C30 = (buf[17] + (buf[16]<<8));
112 :
113 0 : fix_config_bits16(calibration.C0, 12);
114 0 : fix_config_bits16(calibration.C1, 12);
115 0 : fix_config_bits32(calibration.C00, 20);
116 0 : fix_config_bits32(calibration.C10, 20);
117 0 : fix_config_bits16(calibration.C01, 16);
118 0 : fix_config_bits16(calibration.C11, 16);
119 0 : fix_config_bits16(calibration.C20, 16);
120 0 : fix_config_bits16(calibration.C21, 16);
121 0 : fix_config_bits16(calibration.C30, 16);
122 :
123 : /* get calibration source */
124 0 : if (!dev->read_registers(DPS280_REG_CSRC, &calibration.temp_source, 1)) {
125 0 : return false;
126 : }
127 0 : calibration.temp_source &= 0x80;
128 :
129 0 : return true;
130 : }
131 :
132 0 : void AP_Baro_DPS280::set_config_registers(void)
133 : {
134 0 : dev->write_register(DPS280_REG_CREG, 0x0C, true); // shift for 16x oversampling
135 0 : dev->write_register(DPS280_REG_PCONF, 0x54, true); // 32 Hz, 16x oversample
136 0 : dev->write_register(DPS280_REG_TCONF, 0x54 | calibration.temp_source, true); // 32 Hz, 16x oversample
137 0 : dev->write_register(DPS280_REG_MCONF, 0x07); // continuous temp and pressure.
138 0 : }
139 :
140 0 : void AP_Baro_DPS310::set_config_registers(void)
141 : {
142 0 : AP_Baro_DPS280::set_config_registers();
143 :
144 : // work around broken temperature handling on some sensors
145 : // using undocumented register writes
146 : // see https://github.com/infineon/DPS310-Pressure-Sensor/blob/dps310/src/DpsClass.cpp#L442
147 0 : dev->write_register(0x0E, 0xA5);
148 0 : dev->write_register(0x0F, 0x96);
149 0 : dev->write_register(0x62, 0x02);
150 0 : dev->write_register(0x0E, 0x00);
151 0 : dev->write_register(0x0F, 0x00);
152 0 : }
153 :
154 0 : bool AP_Baro_DPS280::init()
155 : {
156 0 : if (!dev) {
157 0 : return false;
158 : }
159 0 : dev->get_semaphore()->take_blocking();
160 :
161 : // setup to allow reads on SPI
162 0 : if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
163 0 : dev->set_read_flag(0x80);
164 : }
165 :
166 0 : dev->set_speed(AP_HAL::Device::SPEED_HIGH);
167 :
168 : // the DPS310 can get into a state on boot where the whoami is not
169 : // read correctly at startup. Toggling the CS line gets its out of
170 : // this state
171 0 : dev->set_chip_select(true);
172 0 : dev->set_chip_select(false);
173 :
174 0 : uint8_t whoami=0;
175 0 : if (!dev->read_registers(DPS280_REG_PID, &whoami, 1) ||
176 0 : whoami != DPS280_WHOAMI) {
177 0 : dev->get_semaphore()->give();
178 0 : return false;
179 : }
180 :
181 0 : if (!read_calibration()) {
182 0 : dev->get_semaphore()->give();
183 0 : return false;
184 : }
185 :
186 0 : dev->setup_checked_registers(4, 20);
187 :
188 0 : set_config_registers();
189 :
190 0 : instance = _frontend.register_sensor();
191 :
192 0 : dev->set_device_type(device_type());
193 0 : set_bus_id(instance, dev->get_bus_id());
194 :
195 0 : dev->get_semaphore()->give();
196 :
197 : // request 64Hz update. New data will be available at 32Hz
198 0 : dev->register_periodic_callback((1000 / 64) * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_DPS280::timer, void));
199 :
200 0 : return true;
201 : }
202 :
203 : /*
204 : calculate corrected pressure and temperature
205 : */
206 0 : void AP_Baro_DPS280::calculate_PT(int32_t UT, int32_t UP, float &pressure, float &temperature)
207 : {
208 0 : const struct dps280_cal &cal = calibration;
209 : // scaling for 16x oversampling
210 0 : const float scaling_16 = 1.0f/253952;
211 :
212 : float temp_scaled;
213 : float press_scaled;
214 :
215 0 : temp_scaled = float(UT) * scaling_16;
216 0 : temperature = cal.C0 * 0.5f + cal.C1 * temp_scaled;
217 :
218 0 : press_scaled = float(UP) * scaling_16;
219 :
220 0 : pressure = cal.C00;
221 0 : pressure += press_scaled * (cal.C10 + press_scaled * (cal.C20 + press_scaled * cal.C30));
222 0 : pressure += temp_scaled * cal.C01;
223 0 : pressure += temp_scaled * press_scaled * (cal.C11 + press_scaled * cal.C21);
224 0 : }
225 :
226 : /*
227 : check health and possibly reset
228 : */
229 0 : void AP_Baro_DPS280::check_health(void)
230 : {
231 0 : dev->check_next_register();
232 :
233 0 : if (fabsf(last_temperature) > TEMPERATURE_LIMIT_C) {
234 0 : err_count++;
235 : }
236 0 : if (err_count > 16) {
237 0 : err_count = 0;
238 0 : dev->write_register(DPS280_REG_RESET, 0x09);
239 0 : set_config_registers();
240 0 : pending_reset = true;
241 : }
242 0 : }
243 :
244 : // accumulate a new sensor reading
245 0 : void AP_Baro_DPS280::timer(void)
246 : {
247 : uint8_t buf[6];
248 : uint8_t ready;
249 :
250 0 : if (pending_reset) {
251 : // reset registers after software reset from check_health()
252 0 : pending_reset = false;
253 0 : set_config_registers();
254 0 : return;
255 : }
256 :
257 0 : if (!dev->read_registers(DPS280_REG_MCONF, &ready, 1) ||
258 0 : !(ready & (1U<<4)) ||
259 0 : !dev->read_registers(DPS280_REG_PRESS, buf, 3) ||
260 0 : !dev->read_registers(DPS280_REG_TEMP, &buf[3], 3)) {
261 : // data not ready
262 0 : err_count++;
263 0 : check_health();
264 0 : return;
265 : }
266 :
267 0 : int32_t press = (buf[2]) + (buf[1]<<8) + (buf[0]<<16);
268 0 : int32_t temp = (buf[5]) + (buf[4]<<8) + (buf[3]<<16);
269 0 : fix_config_bits32(press, 24);
270 0 : fix_config_bits32(temp, 24);
271 :
272 : float pressure, temperature;
273 :
274 0 : calculate_PT(temp, press, pressure, temperature);
275 :
276 0 : last_temperature = temperature;
277 :
278 0 : if (!pressure_ok(pressure)) {
279 0 : return;
280 : }
281 :
282 0 : check_health();
283 :
284 0 : if (fabsf(last_temperature) <= TEMPERATURE_LIMIT_C) {
285 0 : err_count = 0;
286 : }
287 :
288 0 : WITH_SEMAPHORE(_sem);
289 :
290 0 : pressure_sum += pressure;
291 0 : temperature_sum += temperature;
292 0 : count++;
293 0 : }
294 :
295 : // transfer data to the frontend
296 0 : void AP_Baro_DPS280::update(void)
297 : {
298 0 : if (count == 0) {
299 0 : return;
300 : }
301 :
302 0 : WITH_SEMAPHORE(_sem);
303 :
304 0 : _copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
305 0 : pressure_sum = 0;
306 0 : temperature_sum = 0;
307 0 : count=0;
308 0 : }
309 :
310 : #endif // AP_BARO_DPS280_ENABLED
|
