APM:Libraries
AP_InertialSensor_LSM9DS1.cpp
Go to the documentation of this file.
1 /*
2  * This program is free software
3 */
4 #include <AP_HAL/AP_HAL.h>
5 
7 
8 #include <utility>
9 
10 #include <AP_HAL/GPIO.h>
11 
12 extern const AP_HAL::HAL& hal;
13 
14 #define WHO_AM_I 0x68
15 #define WHO_AM_I_M 0x3D
16 #define LSM9DS1_DRY_XG_PIN -1
17 
18 /*
19  * Accelerometer and Gyroscope registers
20 */
21 #define LSM9DS1XG_ACT_THS 0x04
22 # define LSM9DS1XG_ACT_THS_SLEEP_ON (0x1 << 7)
23 #define LSM9DS1XG_ACT_DUR 0x05
24 #define LSM9DS1XG_INT_GEN_CFG_XL 0x06
25 # define LSM9DS1XG_INT_GEN_CFG_XL_AOI_XL (0x1 << 7)
26 # define LSM9DS1XG_INT_GEN_CFG_XL_6D (0x1 << 6)
27 # define LSM9DS1XG_INT_GEN_CFG_XL_ZHIE_XL (0x1 << 5)
28 # define LSM9DS1XG_INT_GEN_CFG_XL_ZLIE_XL (0x1 << 4)
29 # define LSM9DS1XG_INT_GEN_CFG_XL_YHIE_XL (0x1 << 3)
30 # define LSM9DS1XG_INT_GEN_CFG_XL_YLIE_XL (0x1 << 2)
31 # define LSM9DS1XG_INT_GEN_CFG_XL_XHIE_XL (0x1 << 1)
32 # define LSM9DS1XG_INT_GEN_CFG_XL_XLIE_XL (0x1 << 0)
33 #define LSM9DS1XG_INT_GEN_THS_X_XL 0x07
34 #define LSM9DS1XG_INT_GEN_THS_Y_XL 0x08
35 #define LSM9DS1XG_INT_GEN_THS_Z_XL 0x09
36 #define LSM9DS1XG_INT_GEN_DUR_XL 0x0A
37 # define LSM9DS1XG_INT_GEN_DUR_XL_WAIT_XL (0x1 << 7)
38 #define LSM9DS1XG_REFERENCE_G 0x0B
39 #define LSM9DS1XG_INT1_CTRL 0x0C
40 # define LSM9DS1XG_INT1_CTRL_INT1_IG_G (0x1 << 7)
41 # define LSM9DS1XG_INT1_CTRL_INT_IG_XL (0x1 << 6)
42 # define LSM9DS1XG_INT1_CTRL_INT_FSS5 (0x1 << 5)
43 # define LSM9DS1XG_INT1_CTRL_INT_OVR (0x1 << 4)
44 # define LSM9DS1XG_INT1_CTRL_INT_FTH (0x1 << 3)
45 # define LSM9DS1XG_INT1_CTRL_INT_Boot (0x1 << 2)
46 # define LSM9DS1XG_INT1_CTRL_INT_DRDY_G (0x1 << 1)
47 # define LSM9DS1XG_INT1_CTRL_INT_DRDY_XL (0x1 << 0)
48 #define LSM9DS1XG_INT2_CTRL 0x0D
49 # define LSM9DS1XG_INT2_CTRL_INT2_INACT (0x1 << 7)
50 # define LSM9DS1XG_INT2_CTRL_INT2_FSS5 (0x1 << 5)
51 # define LSM9DS1XG_INT2_CTRL_INT2_OVR (0x1 << 4)
52 # define LSM9DS1XG_INT2_CTRL_INT2_FTH (0x1 << 3)
53 # define LSM9DS1XG_INT2_CTRL_INT2_DRDY_TEMP (0x1 << 2)
54 # define LSM9DS1XG_INT2_CTRL_INT2_DRDY_G (0x1 << 1)
55 # define LSM9DS1XG_INT2_CTRL_INT2_DRDY_XL (0x1 << 0)
56 #define LSM9DS1XG_WHO_AM_I 0x0F
57 #define LSM9DS1XG_CTRL_REG1_G 0x10
58 # define LSM9DS1XG_CTRL_REG1_G_ODR_G_14900mHz (0x1 << 5)
59 # define LSM9DS1XG_CTRL_REG1_G_ODR_G_59500mHz (0x2 << 5)
60 # define LSM9DS1XG_CTRL_REG1_G_ODR_G_119Hz (0x3 << 5)
61 # define LSM9DS1XG_CTRL_REG1_G_ODR_G_238Hz (0x4 << 5)
62 # define LSM9DS1XG_CTRL_REG1_G_ODR_G_476Hz (0x5 << 5)
63 # define LSM9DS1XG_CTRL_REG1_G_ODR_G_952Hz (0x6 << 5)
64 # define LSM9DS1XG_CTRL_REG1_FS_G_245DPS (0x0 << 3)
65 # define LSM9DS1XG_CTRL_REG1_FS_G_500DPS (0x1 << 3)
66 # define LSM9DS1XG_CTRL_REG1_FS_G_2000DPS (0x3 << 3)
67 #define LSM9DS1XG_CTRL_REG2_G 0x11
68 # define LSM9DS1XG_CTRL_REG2_G_INT_SEL_00 (0x0 << 2)
69 # define LSM9DS1XG_CTRL_REG2_G_INT_SEL_01 (0x1 << 2)
70 # define LSM9DS1XG_CTRL_REG2_G_INT_SEL_10 (0x2 << 2)
71 # define LSM9DS1XG_CTRL_REG2_G_INT_SEL_11 (0x3 << 2)
72 # define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_00 (0x0 << 0)
73 # define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_01 (0x1 << 0)
74 # define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_10 (0x2 << 0)
75 # define LSM9DS1XG_CTRL_REG2_G_OUT_SEL_11 (0x3 << 0)
76 #define LSM9DS1XG_CTRL_REG3_G 0x12
77 # define LSM9DS1XG_CTRL_REG3_G_LP_MODE (0x1 << 7)
78 # define LSM9DS1XG_CTRL_REG3_G_HP_EN (0x1 << 6)
79 #define LSM9DS1XG_ORIENT_CFG_G 0x13
80 #define LSM9DS1XG_INT_GEN_SRC_G 0x14
81 # define LSM9DS1XG_INT_GEN_SRC_G_IA_G (0x1 << 6)
82 # define LSM9DS1XG_INT_GEN_SRC_G_ZH_G (0x1 << 5)
83 # define LSM9DS1XG_INT_GEN_SRC_G_ZL_G (0x1 << 4)
84 # define LSM9DS1XG_INT_GEN_SRC_G_YH_G (0x1 << 3)
85 # define LSM9DS1XG_INT_GEN_SRC_G_YL_G (0x1 << 2)
86 # define LSM9DS1XG_INT_GEN_SRC_G_XH_G (0x1 << 1)
87 # define LSM9DS1XG_INT_GEN_SRC_G_XL_G (0x1 << 0)
88 #define LSM9DS1XG_OUT_TEMP_L 0x15
89 #define LSM9DS1XG_OUT_TEMP_H 0x16
90 #define LSM9DS1XG_STATUS_REG 0x17
91 # define LSM9DS1XG_STATUS_REG_IG_XL (0x1 << 6)
92 # define LSM9DS1XG_STATUS_REG_IG_G (0x1 << 5)
93 # define LSM9DS1XG_STATUS_REG_INACT (0x1 << 4)
94 # define LSM9DS1XG_STATUS_REG_BOOT_STATUS (0x1 << 3)
95 # define LSM9DS1XG_STATUS_REG_TDA (0x1 << 2)
96 # define LSM9DS1XG_STATUS_REG_GDA (0x1 << 1)
97 # define LSM9DS1XG_STATUS_REG_XLDA (0x1 << 0)
98 #define LSM9DS1XG_OUT_X_L_G 0x18
99 #define LSM9DS1XG_OUT_X_H_G 0x19
100 #define LSM9DS1XG_OUT_Y_L_G 0x1A
101 #define LSM9DS1XG_OUT_Y_H_G 0x1B
102 #define LSM9DS1XG_OUT_Z_L_G 0x1C
103 #define LSM9DS1XG_OUT_Z_H_G 0x1D
104 #define LSM9DS1XG_CTRL_REG4 0x1E
105 # define LSM9DS1XG_CTRL_REG4_Zen_G (0x1 << 5)
106 # define LSM9DS1XG_CTRL_REG4_Yen_G (0x1 << 4)
107 # define LSM9DS1XG_CTRL_REG4_Xen_G (0x1 << 3)
108 # define LSM9DS1XG_CTRL_REG4_LIR_XL1 (0x1 << 1)
109 # define LSM9DS1XG_CTRL_REG4_4D_XL1 (0x1 << 0)
110 #define LSM9DS1XG_CTRL_REG5_XL 0x1F
111 # define LSM9DS1XG_CTRL_REG5_XL_Zen_XL (0x1 << 5)
112 # define LSM9DS1XG_CTRL_REG5_XL_Yen_XL (0x1 << 4)
113 # define LSM9DS1XG_CTRL_REG5_XL_Xen_XL (0x1 << 3)
114 #define LSM9DS1XG_CTRL_REG6_XL 0x20
115 # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_10Hz (0x1 << 5)
116 # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_50Hz (0x2 << 5)
117 # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_119Hz (0x3 << 5)
118 # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_238Hz (0x4 << 5)
119 # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_476Hz (0x5 << 5)
120 # define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_952Hz (0x6 << 5)
121 # define LSM9DS1XG_CTRL_REG6_XL_FS_XL_2G (0x0 << 3)
122 # define LSM9DS1XG_CTRL_REG6_XL_FS_XL_16G (0x1 << 3)
123 # define LSM9DS1XG_CTRL_REG6_XL_FS_XL_4G (0x2 << 3)
124 # define LSM9DS1XG_CTRL_REG6_XL_FS_XL_8G (0x3 << 3)
125 # define LSM9DS1XG_CTRL_REG6_XL_BW_SCAL_ODR (0x1 << 2)
126 # define LSM9DS1XG_CTRL_REG6_XL_BW_XL_408Hz (0x0 << 0)
127 # define LSM9DS1XG_CTRL_REG6_XL_BW_XL_211Hz (0x1 << 0)
128 # define LSM9DS1XG_CTRL_REG6_XL_BW_XL_105Hz (0x2 << 0)
129 # define LSM9DS1XG_CTRL_REG6_XL_BW_XL_50Hz (0x3 << 0)
130 #define LSM9DS1XG_CTRL_REG7_XL 0x21
131 # define LSM9DS1XG_CTRL_REG7_XL_FDS (0x1 << 2)
132 # define LSM9DS1XG_CTRL_REG7_XL_HPIS1 (0x1 << 0)
133 #define LSM9DS1XG_CTRL_REG8 0x22
134 # define LSM9DS1XG_CTRL_REG8_BOOT (0x1 << 7)
135 # define LSM9DS1XG_CTRL_REG8_BDU (0x1 << 6)
136 # define LSM9DS1XG_CTRL_REG8_H_LACTIVE (0x1 << 5)
137 # define LSM9DS1XG_CTRL_REG8_PP_OD (0x1 << 4)
138 # define LSM9DS1XG_CTRL_REG8_SIM (0x1 << 3)
139 # define LSM9DS1XG_CTRL_REG8_IF_ADD_INC (0x1 << 2)
140 # define LSM9DS1XG_CTRL_REG8_BLE (0x1 << 1)
141 # define LSM9DS1XG_CTRL_REG8_SW_RESET (0x1 << 0)
142 #define LSM9DS1XG_CTRL_REG9 0x23
143 # define LSM9DS1XG_CTRL_REG9_SLEEP_G (0x1 << 6)
144 # define LSM9DS1XG_CTRL_REG9_FIFO_TEMP_EN (0x1 << 4)
145 # define LSM9DS1XG_CTRL_REG9_DRDY_mask_bit (0x1 << 3)
146 # define LSM9DS1XG_CTRL_REG9_I2C_DISABLE (0x1 << 2)
147 # define LSM9DS1XG_CTRL_REG9_FIFO_EN (0x1 << 1)
148 # define LSM9DS1XG_CTRL_REG9_STOP_ON_FTH (0x1 << 0)
149 #define LSM9DS1XG_CTRL_REG10 0x24
150 # define LSM9DS1XG_CTRL_REG10_ST_G (0x1 << 2)
151 # define LSM9DS1XG_CTRL_REG10_ST_XL (0x1 << 0)
152 #define LSM9DS1XG_INT_GEN_SRC_XL 0x26
153 # define LSM9DS1XG_INT_GEN_SRC_XL_IA_XL (0x1 << 6)
154 # define LSM9DS1XG_INT_GEN_SRC_XL_ZH_XL (0x1 << 5)
155 # define LSM9DS1XG_INT_GEN_SRC_XL_ZL_XL (0x1 << 4)
156 # define LSM9DS1XG_INT_GEN_SRC_XL_YH_XL (0x1 << 3)
157 # define LSM9DS1XG_INT_GEN_SRC_XL_YL_XL (0x1 << 2)
158 # define LSM9DS1XG_INT_GEN_SRC_XL_XH_XL (0x1 << 1)
159 # define LSM9DS1XG_INT_GEN_SRC_XL_XL_XL (0x1 << 0)
160 //#define LSM9DS1XG_STATUS_REG 0x27 //repeat
161 #define LSM9DS1XG_OUT_X_L_XL 0x28
162 #define LSM9DS1XG_OUT_X_H_XL 0x29
163 #define LSM9DS1XG_OUT_Y_L_XL 0x2A
164 #define LSM9DS1XG_OUT_Y_H_XL 0x2B
165 #define LSM9DS1XG_OUT_Z_L_XL 0x2C
166 #define LSM9DS1XG_OUT_Z_H_XL 0x2D
167 #define LSM9DS1XG_FIFO_CTRL 0x2E
168 # define LSM9DS1XG_FIFO_CTRL_FMODE_BYPASS (0x0 << 5)
169 # define LSM9DS1XG_FIFO_CTRL_FMODE_FIFO (0x1 << 5)
170 # define LSM9DS1XG_FIFO_CTRL_FMODE_STREAM (0x3 << 5)
171 # define LSM9DS1XG_FIFO_CTRL_FMODE_B_TO_S (0x4 << 5)
172 # define LSM9DS1XG_FIFO_CTRL_FMODE_CON (0x5 << 5)
173 #define LSM9DS1XG_FIFO_SRC 0x2F
174 # define LSM9DS1XG_FIFO_SRC_FTH (0x1 << 7)
175 # define LSM9DS1XG_FIFO_SRC_OVRN (0x1 << 6)
176 # define LSM9DS1XG_FIFO_SRC_UNREAD_SAMPLES 0x3F
177 #define LSM9DS1XG_INT_GEN_CFG_G 0x30
178 # define LSM9DS1XG_INT_GEN_CFG_G_AOI_G (0x1 << 7)
179 # define LSM9DS1XG_INT_GEN_CFG_G_LIR_G (0x1 << 6)
180 # define LSM9DS1XG_INT_GEN_CFG_G_ZHIE_G (0x1 << 5)
181 # define LSM9DS1XG_INT_GEN_CFG_G_ZLIE_G (0x1 << 4)
182 # define LSM9DS1XG_INT_GEN_CFG_G_YHIE_G (0x1 << 3)
183 # define LSM9DS1XG_INT_GEN_CFG_G_YLIE_G (0x1 << 2)
184 # define LSM9DS1XG_INT_GEN_CFG_G_XHIE_G (0x1 << 1)
185 # define LSM9DS1XG_INT_GEN_CFG_G_XLIE_G (0x1 << 0)
186 #define LSM9DS1XG_INT_GEN_THS_XH_G 0x31
187 # define LSM9DS1XG_INT_GEN_THS_XH_G_DCRM_G (0x1 << 7)
188 #define LSM9DS1XG_INT_GEN_THS_XL_G 0x32
189 #define LSM9DS1XG_INT_GEN_THS_YH_G 0x33
190 #define LSM9DS1XG_INT_GEN_THS_YL_G 0x34
191 #define LSM9DS1XG_INT_GEN_THS_ZH_G 0x35
192 #define LSM9DS1XG_INT_GEN_THS_ZL_G 0x36
193 #define LSM9DS1XG_INT_GEN_DUR_G 0x37
194 # define LSM9DS1XG_INT_GEN_DUR_G_WAIT_G (0x1 << 7)
195 
198  int drdy_pin_num_xg,
199  enum Rotation rotation)
201  , _dev(std::move(dev))
202  , _drdy_pin_num_xg(drdy_pin_num_xg)
203  , _rotation(rotation)
204 
205 {
206 }
207 
210  enum Rotation rotation)
211 {
212  if (!dev) {
213  return nullptr;
214  }
215 
216  AP_InertialSensor_LSM9DS1 *sensor =
217  new AP_InertialSensor_LSM9DS1(_imu,std::move(dev),
219  rotation);
220  if (!sensor || !sensor->_init_sensor()) {
221  delete sensor;
222  return nullptr;
223  }
224  return sensor;
225 }
226 
228 {
230 
231  if (_drdy_pin_num_xg >= 0) {
233  if (_drdy_pin_xg == nullptr) {
234  AP_HAL::panic("LSM9DS1: null accel data-ready GPIO channel\n");
235  }
237  }
238 
239  bool success = _hardware_init();
240 
241 #if LSM9DS1_DEBUG
242  _dump_registers();
243 #endif
244  return success;
245 }
246 
248 {
250  return false;
251  }
252 
253  uint8_t tries, whoami;
254 
255  // set flag for reading registers
256  _dev->set_read_flag(0x80);
257 
259  if (whoami != WHO_AM_I) {
260  hal.console->printf("LSM9DS1: unexpected acc/gyro WHOAMI 0x%x\n", whoami);
261  goto fail_whoami;
262  }
263 
264  _fifo_reset();
265 
266  for (tries = 0; tries < 5; tries++) {
267 
269 
270  _gyro_init();
271  _accel_init();
272 
274 
275  hal.scheduler->delay(10);
276 
277  int samples = _register_read(LSM9DS1XG_FIFO_SRC);
278 
279  //if samples == 0 -> FIFO empty
280  if (samples > 1) {
281  break;
282  }
283 
284 #if LSM9DS1_DEBUG
285  _dump_registers();
286 #endif
287  }
288  if (tries == 5) {
289  hal.console->printf("Failed to boot LSM9DS1 5 times\n\n");
290  goto fail_tries;
291  }
292 
293  _spi_sem->give();
294  return true;
295 
296 fail_tries:
297 fail_whoami:
298  _spi_sem->give();
299  return false;
300 }
301 
303 {
305 
306  //Disable FIFO
307  int reg_9 = _register_read(LSM9DS1XG_CTRL_REG9);
308  _dev->write_register(LSM9DS1XG_CTRL_REG9, reg_9 & ~0x02);
309 
310  //Enable Bypass for reset FIFO
312 
313  //Enable FIFO and Disable I2C
315 
316  //Enable block data update, allow auto-increment during multiple byte read
319 
320  //Enable FIFO stream mode and set watermark at 32 samples
323 
325 
328 }
329 
330 /*
331  start the sensor going
332  */
334 {
337 
340 
342 
343  /* start the timer process to read samples */
345 }
346 
348 {
349  uint8_t val = 0;
350  _dev->read_registers(reg, &val, 1);
351  return val;
352 }
353 
354 
355 void AP_InertialSensor_LSM9DS1::_register_write(uint8_t reg, uint8_t val, bool checked)
356 {
357  _dev->write_register(reg, val, checked);
358 }
359 
361 {
364  hal.scheduler->delay(1);
365 
369  _set_gyro_scale();
370  hal.scheduler->delay(1);
371 }
372 
374 {
377  hal.scheduler->delay(1);
378 
383  hal.scheduler->delay(1);
384 }
385 
387 {
388  /* scale value from datasheet 2000 mdps/digit */
389  _gyro_scale = 70;
390  /* convert mdps/digit to dps/digit */
391  _gyro_scale /= 1000;
392  /* convert dps/digit to (rad/s)/digit */
394 }
395 
397 {
398  /*
399  * Possible accelerometer scales (and their register bit settings) are:
400  * 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an
401  * algorithm to calculate g/(ADC tick) based on that 3-bit value:
402  */
403  _accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f;
404  if (scale == A_SCALE_16G) {
405  /* the datasheet shows an exception for +-16G */
406  _accel_scale = 0.000732;
407  }
408  /* convert to G/LSB to (m/s/s)/LSB */
410 }
411 
416 {
417  uint16_t samples = _register_read(LSM9DS1XG_FIFO_SRC);
418 
419 
420  samples = samples & LSM9DS1XG_FIFO_SRC_UNREAD_SAMPLES;
421  if (samples > 1) {
422  _read_data_transaction_g(samples);
423  _read_data_transaction_x(samples);
424  }
425 
426  if (samples == 32) {
427  _fifo_reset();
428  }
429 
430  // check next register value for correctness
431  if (!_dev->check_next_register()) {
433  }
434 }
435 
437 {
438  struct sensor_raw_data raw_data = { };
439  int32_t _accel_bias[3] = {0, 0, 0};
440 
441  const uint8_t _reg = LSM9DS1XG_OUT_X_L_XL | 0x80;
442 
443  // Read the accel data stored in the FIFO
444  for (int i = 0; i < samples; i++)
445  {
446  struct sensor_raw_data raw_data_temp = { };
447 
448  if (!_dev->transfer(&_reg, 1, (uint8_t *) &raw_data_temp, sizeof(raw_data_temp))) {
449  hal.console->printf("LSM9DS1: error reading accelerometer\n");
450  return;
451  }
452 
453  _accel_bias[0] += (int32_t) raw_data_temp.x; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
454  _accel_bias[1] += (int32_t) raw_data_temp.y;
455  _accel_bias[2] += (int32_t) raw_data_temp.z;
456  }
457 
458  raw_data.x = _accel_bias[0] / samples; // average the data
459  raw_data.y = _accel_bias[1] / samples;
460  raw_data.z = _accel_bias[2] / samples;
461 
462 
463  Vector3f accel_data(raw_data.x, raw_data.y, -raw_data.z);
464  accel_data *= _accel_scale;
465 
468 }
469 
470 /*
471  * read from the data registers and update filtered data
472  */
474 {
475  struct sensor_raw_data raw_data = { };
476  int32_t _gyro_bias[3] = {0, 0, 0};
477 
478  const uint8_t _reg = LSM9DS1XG_OUT_X_L_G | 0x80;
479 
480  // Read the gyro data stored in the FIFO
481  for (int i = 0; i < samples; i++)
482  {
483  struct sensor_raw_data raw_data_temp = { };
484 
485  if (!_dev->transfer(&_reg, 1, (uint8_t *) &raw_data_temp, sizeof(raw_data_temp))) {
486  hal.console->printf("LSM9DS1: error reading gyroscope\n");
487  return;
488  }
489 
490  _gyro_bias[0] += (int32_t) raw_data_temp.x; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
491  _gyro_bias[1] += (int32_t) raw_data_temp.y;
492  _gyro_bias[2] += (int32_t) raw_data_temp.z;
493  }
494 
495  raw_data.x = _gyro_bias[0] / samples; // average the data
496  raw_data.y = _gyro_bias[1] / samples;
497  raw_data.z = _gyro_bias[2] / samples;
498 
499  Vector3f gyro_data(raw_data.x, raw_data.y, -raw_data.z);
500  gyro_data *= _gyro_scale;
501 
504 }
505 
507 {
510 
511  return true;
512 }
513 
514 #if LSM9DS1_DEBUG
515 /*
516  * dump all config registers - used for debug
517 */
518 void AP_InertialSensor_LSM9DS1::_dump_registers(void)
519 {
520  hal.console->println("LSM9DS1 registers:");
521 
522  const uint8_t first = LSM9DS1XG_ACT_THS;
523  const uint8_t last = LSM9DS1XG_INT_GEN_DUR_G;
524  for (uint8_t reg=first; reg<=last; reg++) {
525  uint8_t v = _register_read(reg);
526  hal.console->printf("%02x:%02x ", reg, v);
527  if ((reg - (first-1)) % 16 == 0) {
528  hal.console->println();
529  }
530  }
531  hal.console->println();
532 }
533 #endif
#define LSM9DS1XG_WHO_AM_I
void set_gyro_orientation(uint8_t instance, enum Rotation rotation)
AP_HAL::DigitalSource * _drdy_pin_xg
#define WHO_AM_I
#define LSM9DS1XG_CTRL_REG6_XL
#define LSM9DS1XG_CTRL_REG9
void _inc_accel_error_count(uint8_t instance)
AP_HAL::UARTDriver * console
Definition: HAL.h:110
#define LSM9DS1XG_CTRL_REG8_BDU
#define LSM9DS1XG_FIFO_SRC_UNREAD_SAMPLES
virtual Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, Device::PeriodicCb) override=0
uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id)
#define LSM9DS1XG_CTRL_REG6_XL_ODR_XL_952Hz
#define LSM9DS1XG_CTRL_REG4_Zen_G
#define LSM9DS1XG_CTRL_REG1_FS_G_2000DPS
static AP_InertialSensor_Backend * probe(AP_InertialSensor &imu, AP_HAL::OwnPtr< AP_HAL::SPIDevice > dev, enum Rotation rotation=ROTATION_NONE)
void notify_accel_fifo_reset(uint8_t instance)
#define HAL_SEMAPHORE_BLOCK_FOREVER
Definition: Semaphores.h:5
#define LSM9DS1XG_ACT_THS
virtual bool take(uint32_t timeout_ms) WARN_IF_UNUSED=0
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
Rotation
Definition: rotations.h:27
uint32_t get_bus_id_devtype(uint8_t devtype)
Definition: Device.h:255
AP_HAL::OwnPtr< AP_HAL::SPIDevice > _dev
#define LSM9DS1XG_FIFO_CTRL_FMODE_BYPASS
#define DEG_TO_RAD
Definition: definitions.h:30
#define LSM9DS1XG_CTRL_REG5_XL_Yen_XL
virtual void mode(uint8_t output)=0
void _set_accel_scale(accel_scale scale)
virtual void delay(uint16_t ms)=0
virtual void printf(const char *,...) FMT_PRINTF(2
Definition: BetterStream.cpp:5
uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id)
void _set_accel_max_abs_offset(uint8_t instance, float offset)
virtual Semaphore * get_semaphore() override=0
static AP_HAL::OwnPtr< AP_HAL::Device > dev
Definition: ICM20789.cpp:16
#define LSM9DS1XG_CTRL_REG1_G_ODR_G_952Hz
#define GRAVITY_MSS
Definition: definitions.h:47
#define f(i)
#define LSM9DS1XG_CTRL_REG5_XL_Xen_XL
#define LSM9DS1XG_OUT_X_L_XL
#define LSM9DS1XG_FIFO_SRC
#define LSM9DS1XG_CTRL_REG5_XL
virtual AP_HAL::DigitalSource * channel(uint16_t n)=0
void println(const char *str)
Definition: BetterStream.h:34
#define LSM9DS1XG_FIFO_CTRL
void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0)
#define LSM9DS1XG_INT_GEN_DUR_G
void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro)
#define LSM9DS1XG_OUT_X_L_G
virtual bool give()=0
#define LSM9DS1XG_CTRL_REG4_Xen_G
float v
Definition: Printf.cpp:15
#define LSM9DS1XG_CTRL_REG9_FIFO_EN
void _read_data_transaction_x(uint16_t samples)
#define LSM9DS1XG_CTRL_REG1_G
#define LSM9DS1XG_CTRL_REG6_XL_FS_XL_16G
#define LSM9DS1XG_CTRL_REG4
void _read_data_transaction_g(uint16_t samples)
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false)
void notify_gyro_fifo_reset(uint8_t instance)
#define LSM9DS1_DRY_XG_PIN
#define LSM9DS1XG_CTRL_REG5_XL_Zen_XL
#define HAL_GPIO_INPUT
Definition: GPIO.h:7
virtual bool set_speed(Device::Speed speed) override=0
virtual bool transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) override=0
#define LSM9DS1XG_CTRL_REG8_IF_ADD_INC
#define LSM9DS1XG_CTRL_REG8
void set_read_flag(uint8_t flag)
Definition: Device.h:221
void set_accel_orientation(uint8_t instance, enum Rotation rotation)
#define LSM9DS1XG_CTRL_REG9_I2C_DISABLE
#define LSM9DS1XG_CTRL_REG4_Yen_G
AP_HAL::GPIO * gpio
Definition: HAL.h:111
#define LSM9DS1XG_FIFO_CTRL_FMODE_FIFO
virtual void delay_microseconds(uint16_t us)=0
bool check_next_register(void)
Definition: Device.cpp:85
bool read_registers(uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
Definition: Device.h:113
#define FUNCTOR_BIND_MEMBER(func, rettype,...)
Definition: functor.h:31
void panic(const char *errormsg,...) FMT_PRINTF(1
Definition: system.cpp:140
AP_InertialSensor_LSM9DS1(AP_InertialSensor &imu, AP_HAL::OwnPtr< AP_HAL::SPIDevice > dev, int drdy_pin_num_xg, enum Rotation rotation)
void _register_write(uint8_t reg, uint8_t val, bool checked=false)
bool write_register(uint8_t reg, uint8_t val, bool checked=false)
Definition: Device.h:125
void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel)
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114