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 MS4525D0 sensor
18 : */
19 : #include "AP_Airspeed_MS4525.h"
20 :
21 : #if AP_AIRSPEED_MS4525_ENABLED
22 :
23 : #include <AP_Common/AP_Common.h>
24 : #include <AP_HAL/AP_HAL.h>
25 : #include <AP_HAL/I2CDevice.h>
26 : #include <AP_Math/AP_Math.h>
27 : #include <GCS_MAVLink/GCS.h>
28 : #include <stdio.h>
29 : #include <utility>
30 :
31 : extern const AP_HAL::HAL &hal;
32 :
33 : #define MS4525D0_I2C_ADDR1 0x28
34 : #define MS4525D0_I2C_ADDR2 0x36
35 : #define MS4525D0_I2C_ADDR3 0x46
36 :
37 : // probe for a sensor
38 0 : bool AP_Airspeed_MS4525::probe(uint8_t bus, uint8_t address)
39 : {
40 0 : _dev = hal.i2c_mgr->get_device_ptr(bus, address);
41 0 : if (!_dev) {
42 0 : return false;
43 : }
44 0 : WITH_SEMAPHORE(_dev->get_semaphore());
45 :
46 : // lots of retries during probe
47 0 : _dev->set_retries(10);
48 :
49 0 : _measure();
50 0 : hal.scheduler->delay(10);
51 0 : _collect();
52 :
53 0 : return _last_sample_time_ms != 0;
54 0 : }
55 :
56 : // probe and initialise the sensor
57 0 : bool AP_Airspeed_MS4525::init()
58 : {
59 : static const uint8_t addresses[] = { MS4525D0_I2C_ADDR1, MS4525D0_I2C_ADDR2, MS4525D0_I2C_ADDR3 };
60 0 : if (bus_is_configured()) {
61 : // the user has configured a specific bus
62 0 : for (uint8_t addr : addresses) {
63 0 : if (probe(get_bus(), addr)) {
64 0 : goto found_sensor;
65 : }
66 : }
67 : } else {
68 : // if bus is not configured then fall back to the old
69 : // behaviour of probing all buses, external first
70 0 : FOREACH_I2C_EXTERNAL(bus) {
71 0 : for (uint8_t addr : addresses) {
72 0 : if (probe(bus, addr)) {
73 0 : goto found_sensor;
74 : }
75 : }
76 : }
77 0 : FOREACH_I2C_INTERNAL(bus) {
78 0 : for (uint8_t addr : addresses) {
79 0 : if (probe(bus, addr)) {
80 0 : goto found_sensor;
81 : }
82 : }
83 : }
84 : }
85 :
86 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MS4525[%u]: no sensor found", get_instance());
87 0 : return false;
88 :
89 0 : found_sensor:
90 0 : _dev->set_device_type(uint8_t(DevType::MS4525));
91 0 : set_bus_id(_dev->get_bus_id());
92 :
93 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MS4525[%u]: Found bus %u addr 0x%02x", get_instance(), _dev->bus_num(), _dev->get_bus_address());
94 :
95 : // drop to 2 retries for runtime
96 0 : _dev->set_retries(2);
97 :
98 0 : _dev->register_periodic_callback(20000,
99 : FUNCTOR_BIND_MEMBER(&AP_Airspeed_MS4525::_timer, void));
100 0 : return true;
101 : }
102 :
103 : // start a measurement
104 0 : void AP_Airspeed_MS4525::_measure()
105 : {
106 0 : _measurement_started_ms = 0;
107 0 : uint8_t cmd = 0;
108 0 : if (_dev->transfer(&cmd, 1, nullptr, 0)) {
109 0 : _measurement_started_ms = AP_HAL::millis();
110 : }
111 0 : }
112 :
113 : /*
114 : this equation is an inversion of the equation in the
115 : pressure transfer function figure on page 4 of the datasheet
116 :
117 : We negate the result so that positive differential pressures
118 : are generated when the bottom port is used as the static
119 : port on the pitot and top port is used as the dynamic port
120 : */
121 0 : float AP_Airspeed_MS4525::_get_pressure(int16_t dp_raw) const
122 : {
123 0 : const float P_max = get_psi_range();
124 0 : const float P_min = - P_max;
125 0 : const float PSI_to_Pa = 6894.757f;
126 :
127 0 : float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
128 0 : float press = diff_press_PSI * PSI_to_Pa;
129 0 : return press;
130 : }
131 :
132 : /*
133 : convert raw temperature to temperature in degrees C
134 : */
135 0 : float AP_Airspeed_MS4525::_get_temperature(int16_t dT_raw) const
136 : {
137 0 : float temp = ((200.0f * dT_raw) / 2047) - 50;
138 0 : return temp;
139 : }
140 :
141 : // read the values from the sensor
142 0 : void AP_Airspeed_MS4525::_collect()
143 : {
144 : uint8_t data[4];
145 : uint8_t data2[4];
146 :
147 0 : _measurement_started_ms = 0;
148 :
149 0 : if (!_dev->transfer(nullptr, 0, data, sizeof(data))) {
150 0 : return;
151 : }
152 : // reread the data, so we can attempt to detect bad inputs
153 0 : if (!_dev->transfer(nullptr, 0, data2, sizeof(data2))) {
154 0 : return;
155 : }
156 :
157 0 : uint8_t status = (data[0] & 0xC0) >> 6;
158 : // only check the status on the first read, the second read is expected to be stale
159 0 : if (status == 2 || status == 3) {
160 0 : return;
161 : }
162 :
163 : int16_t dp_raw, dT_raw;
164 0 : dp_raw = (data[0] << 8) + data[1];
165 0 : dp_raw = 0x3FFF & dp_raw;
166 0 : dT_raw = (data[2] << 8) + data[3];
167 0 : dT_raw = (0xFFE0 & dT_raw) >> 5;
168 :
169 : int16_t dp_raw2, dT_raw2;
170 0 : dp_raw2 = (data2[0] << 8) + data2[1];
171 0 : dp_raw2 = 0x3FFF & dp_raw2;
172 0 : dT_raw2 = (data2[2] << 8) + data2[3];
173 0 : dT_raw2 = (0xFFE0 & dT_raw2) >> 5;
174 :
175 : // reject any values that are the absolute minimum or maximums these
176 : // can happen due to gnd lifts or communication errors on the bus
177 0 : if (dp_raw == 0x3FFF || dp_raw == 0 || dT_raw == 0x7FF || dT_raw == 0 ||
178 0 : dp_raw2 == 0x3FFF || dp_raw2 == 0 || dT_raw2 == 0x7FF || dT_raw2 == 0) {
179 0 : return;
180 : }
181 :
182 : // reject any double reads where the value has shifted in the upper more than
183 : // 0xFF
184 0 : if (abs(dp_raw - dp_raw2) > 0xFF || abs(dT_raw - dT_raw2) > 0xFF) {
185 0 : return;
186 : }
187 :
188 0 : float press = _get_pressure(dp_raw);
189 0 : float press2 = _get_pressure(dp_raw2);
190 0 : float temp = _get_temperature(dT_raw);
191 0 : float temp2 = _get_temperature(dT_raw2);
192 :
193 0 : if (!disable_voltage_correction()) {
194 0 : _voltage_correction(press, temp);
195 0 : _voltage_correction(press2, temp2);
196 : }
197 :
198 0 : WITH_SEMAPHORE(sem);
199 :
200 0 : _press_sum += press + press2;
201 0 : _temp_sum += temp + temp2;
202 0 : _press_count += 2;
203 0 : _temp_count += 2;
204 :
205 0 : _last_sample_time_ms = AP_HAL::millis();
206 0 : }
207 :
208 : /**
209 : correct for 5V rail voltage if the system_power ORB topic is
210 : available
211 :
212 : See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
213 : offset versus voltage for 3 sensors
214 : */
215 0 : void AP_Airspeed_MS4525::_voltage_correction(float &diff_press_pa, float &temperature)
216 : {
217 0 : const float slope = 65.0f;
218 0 : const float temp_slope = 0.887f;
219 :
220 : /*
221 : apply a piecewise linear correction within range given by above graph
222 : */
223 0 : float voltage_diff = hal.analogin->board_voltage() - 5.0f;
224 :
225 0 : voltage_diff = constrain_float(voltage_diff, -0.7f, 0.5f);
226 :
227 0 : diff_press_pa -= voltage_diff * slope;
228 0 : temperature -= voltage_diff * temp_slope;
229 0 : }
230 :
231 : // 50Hz timer
232 0 : void AP_Airspeed_MS4525::_timer()
233 : {
234 0 : if (_measurement_started_ms == 0) {
235 0 : _measure();
236 0 : return;
237 : }
238 0 : if ((AP_HAL::millis() - _measurement_started_ms) > 10) {
239 0 : _collect();
240 : // start a new measurement
241 0 : _measure();
242 : }
243 : }
244 :
245 : // return the current differential_pressure in Pascal
246 0 : bool AP_Airspeed_MS4525::get_differential_pressure(float &pressure)
247 : {
248 0 : WITH_SEMAPHORE(sem);
249 :
250 0 : if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
251 0 : return false;
252 : }
253 :
254 0 : if (_press_count > 0) {
255 0 : _pressure = _press_sum / _press_count;
256 0 : _press_count = 0;
257 0 : _press_sum = 0;
258 : }
259 :
260 0 : pressure = _pressure;
261 0 : return true;
262 0 : }
263 :
264 : // return the current temperature in degrees C, if available
265 0 : bool AP_Airspeed_MS4525::get_temperature(float &temperature)
266 : {
267 0 : WITH_SEMAPHORE(sem);
268 :
269 0 : if ((AP_HAL::millis() - _last_sample_time_ms) > 100) {
270 0 : return false;
271 : }
272 :
273 0 : if (_temp_count > 0) {
274 0 : _temperature = _temp_sum / _temp_count;
275 0 : _temp_count = 0;
276 0 : _temp_sum = 0;
277 : }
278 :
279 0 : temperature = _temperature;
280 0 : return true;
281 0 : }
282 :
283 : #endif // AP_AIRSPEED_MS4525_ENABLED
|
