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 AUAV sensor
18 : */
19 : #include "AP_Airspeed_AUAV.h"
20 :
21 : #if AP_AIRSPEED_AUAV_ENABLED
22 :
23 : #define AUAV_AIRSPEED_I2C_ADDR 0x26
24 : #define MEASUREMENT_TIMEOUT_MS 200
25 :
26 : #include <AP_Common/AP_Common.h>
27 : #include <AP_HAL/AP_HAL.h>
28 : #include <AP_HAL/I2CDevice.h>
29 : #include <AP_Math/AP_Math.h>
30 : #include <GCS_MAVLink/GCS.h>
31 :
32 : extern const AP_HAL::HAL &hal;
33 :
34 0 : AUAV_Pressure_sensor::AUAV_Pressure_sensor(AP_HAL::Device *&_dev, Type _type) :
35 0 : dev(_dev),
36 0 : type(_type)
37 : {
38 0 : }
39 :
40 0 : AP_Airspeed_AUAV::AP_Airspeed_AUAV(AP_Airspeed &_frontend, uint8_t _instance, const float _range_inH2O) :
41 : AP_Airspeed_Backend(_frontend, _instance),
42 0 : range_inH2O(_range_inH2O)
43 : {
44 0 : }
45 :
46 : // probe for a sensor
47 0 : bool AP_Airspeed_AUAV::probe(uint8_t bus, uint8_t address)
48 : {
49 0 : _dev = hal.i2c_mgr->get_device_ptr(bus, address);
50 0 : if (!_dev) {
51 0 : return false;
52 : }
53 :
54 0 : WITH_SEMAPHORE(_dev->get_semaphore());
55 :
56 0 : _dev->set_retries(10);
57 0 : if (!sensor.measure()) {
58 0 : return false;
59 : }
60 0 : hal.scheduler->delay(20);
61 :
62 : // Don't fill in values yet as the compensation coefficients are not configured
63 : float PComp, temperature;
64 0 : return sensor.collect(PComp, temperature) == AUAV_Pressure_sensor::Status::Normal;
65 0 : }
66 :
67 : // probe and initialise the sensor
68 0 : bool AP_Airspeed_AUAV::init()
69 : {
70 0 : const auto bus = get_bus();
71 0 : if (!probe(bus, AUAV_AIRSPEED_I2C_ADDR)) {
72 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AUAV AIRSPEED[%u]: no sensor found on bus %u", get_instance(), bus);
73 0 : return false;
74 : }
75 :
76 0 : _dev->set_device_type(uint8_t(DevType::AUAV));
77 0 : set_bus_id(_dev->get_bus_id());
78 :
79 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AUAV AIRSPEED[%u]: Found bus %u addr 0x%02x", get_instance(), _dev->bus_num(), _dev->get_bus_address());
80 :
81 : // drop to 2 retries for runtime
82 0 : _dev->register_periodic_callback(20000,
83 : FUNCTOR_BIND_MEMBER(&AP_Airspeed_AUAV::_timer, void));
84 0 : return true;
85 : }
86 :
87 : // start a measurement
88 0 : bool AUAV_Pressure_sensor::measure()
89 : {
90 0 : uint8_t cmd = 0xAE; // start average8
91 : // Differential: max 16.2ms measurement time
92 : // Absolute: 37ms
93 0 : if (dev->transfer(&cmd, 1, nullptr, 0)) {
94 0 : return true;
95 : }
96 0 : return false;
97 : }
98 :
99 : // read the values from the sensor
100 : // Return true if successful
101 : // pressure corrected but not scaled to the correct units
102 : // temperature in degrees centigrade
103 0 : AUAV_Pressure_sensor::Status AUAV_Pressure_sensor::collect(float &PComp, float &temperature)
104 : {
105 : uint8_t inbuf[7];
106 0 : if (!dev->read((uint8_t *)&inbuf, sizeof(inbuf))) {
107 0 : return Status::Fault;
108 : }
109 0 : const uint8_t status = inbuf[0];
110 :
111 : // power on, not busy, reading normal, not out of range
112 0 : const uint8_t healthy_status = type == Type::Differential ? 0x50 : 0x40;
113 0 : if (status != healthy_status) {
114 0 : if ((status & (1U << 5)) != 0) {
115 0 : return Status::Busy;
116 : }
117 0 : return Status::Fault;
118 : }
119 0 : const int32_t Tref_Counts = 7576807; // temperature counts at 25C
120 0 : const float TC50Scale = 100.0f * 100.0f * 167772.2f; // scale TC50 to 1.0% FS0
121 : float Pdiff, TC50;
122 :
123 : // Convert unsigned 24-bit pressure value to signed +/- 23-bit:
124 0 : const int32_t iPraw = (inbuf[1]<<16) + (inbuf[2]<<8) + inbuf[3] - 0x800000;
125 :
126 : // Convert signed 23-bit value to float, normalized to +/- 1.0:
127 0 : const float Pnorm = float(iPraw) / float(0x7FFFFF);
128 0 : const float AP3 = LIN_A * Pnorm * Pnorm * Pnorm; // A*Pout^3
129 0 : const float BP2 = LIN_B * Pnorm * Pnorm; // B*Pout^2
130 0 : const float CP = LIN_C * Pnorm; // C*POut
131 0 : const float Corr = AP3 + BP2 + CP + LIN_D; // Linearity correction term
132 0 : const float Pcorr = Pnorm + Corr; // Corrected P, range +/-1.0.
133 :
134 : // Compute difference from reference temperature, in sensor counts:
135 0 : const int32_t iTemp = (inbuf[4]<<16) + (inbuf[5]<<8) + inbuf[6]; // 24-bit temperature
136 :
137 : // get temperature in degrees C
138 0 : temperature = (iTemp * 155) / float(1U<<24) - 45;
139 :
140 0 : const int32_t Tdiff = iTemp - Tref_Counts; // see constant defined above.
141 0 : const float Pnfso = (Pcorr + 1.0f) * 0.5;
142 :
143 : //TC50: Select High/Low, based on current temp above/below 25C:
144 0 : if (Tdiff > 0) {
145 0 : TC50 = TC50H;
146 : } else {
147 0 : TC50 = TC50L;
148 : }
149 :
150 : // Find absolute difference between midrange and reading (abs(Pnfso-0.5)):
151 0 : if (Pnfso > 0.5) {
152 0 : Pdiff = Pnfso - 0.5;
153 : } else {
154 0 : Pdiff = 0.5f - Pnfso;
155 : }
156 :
157 0 : const float Tcorr = (1.0f - (Es * 2.5f * Pdiff)) * Tdiff * TC50 / TC50Scale;
158 0 : const float PCorrt = Pnfso - Tcorr; // corrected P: float, [0 to +1.0)
159 0 : PComp = PCorrt * (float)0xFFFFFF;
160 :
161 0 : return Status::Normal;
162 : }
163 :
164 0 : bool AUAV_Pressure_sensor::read_register(uint8_t cmd, uint16_t &result)
165 : {
166 0 : switch (coefficient_step) {
167 0 : case CoefficientStep::request: {
168 0 : if (dev->transfer(&cmd, sizeof(cmd), nullptr, 0)) {
169 0 : coefficient_step = CoefficientStep::read;
170 : }
171 0 : return false;
172 : }
173 :
174 0 : case CoefficientStep::read: {
175 : // Next step is always another request, either for the next coefficient or a re-request if this read fails
176 0 : coefficient_step = CoefficientStep::request;
177 :
178 0 : const uint8_t healthy_status = type == Type::Differential ? 0x50 : 0x40;
179 : uint8_t raw_bytes[3];
180 0 : if (!dev->read((uint8_t *)&raw_bytes, sizeof(raw_bytes))) {
181 0 : return false;
182 : }
183 0 : if (raw_bytes[0] != healthy_status) {
184 0 : return false;
185 : }
186 0 : result = ((uint16_t)raw_bytes[1] << 8) | (uint16_t)raw_bytes[2];
187 0 : return true;
188 : }
189 : }
190 :
191 0 : return false;
192 :
193 : }
194 :
195 0 : void AUAV_Pressure_sensor::read_coefficients()
196 : {
197 0 : const uint8_t cmd_start = type == Type::Differential ? 0x2B : 0x2F;
198 :
199 : // Differential can happily run through these in one call, Absolute gets stuck, no idea why.
200 : // Space them out anyway to be inline with the spec
201 :
202 0 : switch (stage) {
203 0 : case CoefficientStage::A_high: {
204 : uint16_t high;
205 0 : if (!read_register(cmd_start + 0, high)) {
206 0 : return;
207 : }
208 0 : LIN_A = (float)(high << 16)/(float)(0x7FFFFFFF);
209 0 : stage = CoefficientStage::A_low;
210 0 : break;
211 : }
212 :
213 0 : case CoefficientStage::A_low: {
214 : uint16_t low;
215 0 : if (!read_register(cmd_start + 1, low)) {
216 0 : return;
217 : }
218 0 : LIN_A += (float)(low)/(float)(0x7FFFFFFF);
219 0 : stage = CoefficientStage::B_high;
220 0 : break;
221 : }
222 :
223 0 : case CoefficientStage::B_high: {
224 : uint16_t high;
225 0 : if (!read_register(cmd_start + 2, high)) {
226 0 : return;
227 : }
228 0 : LIN_B = (float)(high << 16)/(float)(0x7FFFFFFF);
229 0 : stage = CoefficientStage::B_low;
230 0 : break;
231 : }
232 :
233 0 : case CoefficientStage::B_low: {
234 : uint16_t low;
235 0 : if (!read_register(cmd_start + 3, low)) {
236 0 : return;
237 : }
238 0 : LIN_B += (float)(low)/(float)(0x7FFFFFFF);
239 0 : stage = CoefficientStage::C_high;
240 0 : break;
241 : }
242 :
243 0 : case CoefficientStage::C_high: {
244 : uint16_t high;
245 0 : if (!read_register(cmd_start + 4, high)) {
246 0 : return;
247 : }
248 0 : LIN_C = (float)(high << 16)/(float)(0x7FFFFFFF);
249 0 : stage = CoefficientStage::C_low;
250 0 : break;
251 : }
252 :
253 0 : case CoefficientStage::C_low: {
254 : uint16_t low;
255 0 : if (!read_register(cmd_start + 5, low)) {
256 0 : return;
257 : }
258 0 : LIN_C += (float)(low)/(float)(0x7FFFFFFF);
259 0 : stage = CoefficientStage::D_high;
260 0 : break;
261 : }
262 :
263 0 : case CoefficientStage::D_high: {
264 : uint16_t high;
265 0 : if (!read_register(cmd_start + 6, high)) {
266 0 : return;
267 : }
268 0 : LIN_D = (float)(high << 16)/(float)(0x7FFFFFFF);
269 0 : stage = CoefficientStage::D_low;
270 0 : break;
271 : }
272 :
273 0 : case CoefficientStage::D_low: {
274 : uint16_t low;
275 0 : if (!read_register(cmd_start + 7, low)) {
276 0 : return;
277 : }
278 0 : LIN_D += (float)(low)/(float)(0x7FFFFFFF);
279 0 : stage = CoefficientStage::E_high;
280 0 : break;
281 : }
282 :
283 0 : case CoefficientStage::E_high: {
284 : uint16_t TC50HL;
285 0 : if (!read_register(cmd_start + 8, TC50HL)) {
286 0 : return;
287 : }
288 0 : const int8_t i8TC50H = (TC50HL >> 8) & 0xFF;
289 0 : const int8_t i8TC50L = TC50HL & 0xFF;
290 0 : TC50H = (float)(i8TC50H)/(float)(0x7F);
291 0 : TC50L = (float)(i8TC50L)/(float)(0x7F);
292 0 : stage = CoefficientStage::E_low;
293 0 : break;
294 : }
295 :
296 0 : case CoefficientStage::E_low: {
297 : uint16_t es;
298 0 : if (!read_register(cmd_start + 9, es)) {
299 0 : return;
300 : }
301 0 : Es = (float)(es & 0xFF)/(float)(0x7F);
302 0 : stage = CoefficientStage::Done;
303 :
304 : // Drop to 2 retry's for runtime
305 0 : dev->set_retries(2);
306 0 : break;
307 : }
308 :
309 0 : case CoefficientStage::Done:
310 0 : break;
311 : }
312 : }
313 :
314 0 : void AP_Airspeed_AUAV::_timer()
315 : {
316 0 : if (sensor.stage != AUAV_Pressure_sensor::CoefficientStage::Done) {
317 0 : sensor.read_coefficients();
318 0 : return;
319 : }
320 :
321 0 : if (measurement_requested) {
322 : // Read in result of last measurement
323 : float Pcomp;
324 0 : switch (sensor.collect(Pcomp, temp_C)) {
325 0 : case AUAV_Pressure_sensor::Status::Normal:
326 : // Calculate pressure and update last read time
327 0 : last_sample_time_ms = AP_HAL::millis();
328 :
329 : // Convert to correct units and apply range
330 0 : pressure = 248.8f*1.25f*((Pcomp-8388608)/16777216.0f) * 2 * range_inH2O;
331 0 : break;
332 :
333 0 : case AUAV_Pressure_sensor::Status::Busy:
334 : // Don't request a new measurement
335 0 : return;
336 :
337 0 : case AUAV_Pressure_sensor::Status::Fault:
338 0 : break;
339 : }
340 : }
341 :
342 : // Request a new measurement
343 0 : measurement_requested = sensor.measure();
344 : }
345 :
346 : // return the current differential_pressure in Pascal
347 0 : bool AP_Airspeed_AUAV::get_differential_pressure(float &_pressure)
348 : {
349 0 : WITH_SEMAPHORE(sem);
350 0 : _pressure = pressure;
351 0 : return AP_HAL::millis() - last_sample_time_ms < MEASUREMENT_TIMEOUT_MS;
352 0 : }
353 :
354 : // return the current temperature in degrees C, if available
355 0 : bool AP_Airspeed_AUAV::get_temperature(float &_temperature)
356 : {
357 0 : WITH_SEMAPHORE(sem);
358 0 : _temperature = temp_C;
359 0 : return AP_HAL::millis() - last_sample_time_ms < MEASUREMENT_TIMEOUT_MS;
360 0 : }
361 :
362 : #endif // AP_AIRSPEED_AUAV_ENABLED
|
