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 : #include "AP_Baro_KellerLD.h"
17 :
18 : #if AP_BARO_KELLERLD_ENABLED
19 :
20 : #include <stdio.h>
21 :
22 : #include <AP_Math/AP_Math.h>
23 :
24 : #define KELLER_DEBUG 0
25 :
26 : #if KELLER_DEBUG
27 : # define Debug(fmt, args ...) do {printf(fmt "\n", ## args);} while(0)
28 : #else
29 : # define Debug(fmt, args ...)
30 : #endif
31 :
32 : extern const AP_HAL::HAL &hal;
33 :
34 : // sensor metadata register
35 : static const uint8_t CMD_METADATA_PMODE = 0x12;
36 : // Measurement range registers
37 : static const uint8_t CMD_PRANGE_MIN_MSB = 0x13;
38 : static const uint8_t CMD_PRANGE_MIN_LSB = 0x14;
39 : static const uint8_t CMD_PRANGE_MAX_MSB = 0x15;
40 : static const uint8_t CMD_PRANGE_MAX_LSB = 0x16;
41 :
42 : // write to this address to start pressure measurement
43 : static const uint8_t CMD_REQUEST_MEASUREMENT = 0xAC;
44 :
45 : // Status byte handling, per Keller LD "Communication Protocol 4LD-9LD" (v2.6, pg 7)
46 : // bits 7-6 : reserved, must be 01
47 : // bit 5 : ignored, known to be unreliable in block reads
48 : // bits 4-3 : operating mode (00 = normal measurement)
49 : // bit 2 : checksum error? (0 = ok, 1 = error)
50 : // bits 1-0 : don't care
51 : static const uint8_t STATUS_REQUIRED_MASK = 0b11011100;
52 : static const uint8_t STATUS_REQUIRED_BITS = 0b01000000;
53 :
54 0 : AP_Baro_KellerLD::AP_Baro_KellerLD(AP_Baro &baro, AP_HAL::Device &dev)
55 : : AP_Baro_Backend(baro)
56 0 : , _dev(&dev)
57 : {
58 0 : }
59 :
60 : // Look for the device on the bus and see if it responds appropriately
61 0 : AP_Baro_Backend *AP_Baro_KellerLD::probe(AP_Baro &baro, AP_HAL::Device &dev)
62 : {
63 0 : AP_Baro_KellerLD *sensor = NEW_NOTHROW AP_Baro_KellerLD(baro, dev);
64 0 : if (!sensor || !sensor->_init()) {
65 0 : delete sensor;
66 0 : return nullptr;
67 : }
68 0 : return sensor;
69 : }
70 :
71 :
72 : // convenience function to work around device transfer oddities
73 0 : bool AP_Baro_KellerLD::transfer_with_delays(uint8_t *send, uint8_t sendlen, uint8_t *recv, uint8_t recvlen)
74 : {
75 0 : if (!_dev->transfer(send, sendlen, nullptr, 0)) {
76 0 : return false;
77 : }
78 0 : hal.scheduler->delay(1);
79 :
80 0 : if(!_dev->transfer(nullptr, 0, recv, recvlen)) {
81 0 : return false;
82 : }
83 0 : hal.scheduler->delay(1);
84 0 : return true;
85 : }
86 :
87 : // This device has some undocumented finicky quirks and requires
88 : // delays when reading out the measurement range, but for some reason
89 : // this isn't an issue when requesting measurements. This is why we
90 : // need to split the transfers with delays like this. (Using
91 : // AP_HAL::I2CDevice::set_split_transfers will not work with these
92 : // sensors)
93 0 : bool AP_Baro_KellerLD::read_measurement_limit(float *limit, uint8_t msb_addr, uint8_t lsb_addr)
94 : {
95 : uint8_t data[3];
96 :
97 0 : if (!transfer_with_delays(&msb_addr, 1, data, ARRAY_SIZE(data))) {
98 0 : return false;
99 : }
100 :
101 0 : const uint16_t ms_word = (data[1] << 8) | data[2];
102 : Debug("0x%02x: %d [%d, %d, %d]", msb_addr, ms_word, data[0], data[1], data[2]);
103 :
104 0 : if (!transfer_with_delays(&lsb_addr, 1, data, ARRAY_SIZE(data))) {
105 0 : return false;
106 : }
107 :
108 0 : const uint16_t ls_word = (data[1] << 8) | data[2];
109 : Debug("0x%02x: %d [%d, %d, %d]", lsb_addr, ls_word, data[0], data[1], data[2]);
110 :
111 0 : const uint32_t cal_data = (ms_word << 16) | ls_word;
112 0 : memcpy(limit, &cal_data, sizeof(*limit));
113 :
114 0 : if (isinf(*limit) || isnan(*limit)) {
115 0 : return false;
116 : }
117 :
118 : Debug("data: %d, float: %.2f", cal_data, _p_min);
119 0 : return true;
120 : }
121 :
122 0 : bool AP_Baro_KellerLD::read_cal()
123 : {
124 : // Read out pressure measurement range
125 0 : if (!read_measurement_limit(&_p_min, CMD_PRANGE_MIN_MSB, CMD_PRANGE_MIN_LSB)) {
126 0 : return false;
127 : }
128 :
129 0 : if (!read_measurement_limit(&_p_max, CMD_PRANGE_MAX_MSB, CMD_PRANGE_MAX_LSB)) {
130 0 : return false;
131 : }
132 :
133 0 : if (_p_max <= _p_min) {
134 0 : return false;
135 : }
136 :
137 0 : return true;
138 : }
139 :
140 : // Read sensor P-Mode type and set pressure reference offset
141 : // This determines the pressure offset based on the type of sensor
142 : // vented to atmosphere, gauged to vacuum, or gauged to standard sea-level pressure
143 0 : bool AP_Baro_KellerLD::read_mode_type()
144 : {
145 0 : uint8_t cmd = CMD_METADATA_PMODE;
146 : uint8_t data[3];
147 0 : if (!transfer_with_delays(&cmd, 1, data, ARRAY_SIZE(data))) {
148 0 : return false;
149 : }
150 :
151 : // Byte 3, Bit 0 & 1: Represents P-Mode
152 : // "Communication Protocol 4 LD…9 LD", Version 2.6 pg 12 of 25
153 : // https://keller-druck.com/?d=VeMYAQBxgoSNjUSHbdnBTU
154 0 : _p_mode = (SensorMode)(data[2] & 0b11);
155 :
156 : // update pressure offset based on P-Mode
157 0 : switch (_p_mode) {
158 0 : case SensorMode::PR_MODE:
159 : // PR-Mode vented gauge sensor
160 : // pressure reads zero when the pressure outside is equal to the pressure inside the enclosure
161 0 : _p_mode_offset = _frontend.get_pressure(0);
162 0 : break;
163 0 : case SensorMode::PA_MODE:
164 : // PA-Mode sealed gauge sensor
165 : // pressure reads zero when the pressure outside is equal to 1.0 bar
166 : // i.e., the pressure at which the vent is sealed
167 0 : _p_mode_offset = 1.0;
168 0 : break;
169 0 : case SensorMode::PAA_MODE:
170 : // PAA-mode Absolute sensor (zero at vacuum)
171 0 : _p_mode_offset = 0.0;
172 0 : break;
173 0 : case SensorMode::UNDEFINED:
174 : // we should give an error here
175 0 : printf("KellerLD Device Mode UNDEFINED\n");
176 0 : return false;
177 : }
178 :
179 0 : return true;
180 : }
181 :
182 : // We read out the measurement range to be used in raw value conversions
183 0 : bool AP_Baro_KellerLD::_init()
184 : {
185 0 : if (!_dev) {
186 0 : return false;
187 : }
188 :
189 0 : WITH_SEMAPHORE(_dev->get_semaphore());
190 :
191 : // high retries for init
192 0 : _dev->set_retries(10);
193 :
194 0 : if (!read_cal()) {
195 0 : printf("Cal read bad!\n");
196 0 : return false;
197 : }
198 :
199 0 : if (!read_mode_type()) {
200 0 : printf("Mode_Type read bad!\n");
201 0 : return false;
202 : }
203 :
204 0 : printf("Keller LD found on bus %u address 0x%02x\n", _dev->bus_num(), _dev->get_bus_address());
205 :
206 : // Send a command to take a measurement
207 0 : _dev->transfer(&CMD_REQUEST_MEASUREMENT, 1, nullptr, 0);
208 :
209 0 : memset(&_accum, 0, sizeof(_accum));
210 :
211 0 : _instance = _frontend.register_sensor();
212 :
213 0 : _dev->set_device_type(DEVTYPE_BARO_KELLERLD);
214 0 : set_bus_id(_instance, _dev->get_bus_id());
215 :
216 0 : _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER);
217 :
218 : // lower retries for run
219 0 : _dev->set_retries(3);
220 :
221 : // The sensor needs time to take a deep breath after reading out the calibration...
222 0 : hal.scheduler->delay(150);
223 :
224 : // Request 50Hz update
225 : // The sensor really struggles with any jitter in timing at 100Hz, and will sometimes start reading out all zeros
226 0 : _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC,
227 : FUNCTOR_BIND_MEMBER(&AP_Baro_KellerLD::_timer, void));
228 0 : return true;
229 0 : }
230 :
231 : // Read out most recent measurement from sensor hw
232 0 : bool AP_Baro_KellerLD::_read()
233 : {
234 : uint8_t data[5];
235 0 : if (!_dev->transfer(nullptr, 0, data, sizeof(data))) {
236 : Debug("Keller LD read failed!");
237 0 : return false;
238 : }
239 :
240 0 : const uint8_t status = data[0];
241 0 : uint16_t pressure_raw = (data[1] << 8) | data[2];
242 0 : uint16_t temperature_raw = (data[3] << 8) | data[4];
243 :
244 : #if KELLER_DEBUG
245 : static uint8_t samples = 0;
246 : if (samples < 3) {
247 : samples++;
248 : Debug("data: [%d, %d, %d, %d, %d]", data[0], data[1], data[2], data[3], data[4]);
249 : Debug("pressure_raw: %d\ttemperature_raw: %d", pressure_raw, temperature_raw);
250 : }
251 : #endif
252 :
253 : // Reject if the sensor is not in the normal measurement state, or is reporting an error.
254 0 : if ((status & STATUS_REQUIRED_MASK) != STATUS_REQUIRED_BITS) {
255 : Debug("Keller: bad status 0x%02x", status);
256 0 : return false;
257 : }
258 :
259 0 : if (pressure_raw == 0 || temperature_raw == 0) {
260 : Debug("Keller: bad read");
261 0 : return false;
262 : }
263 :
264 0 : if (!pressure_ok(pressure_raw)) {
265 0 : return false;
266 : }
267 :
268 0 : WITH_SEMAPHORE(_sem);
269 :
270 0 : _update_and_wrap_accumulator(pressure_raw, temperature_raw, 128);
271 :
272 0 : return true;
273 0 : }
274 :
275 : // Periodic callback, regular update at 50Hz
276 : // Read out most recent measurement, and request another
277 : // Max conversion time according to datasheet is ~8ms, so
278 : // max update rate is ~125Hz, yet we struggle to get consistent
279 : // performance/data at 100Hz
280 0 : void AP_Baro_KellerLD::_timer(void)
281 : {
282 0 : _read();
283 0 : _dev->transfer(&CMD_REQUEST_MEASUREMENT, 1, nullptr, 0);
284 0 : }
285 :
286 : // Accumulate a reading, shrink if necessary to prevent overflow
287 0 : void AP_Baro_KellerLD::_update_and_wrap_accumulator(uint16_t pressure, uint16_t temperature, uint8_t max_count)
288 : {
289 0 : _accum.sum_pressure += pressure;
290 0 : _accum.sum_temperature += temperature;
291 0 : _accum.num_samples += 1;
292 :
293 0 : if (_accum.num_samples == max_count) {
294 0 : _accum.sum_pressure /= 2;
295 0 : _accum.sum_temperature /= 2;
296 0 : _accum.num_samples /= 2;
297 : }
298 0 : }
299 :
300 : // Take the average of accumulated values and push to frontend
301 0 : void AP_Baro_KellerLD::update()
302 : {
303 : float sum_pressure, sum_temperature;
304 : float num_samples;
305 :
306 : // update _p_mode_offset if vented guage
307 0 : if (_p_mode == SensorMode::PR_MODE) {
308 : // we need to get the pressure from on-board barometer
309 0 : _p_mode_offset = _frontend.get_pressure(0);
310 : }
311 :
312 : {
313 0 : WITH_SEMAPHORE(_sem);
314 :
315 0 : if (_accum.num_samples == 0) {
316 0 : return;
317 : }
318 :
319 0 : sum_pressure = _accum.sum_pressure;
320 0 : sum_temperature = _accum.sum_temperature;
321 0 : num_samples = _accum.num_samples;
322 0 : memset(&_accum, 0, sizeof(_accum));
323 0 : }
324 :
325 0 : uint16_t raw_pressure_avg = sum_pressure / num_samples;
326 0 : uint16_t raw_temperature_avg = sum_temperature / num_samples;
327 :
328 : // per datasheet
329 0 : float pressure = (raw_pressure_avg - 16384) * (_p_max - _p_min) / 32768 + _p_min + _p_mode_offset;
330 0 : pressure *= 100000; // bar -> Pascal
331 0 : float temperature = ((raw_temperature_avg >> 4) - 24) * 0.05f - 50;
332 :
333 0 : _copy_to_frontend(_instance, pressure, temperature);
334 : }
335 :
336 : #endif // AP_BARO_KELLERLD_ENABLED
|
