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 : /*
17 : backend driver for airspeed from a I2C MS5525D0 sensor
18 : */
19 : #include "AP_Airspeed_MS5525.h"
20 :
21 : #if AP_AIRSPEED_MS5525_ENABLED
22 :
23 : #include <stdio.h>
24 : #include <utility>
25 :
26 : #include <AP_Common/AP_Common.h>
27 : #include <AP_HAL/AP_HAL.h>
28 : #include <AP_HAL/I2CDevice.h>
29 : #include <AP_HAL/utility/sparse-endian.h>
30 : #include <AP_Math/AP_Math.h>
31 : #include <AP_Math/crc.h>
32 : #include <GCS_MAVLink/GCS.h>
33 :
34 : extern const AP_HAL::HAL &hal;
35 :
36 : #define MS5525D0_I2C_ADDR_1 0x76
37 : #define MS5525D0_I2C_ADDR_2 0x77
38 :
39 : #define REG_RESET 0x1E
40 : #define REG_CONVERT_D1_OSR_256 0x40
41 : #define REG_CONVERT_D1_OSR_512 0x42
42 : #define REG_CONVERT_D1_OSR_1024 0x44
43 : #define REG_CONVERT_D1_OSR_2048 0x46
44 : #define REG_CONVERT_D1_OSR_4096 0x48
45 : #define REG_CONVERT_D2_OSR_256 0x50
46 : #define REG_CONVERT_D2_OSR_512 0x52
47 : #define REG_CONVERT_D2_OSR_1024 0x54
48 : #define REG_CONVERT_D2_OSR_2048 0x56
49 : #define REG_CONVERT_D2_OSR_4096 0x58
50 : #define REG_ADC_READ 0x00
51 : #define REG_PROM_BASE 0xA0
52 :
53 : // go for 1024 oversampling. This should be fast enough to reduce
54 : // noise but low enough to keep self-heating small
55 : #define REG_CONVERT_PRESSURE REG_CONVERT_D1_OSR_1024
56 : #define REG_CONVERT_TEMPERATURE REG_CONVERT_D2_OSR_1024
57 :
58 3 : AP_Airspeed_MS5525::AP_Airspeed_MS5525(AP_Airspeed &_frontend, uint8_t _instance, MS5525_ADDR address) :
59 3 : AP_Airspeed_Backend(_frontend, _instance)
60 : {
61 3 : _address = address;
62 3 : }
63 :
64 : // probe and initialise the sensor
65 3 : bool AP_Airspeed_MS5525::init()
66 : {
67 3 : const uint8_t addresses[] = { MS5525D0_I2C_ADDR_1, MS5525D0_I2C_ADDR_2 };
68 3 : bool found = false;
69 3 : for (uint8_t i=0; i<ARRAY_SIZE(addresses); i++) {
70 3 : if (_address != MS5525_ADDR_AUTO && i != (uint8_t)_address) {
71 0 : continue;
72 : }
73 3 : dev = hal.i2c_mgr->get_device_ptr(get_bus(), addresses[i]);
74 3 : if (!dev) {
75 0 : continue;
76 : }
77 3 : WITH_SEMAPHORE(dev->get_semaphore());
78 :
79 : // lots of retries during probe
80 3 : dev->set_retries(5);
81 :
82 3 : found = read_prom();
83 :
84 3 : if (found) {
85 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MS5525[%u]: Found on bus %u addr 0x%02x", get_instance(), get_bus(), addresses[i]);
86 3 : break;
87 : }
88 3 : }
89 3 : if (!found) {
90 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MS5525[%u]: no sensor found", get_instance());
91 0 : return false;
92 : }
93 :
94 : // Send a command to read temperature first
95 3 : WITH_SEMAPHORE(dev->get_semaphore());
96 3 : uint8_t reg = REG_CONVERT_TEMPERATURE;
97 3 : dev->transfer(®, 1, nullptr, 0);
98 3 : state = 0;
99 3 : command_send_us = AP_HAL::micros();
100 :
101 3 : dev->set_device_type(uint8_t(DevType::MS5525));
102 3 : set_bus_id(dev->get_bus_id());
103 :
104 : // drop to 2 retries for runtime
105 3 : dev->set_retries(2);
106 :
107 : // read at 80Hz
108 3 : dev->register_periodic_callback(1000000UL/80U,
109 : FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS5525::timer, void));
110 3 : return true;
111 3 : }
112 :
113 :
114 : /**
115 : * CRC used by MS pressure devices
116 : */
117 3 : uint16_t AP_Airspeed_MS5525::crc4_prom(void)
118 : {
119 3 : return crc_crc4(prom);
120 : }
121 :
122 3 : bool AP_Airspeed_MS5525::read_prom(void)
123 : {
124 : // reset the chip to ensure it has correct prom values loaded
125 3 : uint8_t reg = REG_RESET;
126 3 : if (!dev->transfer(®, 1, nullptr, 0)) {
127 0 : return false;
128 : }
129 3 : hal.scheduler->delay(5);
130 :
131 3 : bool all_zero = true;
132 27 : for (uint8_t i = 0; i < 8; i++) {
133 : be16_t val;
134 24 : if (!dev->read_registers(REG_PROM_BASE+i*2, (uint8_t *) &val,
135 : sizeof(uint16_t))) {
136 0 : return false;
137 : }
138 24 : prom[i] = be16toh(val);
139 24 : if (prom[i] != 0) {
140 24 : all_zero = false;
141 : }
142 : }
143 :
144 3 : if (all_zero) {
145 0 : return false;
146 : }
147 :
148 : /* save the read crc */
149 3 : const uint16_t crc_read = prom[7] & 0xf;
150 :
151 : /* remove CRC byte */
152 3 : prom[7] &= 0xff00;
153 :
154 3 : uint16_t crc_calc = crc4_prom();
155 3 : if (crc_read != crc_calc) {
156 0 : printf("MS5525: CRC mismatch 0x%04x 0x%04x\n", crc_read, crc_calc);
157 : }
158 3 : return crc_read == crc_calc;
159 : }
160 :
161 :
162 : /*
163 : read from the ADC
164 : */
165 56776 : int32_t AP_Airspeed_MS5525::read_adc()
166 : {
167 : uint8_t val[3];
168 56776 : if (!dev->read_registers(REG_ADC_READ, val, 3)) {
169 0 : return 0;
170 : }
171 56776 : return (val[0] << 16) | (val[1] << 8) | val[2];
172 : }
173 :
174 : /*
175 : calculate pressure and temperature
176 : */
177 45416 : void AP_Airspeed_MS5525::calculate(void)
178 : {
179 : // table for the 001DS part, 1PSI range
180 45416 : const uint8_t Q1 = 15;
181 45416 : const uint8_t Q2 = 17;
182 45416 : const uint8_t Q3 = 7;
183 45416 : const uint8_t Q4 = 5;
184 45416 : const uint8_t Q5 = 7;
185 45416 : const uint8_t Q6 = 21;
186 :
187 45416 : float dT = float(D2) - int64_t(prom[5]) * (1L<<Q5);
188 45416 : float TEMP = 2000 + (dT*int64_t(prom[6]))/(1L<<Q6);
189 45416 : float OFF = int64_t(prom[2])*(1L<<Q2) + (int64_t(prom[4])*dT)/(1L<<Q4);
190 45416 : float SENS = int64_t(prom[1])*(1L<<Q1) + (int64_t(prom[3])*dT)/(1L<<Q3);
191 45416 : float P = (float(D1)*SENS/(1L<<21)-OFF)/(1L<<15);
192 45416 : const float PSI_to_Pa = 6894.757f;
193 45416 : float P_Pa = PSI_to_Pa * 1.0e-4 * P;
194 45416 : float Temp_C = TEMP * 0.01;
195 :
196 : #if 0
197 : static uint16_t counter;
198 : if (counter++ == 100) {
199 : printf("P=%.6f T=%.2f D1=%d D2=%d\n", P_Pa, Temp_C, D1, D2);
200 : counter=0;
201 : }
202 : #endif
203 :
204 45416 : WITH_SEMAPHORE(sem);
205 :
206 45416 : pressure_sum += P_Pa;
207 45416 : temperature_sum += Temp_C;
208 45416 : press_count++;
209 45416 : temp_count++;
210 45416 : last_sample_time_ms = AP_HAL::millis();
211 45416 : }
212 :
213 : // 80Hz timer
214 56776 : void AP_Airspeed_MS5525::timer()
215 : {
216 56776 : if (AP_HAL::micros() - command_send_us < 10000) {
217 : // we should avoid trying to read the ADC too soon after
218 : // sending the command
219 0 : return;
220 : }
221 :
222 56776 : uint32_t adc_val = read_adc();
223 :
224 56776 : if (adc_val == 0) {
225 : // we have either done a read too soon after sending the
226 : // request or we have tried to read the same sample twice. We
227 : // re-send the command now as we don't know what state the
228 : // sensor is in, and we do want to trigger it sending a value,
229 : // which we will discard
230 0 : if (dev->transfer(&cmd_sent, 1, nullptr, 0)) {
231 0 : command_send_us = AP_HAL::micros();
232 : }
233 : // when we get adc_val == 0 then then both the current value and
234 : // the next value we read from the sensor are invalid
235 0 : ignore_next = true;
236 0 : return;
237 : }
238 :
239 : /*
240 : * If read fails, re-initiate a read command for current state or we are
241 : * stuck
242 : */
243 56776 : if (!ignore_next) {
244 56776 : if (cmd_sent == REG_CONVERT_TEMPERATURE) {
245 11357 : D2 = adc_val;
246 45419 : } else if (cmd_sent == REG_CONVERT_PRESSURE) {
247 45416 : D1 = adc_val;
248 45416 : calculate();
249 : }
250 : }
251 :
252 56776 : ignore_next = false;
253 :
254 56776 : cmd_sent = (state == 0) ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE;
255 56776 : if (!dev->transfer(&cmd_sent, 1, nullptr, 0)) {
256 : // we don't know for sure what state the sensor is in when we
257 : // fail to send the command, so ignore the next response
258 0 : ignore_next = true;
259 0 : return;
260 : }
261 56776 : command_send_us = AP_HAL::micros();
262 :
263 56776 : state = (state + 1) % 5;
264 : }
265 :
266 : // return the current differential_pressure in Pascal
267 7067 : bool AP_Airspeed_MS5525::get_differential_pressure(float &_pressure)
268 : {
269 7067 : WITH_SEMAPHORE(sem);
270 :
271 7067 : if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
272 0 : return false;
273 : }
274 :
275 7067 : if (press_count > 0) {
276 7067 : pressure = pressure_sum / press_count;
277 7067 : press_count = 0;
278 7067 : pressure_sum = 0;
279 : }
280 7067 : _pressure = pressure;
281 :
282 7067 : return true;
283 7067 : }
284 :
285 : // return the current temperature in degrees C, if available
286 14925 : bool AP_Airspeed_MS5525::get_temperature(float &_temperature)
287 : {
288 14925 : WITH_SEMAPHORE(sem);
289 :
290 14925 : if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
291 4 : return false;
292 : }
293 :
294 14921 : if (temp_count > 0) {
295 11171 : temperature = temperature_sum / temp_count;
296 11171 : temp_count = 0;
297 11171 : temperature_sum = 0;
298 : }
299 :
300 14921 : _temperature = temperature;
301 14921 : return true;
302 14925 : }
303 :
304 : #endif // AP_AIRSPEED_MS5525_ENABLED
|
