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_ICM20789.h"
17 :
18 : #if AP_BARO_ICM20789_ENABLED
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <AP_HAL/Device.h>
22 :
23 : #include <AP_Common/AP_Common.h>
24 : #include <AP_BoardConfig/AP_BoardConfig.h>
25 :
26 : #include <stdio.h>
27 :
28 : #include <AP_Math/AP_Math.h>
29 : #include <AP_Logger/AP_Logger.h>
30 :
31 : #include <AP_InertialSensor/AP_InertialSensor_Invensense_registers.h>
32 :
33 : extern const AP_HAL::HAL &hal;
34 :
35 : /*
36 : CMD_READ options. The draft datasheet doesn't specify, but it seems
37 : Mode_1 has a conversion interval of 2ms. Mode_3 has a conversion
38 : interval of 20ms. Both seem to produce equally as smooth results, so
39 : presumably Mode_3 is doing internal averaging
40 : */
41 : #define CMD_READ_PT_MODE_1 0x401A
42 : #define CMD_READ_PT_MODE_3 0x5059
43 : #define CMD_READ_TP_MODE_1 0x609C
44 : #define CMD_READ_TP_MODE_3 0x70DF
45 :
46 : #define CONVERSION_INTERVAL_MODE_1 2000
47 : #define CONVERSION_INTERVAL_MODE_3 20000
48 :
49 : // setup for Mode_3
50 : #define CMD_READ_PT CMD_READ_PT_MODE_3
51 : #define CONVERSION_INTERVAL CONVERSION_INTERVAL_MODE_3
52 :
53 : #define CMD_SOFT_RESET 0x805D
54 : #define CMD_READ_ID 0xEFC8
55 :
56 : #define BARO_ICM20789_DEBUG 0
57 :
58 : #if BARO_ICM20789_DEBUG
59 : #define debug(fmt, args...) hal.console->printf(fmt, ##args)
60 : #else
61 : #define debug(fmt, args...)
62 : #endif
63 :
64 : /*
65 : constructor
66 : */
67 0 : AP_Baro_ICM20789::AP_Baro_ICM20789(AP_Baro &baro, AP_HAL::Device &_dev, AP_HAL::Device &_dev_imu)
68 : : AP_Baro_Backend(baro)
69 0 : , dev(&_dev)
70 0 : , dev_imu(&_dev_imu)
71 : {
72 0 : }
73 :
74 0 : AP_Baro_Backend *AP_Baro_ICM20789::probe(AP_Baro &baro,
75 : AP_HAL::Device &dev,
76 : AP_HAL::Device &dev_imu)
77 : {
78 : debug("Probing for ICM20789 baro\n");
79 0 : AP_Baro_ICM20789 *sensor = NEW_NOTHROW AP_Baro_ICM20789(baro, dev, dev_imu);
80 0 : if (!sensor || !sensor->init()) {
81 0 : delete sensor;
82 0 : return nullptr;
83 : }
84 0 : return sensor;
85 : }
86 :
87 :
88 : /*
89 : setup ICM20789 to enable barometer, assuming IMU is on SPI and baro is on I2C
90 : */
91 0 : bool AP_Baro_ICM20789::imu_spi_init(void)
92 : {
93 0 : dev_imu->get_semaphore()->take_blocking();
94 :
95 0 : dev_imu->set_read_flag(0x80);
96 :
97 0 : dev_imu->set_speed(AP_HAL::Device::SPEED_LOW);
98 0 : uint8_t whoami = 0;
99 : uint8_t v;
100 :
101 0 : dev_imu->read_registers(MPUREG_USER_CTRL, &v, 1);
102 0 : dev_imu->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO);
103 :
104 0 : hal.scheduler->delay(1);
105 0 : dev_imu->write_register(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
106 0 : dev_imu->write_register(MPUREG_PWR_MGMT_1,
107 : BIT_PWR_MGMT_1_SLEEP | BIT_PWR_MGMT_1_CLK_XGYRO);
108 :
109 0 : hal.scheduler->delay(1);
110 0 : dev_imu->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO);
111 :
112 0 : hal.scheduler->delay(1);
113 0 : dev_imu->write_register(MPUREG_FIFO_EN, 0x00);
114 0 : dev_imu->write_register(MPUREG_PWR_MGMT_1,
115 : BIT_PWR_MGMT_1_SLEEP | BIT_PWR_MGMT_1_CLK_XGYRO);
116 :
117 0 : dev_imu->read_registers(MPUREG_WHOAMI, &whoami, 1);
118 :
119 : // wait for sensor to settle
120 0 : hal.scheduler->delay(100);
121 :
122 0 : dev_imu->read_registers(MPUREG_WHOAMI, &whoami, 1);
123 :
124 0 : dev_imu->write_register(MPUREG_INT_PIN_CFG, 0x00);
125 0 : dev_imu->write_register(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
126 :
127 0 : dev_imu->get_semaphore()->give();
128 :
129 0 : return true;
130 : }
131 :
132 : /*
133 : setup ICM20789 to enable barometer, assuming both IMU and baro on the same i2c bus
134 : */
135 0 : bool AP_Baro_ICM20789::imu_i2c_init(void)
136 : {
137 : // as the baro device is already locked we need to re-use it,
138 : // changing its address to match the IMU address
139 0 : uint8_t old_address = dev->get_bus_address();
140 0 : dev->set_address(dev_imu->get_bus_address());
141 :
142 0 : dev->set_retries(4);
143 :
144 0 : uint8_t whoami=0;
145 0 : dev->read_registers(MPUREG_WHOAMI, &whoami, 1);
146 : debug("ICM20789: whoami 0x%02x old_address=%02x\n", whoami, old_address);
147 :
148 0 : dev->write_register(MPUREG_FIFO_EN, 0x00);
149 0 : dev->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO);
150 :
151 : // wait for sensor to settle
152 0 : hal.scheduler->delay(10);
153 :
154 0 : dev->write_register(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
155 :
156 0 : dev->set_address(old_address);
157 :
158 0 : return true;
159 : }
160 :
161 0 : bool AP_Baro_ICM20789::init()
162 : {
163 0 : if (!dev) {
164 0 : return false;
165 : }
166 :
167 : debug("Looking for 20789 baro\n");
168 :
169 0 : dev->get_semaphore()->take_blocking();
170 :
171 : debug("Setting up IMU\n");
172 0 : if (dev_imu->bus_type() != AP_HAL::Device::BUS_TYPE_I2C) {
173 0 : if (!imu_spi_init()) {
174 : debug("ICM20789: failed to initialise IMU SPI device\n");
175 0 : return false;
176 : }
177 0 : } else if (!imu_i2c_init()) {
178 : debug("ICM20789: failed to initialise IMU I2C device\n");
179 0 : return false;
180 : }
181 :
182 0 : if (!send_cmd16(CMD_SOFT_RESET)) {
183 : debug("ICM20789: reset failed\n");
184 0 : goto failed;
185 : }
186 :
187 : // wait for sensor to settle
188 0 : hal.scheduler->delay(10);
189 :
190 0 : if (!read_calibration_data()) {
191 : debug("ICM20789: read_calibration_data failed\n");
192 0 : goto failed;
193 : }
194 :
195 : // start a reading
196 0 : if (!send_cmd16(CMD_READ_PT)) {
197 : debug("ICM20789: start read failed\n");
198 0 : goto failed;
199 : }
200 :
201 0 : dev->set_retries(0);
202 :
203 0 : instance = _frontend.register_sensor();
204 :
205 0 : dev->set_device_type(DEVTYPE_BARO_ICM20789);
206 0 : set_bus_id(instance, dev->get_bus_id());
207 :
208 0 : dev->get_semaphore()->give();
209 :
210 : debug("ICM20789: startup OK\n");
211 :
212 : // use 10ms to ensure we don't lose samples, with max lag of 10ms
213 0 : dev->register_periodic_callback(CONVERSION_INTERVAL/2, FUNCTOR_BIND_MEMBER(&AP_Baro_ICM20789::timer, void));
214 :
215 0 : return true;
216 :
217 0 : failed:
218 0 : dev->get_semaphore()->give();
219 0 : return false;
220 : }
221 :
222 0 : bool AP_Baro_ICM20789::send_cmd16(uint16_t cmd)
223 : {
224 0 : uint8_t cmd_b[2] = { uint8_t(cmd >> 8), uint8_t(cmd & 0xFF) };
225 0 : return dev->transfer(cmd_b, 2, nullptr, 0);
226 : }
227 :
228 0 : bool AP_Baro_ICM20789::read_calibration_data(void)
229 : {
230 : // setup for OTP read
231 0 : const uint8_t cmd[5] = { 0xC5, 0x95, 0x00, 0x66, 0x9C };
232 0 : if (!dev->transfer(cmd, sizeof(cmd), nullptr, 0)) {
233 : debug("ICM20789: read cal1 failed\n");
234 0 : return false;
235 : }
236 0 : for (uint8_t i=0; i<4; i++) {
237 0 : if (!send_cmd16(0xC7F7)) {
238 : debug("ICM20789: read cal2[%u] failed\n", i);
239 0 : return false;
240 : }
241 : uint8_t d[3];
242 0 : if (!dev->transfer(nullptr, 0, d, sizeof(d))) {
243 : debug("ICM20789: read cal3[%u] failed\n", i);
244 0 : return false;
245 : }
246 0 : sensor_constants[i] = int16_t((d[0]<<8) | d[1]);
247 : debug("sensor_constants[%u]=%d\n", i, sensor_constants[i]);
248 : }
249 0 : return true;
250 : }
251 :
252 0 : void AP_Baro_ICM20789::calculate_conversion_constants(const float p_Pa[3], const float p_LUT[3],
253 : float &A, float &B, float &C)
254 : {
255 0 : C = (p_LUT[0] * p_LUT[1] * (p_Pa[0] - p_Pa[1]) +
256 0 : p_LUT[1] * p_LUT[2] * (p_Pa[1] - p_Pa[2]) +
257 0 : p_LUT[2] * p_LUT[0] * (p_Pa[2] - p_Pa[0])) /
258 0 : (p_LUT[2] * (p_Pa[0] - p_Pa[1]) +
259 0 : p_LUT[0] * (p_Pa[1] - p_Pa[2]) +
260 0 : p_LUT[1] * (p_Pa[2] - p_Pa[0]));
261 0 : A = (p_Pa[0] * p_LUT[0] - p_Pa[1] * p_LUT[1] - (p_Pa[1] - p_Pa[0]) * C) / (p_LUT[0] - p_LUT[1]);
262 0 : B = (p_Pa[0] - A) * (p_LUT[0] + C);
263 0 : }
264 :
265 : /*
266 : Convert an output from a calibrated sensor to a pressure in Pa.
267 : Arguments:
268 : p_LSB -- Raw pressure data from sensor
269 : T_LSB -- Raw temperature data from sensor
270 : */
271 0 : float AP_Baro_ICM20789::get_pressure(uint32_t p_LSB, uint32_t T_LSB)
272 : {
273 0 : float t = T_LSB - 32768.0;
274 : float s[3];
275 0 : s[0] = LUT_lower + float(sensor_constants[0] * t * t) * quadr_factor;
276 0 : s[1] = offst_factor * sensor_constants[3] + float(sensor_constants[1] * t * t) * quadr_factor;
277 0 : s[2] = LUT_upper + float(sensor_constants[2] * t * t) * quadr_factor;
278 : float A, B, C;
279 0 : calculate_conversion_constants(p_Pa_calib, s, A, B, C);
280 0 : return A + B / (C + p_LSB);
281 : }
282 :
283 :
284 : #if BARO_ICM20789_DEBUG
285 : static struct {
286 : uint32_t Praw, Traw;
287 : float T, P;
288 : } dd;
289 : #endif
290 :
291 0 : void AP_Baro_ICM20789::convert_data(uint32_t Praw, uint32_t Traw)
292 : {
293 : // temperature is easy
294 0 : float T = -45 + (175.0f / (1U<<16)) * Traw;
295 :
296 : // pressure involves a few more calculations
297 0 : float P = get_pressure(Praw, Traw);
298 :
299 0 : if (!pressure_ok(P)) {
300 0 : return;
301 : }
302 :
303 0 : WITH_SEMAPHORE(_sem);
304 :
305 : #if BARO_ICM20789_DEBUG
306 : dd.Praw = Praw;
307 : dd.Traw = Traw;
308 : dd.P = P;
309 : dd.T = T;
310 : #endif
311 :
312 0 : accum.psum += P;
313 0 : accum.tsum += T;
314 0 : accum.count++;
315 0 : }
316 :
317 0 : void AP_Baro_ICM20789::timer(void)
318 : {
319 0 : uint8_t d[9] {};
320 0 : if (dev->transfer(nullptr, 0, d, sizeof(d))) {
321 : // ignore CRC bytes for now
322 0 : uint32_t Praw = (uint32_t(d[0]) << 16) | (uint32_t(d[1]) << 8) | d[3];
323 0 : uint32_t Traw = (uint32_t(d[6]) << 8) | d[7];
324 :
325 0 : convert_data(Praw, Traw);
326 0 : send_cmd16(CMD_READ_PT);
327 0 : last_measure_us = AP_HAL::micros();
328 : } else {
329 0 : if (AP_HAL::micros() - last_measure_us > CONVERSION_INTERVAL*3) {
330 : // lost a sample
331 0 : send_cmd16(CMD_READ_PT);
332 0 : last_measure_us = AP_HAL::micros();
333 : }
334 : }
335 0 : }
336 :
337 0 : void AP_Baro_ICM20789::update()
338 : {
339 : #if BARO_ICM20789_DEBUG
340 : // useful for debugging
341 : // @LoggerMessage: ICMB
342 : // @Description: ICM20789 diagnostics
343 : // @Field: TimeUS: Time since system startup
344 : // @Field: Traw: raw temperature from sensor
345 : // @Field: Praw: raw pressure from sensor
346 : // @Field: P: pressure
347 : // @Field: T: temperature
348 : AP::logger().WriteStreaming("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff",
349 : AP_HAL::micros64(),
350 : dd.Traw, dd.Praw, dd.P, dd.T);
351 : #endif
352 :
353 0 : WITH_SEMAPHORE(_sem);
354 :
355 0 : if (accum.count > 0) {
356 0 : _copy_to_frontend(instance, accum.psum/accum.count, accum.tsum/accum.count);
357 0 : accum.psum = accum.tsum = 0;
358 0 : accum.count = 0;
359 : }
360 0 : }
361 :
362 : #endif // AP_BARO_ICM20789_ENABLED
|
