forked from ArduPilot/ardupilot
/
AP_InertialSensor_LSM9DS0.cpp
813 lines (727 loc) · 33 KB
/
AP_InertialSensor_LSM9DS0.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
/*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor_LSM9DS0.h"
#include <utility>
extern const AP_HAL::HAL &hal;
#define LSM9DS0_DRY_X_PIN -1
#define LSM9DS0_DRY_G_PIN -1
#define LSM9DS0_G_WHOAMI 0xD4 // L3GD20
#define LSM9DS0_G_WHOAMI_H 0xD7 // L3GD20H
#define LSM9DS0_XM_WHOAMI 0x49
////////////////////////////
// LSM9DS0 Gyro Registers //
////////////////////////////
#define WHO_AM_I_G 0x0F
#define CTRL_REG1_G 0x20
# define CTRL_REG1_G_DR_95Hz_BW_12500mHz (0x0 << 4)
# define CTRL_REG1_G_DR_95Hz_BW_25Hz (0x1 << 4)
# define CTRL_REG1_G_DR_190Hz_BW_12500mHz (0x4 << 4)
# define CTRL_REG1_G_DR_190Hz_BW_25Hz (0x5 << 4)
# define CTRL_REG1_G_DR_190Hz_BW_50Hz (0x6 << 4)
# define CTRL_REG1_G_DR_190Hz_BW_70Hz (0x7 << 4)
# define CTRL_REG1_G_DR_380Hz_BW_20Hz (0x8 << 4)
# define CTRL_REG1_G_DR_380Hz_BW_25Hz (0x9 << 4)
# define CTRL_REG1_G_DR_380Hz_BW_50Hz (0xA << 4)
# define CTRL_REG1_G_DR_380Hz_BW_100Hz (0xB << 4)
# define CTRL_REG1_G_DR_760Hz_BW_30Hz (0xC << 4)
# define CTRL_REG1_G_DR_760Hz_BW_35Hz (0xD << 4)
# define CTRL_REG1_G_DR_760Hz_BW_50Hz (0xE << 4)
# define CTRL_REG1_G_DR_760Hz_BW_100Hz (0xF << 4)
# define CTRL_REG1_G_PD (0x1 << 3)
# define CTRL_REG1_G_ZEN (0x1 << 2)
# define CTRL_REG1_G_YEN (0x1 << 1)
# define CTRL_REG1_G_XEN (0x1 << 0)
#define CTRL_REG2_G 0x21
# define CTRL_REG2_G_HPM_NORMAL_RESET (0x0 << 4)
# define CTRL_REG2_G_HPM_REFERENCE (0x1 << 4)
# define CTRL_REG2_G_HPM_NORMAL (0x2 << 4)
# define CTRL_REG2_G_HPM_AUTORESET (0x3 << 4)
# define CTRL_REG2_G_HPCF_0 (0x0 << 0)
# define CTRL_REG2_G_HPCF_1 (0x1 << 0)
# define CTRL_REG2_G_HPCF_2 (0x2 << 0)
# define CTRL_REG2_G_HPCF_3 (0x3 << 0)
# define CTRL_REG2_G_HPCF_4 (0x4 << 0)
# define CTRL_REG2_G_HPCF_5 (0x5 << 0)
# define CTRL_REG2_G_HPCF_6 (0x6 << 0)
# define CTRL_REG2_G_HPCF_7 (0x7 << 0)
# define CTRL_REG2_G_HPCF_8 (0x8 << 0)
# define CTRL_REG2_G_HPCF_9 (0x9 << 0)
#define CTRL_REG3_G 0x22
# define CTRL_REG3_G_I1_INT1 (0x1 << 7)
# define CTRL_REG3_G_I1_BOOT (0x1 << 6)
# define CTRL_REG3_G_H_LACTIVE (0x1 << 5)
# define CTRL_REG3_G_PP_OD (0x1 << 4)
# define CTRL_REG3_G_I2_DRDY (0x1 << 3)
# define CTRL_REG3_G_I2_WTM (0x1 << 2)
# define CTRL_REG3_G_I2_ORUN (0x1 << 1)
# define CTRL_REG3_G_I2_EMPTY (0x1 << 0)
#define CTRL_REG4_G 0x23
# define CTRL_REG4_G_BDU (0x1 << 7)
# define CTRL_REG4_G_BLE (0x1 << 6)
# define CTRL_REG4_G_FS_245DPS (0x0 << 4)
# define CTRL_REG4_G_FS_500DPS (0x1 << 4)
# define CTRL_REG4_G_FS_2000DPS (0x2 << 4)
# define CTRL_REG4_G_ST_NORMAL (0x0 << 1)
# define CTRL_REG4_G_ST_0 (0x1 << 1)
# define CTRL_REG4_G_ST_1 (0x3 << 1)
# define CTRL_REG4_G_SIM_3WIRE (0x1 << 0)
#define CTRL_REG5_G 0x24
# define CTRL_REG5_G_BOOT (0x1 << 7)
# define CTRL_REG5_G_FIFO_EN (0x1 << 6)
# define CTRL_REG5_G_HPEN (0x1 << 4)
# define CTRL_REG5_G_INT1_SEL_00 (0x0 << 2)
# define CTRL_REG5_G_INT1_SEL_01 (0x1 << 2)
# define CTRL_REG5_G_INT1_SEL_10 (0x2 << 2)
# define CTRL_REG5_G_INT1_SEL_11 (0x3 << 2)
# define CTRL_REG5_G_OUT_SEL_00 (0x0 << 0)
# define CTRL_REG5_G_OUT_SEL_01 (0x1 << 0)
# define CTRL_REG5_G_OUT_SEL_10 (0x2 << 0)
# define CTRL_REG5_G_OUT_SEL_11 (0x3 << 0)
#define REFERENCE_G 0x25
#define STATUS_REG_G 0x27
# define STATUS_REG_G_ZYXOR (0x1 << 7)
# define STATUS_REG_G_ZOR (0x1 << 6)
# define STATUS_REG_G_YOR (0x1 << 5)
# define STATUS_REG_G_XOR (0x1 << 4)
# define STATUS_REG_G_ZYXDA (0x1 << 3)
# define STATUS_REG_G_ZDA (0x1 << 2)
# define STATUS_REG_G_YDA (0x1 << 1)
# define STATUS_REG_G_XDA (0x1 << 0)
#define OUT_X_L_G 0x28
#define OUT_X_H_G 0x29
#define OUT_Y_L_G 0x2A
#define OUT_Y_H_G 0x2B
#define OUT_Z_L_G 0x2C
#define OUT_Z_H_G 0x2D
#define FIFO_CTRL_REG_G 0x2E
# define FIFO_CTRL_REG_G_FM_BYPASS (0x0 << 5)
# define FIFO_CTRL_REG_G_FM_FIFO (0x1 << 5)
# define FIFO_CTRL_REG_G_FM_STREAM (0x2 << 5)
# define FIFO_CTRL_REG_G_FM_STREAM_TO_FIFO (0x3 << 5)
# define FIFO_CTRL_REG_G_FM_BYPASS_TO_STREAM (0x4 << 5)
# define FIFO_CTRL_REG_G_WTM_MASK 0x1F
#define FIFO_SRC_REG_G 0x2F
# define FIFO_SRC_REG_G_WTM (0x1 << 7)
# define FIFO_SRC_REG_G_OVRN (0x1 << 6)
# define FIFO_SRC_REG_G_EMPTY (0x1 << 5)
# define FIFO_SRC_REG_G_FSS_MASK 0x1F
#define INT1_CFG_G 0x30
# define INT1_CFG_G_AND_OR (0x1 << 7)
# define INT1_CFG_G_LIR (0x1 << 6)
# define INT1_CFG_G_ZHIE (0x1 << 5)
# define INT1_CFG_G_ZLIE (0x1 << 4)
# define INT1_CFG_G_YHIE (0x1 << 3)
# define INT1_CFG_G_YLIE (0x1 << 2)
# define INT1_CFG_G_XHIE (0x1 << 1)
# define INT1_CFG_G_XLIE (0x1 << 0)
#define INT1_SRC_G 0x31
# define INT1_SRC_G_IA (0x1 << 6)
# define INT1_SRC_G_ZH (0x1 << 5)
# define INT1_SRC_G_ZL (0x1 << 4)
# define INT1_SRC_G_YH (0x1 << 3)
# define INT1_SRC_G_YL (0x1 << 2)
# define INT1_SRC_G_XH (0x1 << 1)
# define INT1_SRC_G_XL (0x1 << 0)
#define INT1_THS_XH_G 0x32
#define INT1_THS_XL_G 0x33
#define INT1_THS_YH_G 0x34
#define INT1_THS_YL_G 0x35
#define INT1_THS_ZH_G 0x36
#define INT1_THS_ZL_G 0x37
#define INT1_DURATION_G 0x38
# define INT1_DURATION_G_WAIT (0x1 << 7)
# define INT1_DURATION_G_D_MASK 0x7F
//////////////////////////////////////////
// LSM9DS0 Accel/Magneto (XM) Registers //
//////////////////////////////////////////
#define OUT_TEMP_L_XM 0x05
#define OUT_TEMP_H_XM 0x06
#define STATUS_REG_M 0x07
# define STATUS_REG_M_ZYXMOR (0x1 << 7)
# define STATUS_REG_M_ZMOR (0x1 << 6)
# define STATUS_REG_M_YMOR (0x1 << 5)
# define STATUS_REG_M_XMOR (0x1 << 4)
# define STATUS_REG_M_ZYXMDA (0x1 << 3)
# define STATUS_REG_M_ZMDA (0x1 << 2)
# define STATUS_REG_M_YMDA (0x1 << 1)
# define STATUS_REG_M_XMDA (0x1 << 0)
#define OUT_X_L_M 0x08
#define OUT_X_H_M 0x09
#define OUT_Y_L_M 0x0A
#define OUT_Y_H_M 0x0B
#define OUT_Z_L_M 0x0C
#define OUT_Z_H_M 0x0D
#define WHO_AM_I_XM 0x0F
#define INT_CTRL_REG_M 0x12
# define INT_CTRL_REG_M_XMIEN (0x1 << 7)
# define INT_CTRL_REG_M_YMIEN (0x1 << 6)
# define INT_CTRL_REG_M_ZMIEN (0x1 << 5)
# define INT_CTRL_REG_M_PP_OD (0x1 << 4)
# define INT_CTRL_REG_M_IEA (0x1 << 3)
# define INT_CTRL_REG_M_IEL (0x1 << 2)
# define INT_CTRL_REG_M_4D (0x1 << 1)
# define INT_CTRL_REG_M_MIEN (0x1 << 0)
#define INT_SRC_REG_M 0x13
# define INT_SRC_REG_M_M_PTH_X (0x1 << 7)
# define INT_SRC_REG_M_M_PTH_Y (0x1 << 6)
# define INT_SRC_REG_M_M_PTH_Z (0x1 << 5)
# define INT_SRC_REG_M_M_NTH_X (0x1 << 4)
# define INT_SRC_REG_M_M_NTH_Y (0x1 << 3)
# define INT_SRC_REG_M_M_NTH_Z (0x1 << 2)
# define INT_SRC_REG_M_MROI (0x1 << 1)
# define INT_SRC_REG_M_MINT (0x1 << 0)
#define INT_THS_L_M 0x14
#define INT_THS_H_M 0x15
#define OFFSET_X_L_M 0x16
#define OFFSET_X_H_M 0x17
#define OFFSET_Y_L_M 0x18
#define OFFSET_Y_H_M 0x19
#define OFFSET_Z_L_M 0x1A
#define OFFSET_Z_H_M 0x1B
#define REFERENCE_X 0x1C
#define REFERENCE_Y 0x1D
#define REFERENCE_Z 0x1E
#define CTRL_REG0_XM 0x1F
# define CTRL_REG0_XM_B00T (0x1 << 7)
# define CTRL_REG0_XM_FIFO_EN (0x1 << 6)
# define CTRL_REG0_XM_WTM_EN (0x1 << 5)
# define CTRL_REG0_XM_HP_CLICK (0x1 << 2)
# define CTRL_REG0_XM_HPIS1 (0x1 << 1)
# define CTRL_REG0_XM_HPIS2 (0x1 << 0)
#define CTRL_REG1_XM 0x20
# define CTRL_REG1_XM_AODR_POWERDOWN (0x0 << 4)
# define CTRL_REG1_XM_AODR_3125mHz (0x1 << 4)
# define CTRL_REG1_XM_AODR_6250mHz (0x2 << 4)
# define CTRL_REG1_XM_AODR_12500mHz (0x3 << 4)
# define CTRL_REG1_XM_AODR_25Hz (0x4 << 4)
# define CTRL_REG1_XM_AODR_50Hz (0x5 << 4)
# define CTRL_REG1_XM_AODR_100Hz (0x6 << 4)
# define CTRL_REG1_XM_AODR_200Hz (0x7 << 4)
# define CTRL_REG1_XM_AODR_400Hz (0x8 << 4)
# define CTRL_REG1_XM_AODR_800Hz (0x9 << 4)
# define CTRL_REG1_XM_AODR_1600Hz (0xA << 4)
# define CTRL_REG1_XM_BDU (0x1 << 3)
# define CTRL_REG1_XM_AZEN (0x1 << 2)
# define CTRL_REG1_XM_AYEN (0x1 << 1)
# define CTRL_REG1_XM_AXEN (0x1 << 0)
#define CTRL_REG2_XM 0x21
# define CTRL_REG2_XM_ABW_773Hz (0x0 << 6)
# define CTRL_REG2_XM_ABW_194Hz (0x1 << 6)
# define CTRL_REG2_XM_ABW_362Hz (0x2 << 6)
# define CTRL_REG2_XM_ABW_50Hz (0x3 << 6)
# define CTRL_REG2_XM_AFS_2G (0x0 << 3)
# define CTRL_REG2_XM_AFS_4G (0x1 << 3)
# define CTRL_REG2_XM_AFS_6G (0x2 << 3)
# define CTRL_REG2_XM_AFS_8G (0x3 << 3)
# define CTRL_REG2_XM_AFS_16G (0x4 << 3)
# define CTRL_REG2_XM_AST_NORMAL (0x0 << 1)
# define CTRL_REG2_XM_AST_POSITIVE (0x1 << 1)
# define CTRL_REG2_XM_AST_NEGATIVE (0x2 << 1)
# define CTRL_REG2_XM_SIM_3WIRE (0x1 << 0)
#define CTRL_REG3_XM 0x22
# define CTRL_REG3_XM_P1_BOOT (0x1 << 7)
# define CTRL_REG3_XM_P1_TAP (0x1 << 6)
# define CTRL_REG3_XM_P1_INT1 (0x1 << 5)
# define CTRL_REG3_XM_P1_INT2 (0x1 << 4)
# define CTRL_REG3_XM_P1_INTM (0x1 << 3)
# define CTRL_REG3_XM_P1_DRDYA (0x1 << 2)
# define CTRL_REG3_XM_P1_DRDYM (0x1 << 1)
# define CTRL_REG3_XM_P1_EMPTY (0x1 << 0)
#define CTRL_REG4_XM 0x23
# define CTRL_REG4_XM_P2_TAP (0x1 << 7)
# define CTRL_REG4_XM_P2_INT1 (0x1 << 6)
# define CTRL_REG4_XM_P2_INT2 (0x1 << 5)
# define CTRL_REG4_XM_P2_INTM (0x1 << 4)
# define CTRL_REG4_XM_P2_DRDYA (0x1 << 3)
# define CTRL_REG4_XM_P2_DRDYM (0x1 << 2)
# define CTRL_REG4_XM_P2_OVERRUN (0x1 << 1)
# define CTRL_REG4_XM_P2_WTM (0x1 << 0)
#define CTRL_REG5_XM 0x24
# define CTRL_REG5_XM_TEMP_EN (0x1 << 7)
# define CTRL_REG5_XM_M_RES_LOW (0x0 << 5)
# define CTRL_REG5_XM_M_RES_HIGH (0x3 << 5)
# define CTRL_REG5_XM_ODR_3125mHz (0x0 << 2)
# define CTRL_REG5_XM_ODR_6250mHz (0x1 << 2)
# define CTRL_REG5_XM_ODR_12500mHz (0x2 << 2)
# define CTRL_REG5_XM_ODR_25Hz (0x3 << 2)
# define CTRL_REG5_XM_ODR_50Hz (0x4 << 2)
# define CTRL_REG5_XM_ODR_100Hz (0x5 << 2)
# define CTRL_REG5_XM_LIR2 (0x1 << 1)
# define CTRL_REG5_XM_LIR1 (0x1 << 0)
#define CTRL_REG6_XM 0x25
# define CTRL_REG6_XM_MFS_2Gs (0x0 << 5)
# define CTRL_REG6_XM_MFS_4Gs (0x1 << 5)
# define CTRL_REG6_XM_MFS_8Gs (0x2 << 5)
# define CTRL_REG6_XM_MFS_12Gs (0x3 << 5)
#define CTRL_REG7_XM 0x26
# define CTRL_REG7_XM_AHPM_NORMAL_RESET (0x0 << 6)
# define CTRL_REG7_XM_AHPM_REFERENCE (0x1 << 6)
# define CTRL_REG7_XM_AHPM_NORMAL (0x2 << 6)
# define CTRL_REG7_XM_AHPM_AUTORESET (0x3 << 6)
# define CTRL_REG7_XM_AFDS (0x1 << 5)
# define CTRL_REG7_XM_MLP (0x1 << 2)
# define CTRL_REG7_XM_MD_CONTINUOUS (0x0 << 0)
# define CTRL_REG7_XM_MD_SINGLE (0x1 << 0)
# define CTRL_REG7_XM_MD_POWERDOWN (0x2 << 0)
#define STATUS_REG_A 0x27
# define STATUS_REG_A_ZYXAOR (0x1 << 7)
# define STATUS_REG_A_ZAOR (0x1 << 6)
# define STATUS_REG_A_YAOR (0x1 << 5)
# define STATUS_REG_A_XAOR (0x1 << 4)
# define STATUS_REG_A_ZYXADA (0x1 << 3)
# define STATUS_REG_A_ZADA (0x1 << 2)
# define STATUS_REG_A_YADA (0x1 << 1)
# define STATUS_REG_A_XADA (0x1 << 0)
#define OUT_X_L_A 0x28
#define OUT_X_H_A 0x29
#define OUT_Y_L_A 0x2A
#define OUT_Y_H_A 0x2B
#define OUT_Z_L_A 0x2C
#define OUT_Z_H_A 0x2D
#define FIFO_CTRL_REG 0x2E
# define FIFO_CTRL_REG_FM_BYPASS (0x0 << 5)
# define FIFO_CTRL_REG_FM_FIFO (0x1 << 5)
# define FIFO_CTRL_REG_FM_STREAM (0x2 << 5)
# define FIFO_CTRL_REG_FM_STREAM_TO_FIFO (0x3 << 5)
# define FIFO_CTRL_REG_FM_BYPASS_TO_STREAM (0x4 << 5)
# define FIFO_CTRL_REG_FTH_MASK 0x1F
#define FIFO_SRC_REG 0x2F
# define FIFO_SRC_REG_WTM (0x1 << 7)
# define FIFO_SRC_REG_OVRN (0x1 << 6)
# define FIFO_SRC_REG_EMPTY (0x1 << 5)
# define FIFO_SRC_REG_FSS_MASK 0x1F
#define INT_GEN_1_REG 0x30
# define INT_GEN_1_REG_AOI (0x1 << 7)
# define INT_GEN_1_REG_6D (0x1 << 6)
# define INT_GEN_1_REG_ZHIE_ZUPE (0x1 << 5)
# define INT_GEN_1_REG_ZLIE_ZDOWNE (0x1 << 4)
# define INT_GEN_1_REG_YHIE_YUPE (0x1 << 3)
# define INT_GEN_1_REG_YLIE_YDOWNE (0x1 << 2)
# define INT_GEN_1_REG_XHIE_XUPE (0x1 << 1)
# define INT_GEN_1_REG_XLIE_XDOWNE (0x1 << 0)
#define INT_GEN_1_SRC 0x31
# define INT_GEN_1_SRC_IA (0x1 << 6)
# define INT_GEN_1_SRC_ZH (0x1 << 5)
# define INT_GEN_1_SRC_ZL (0x1 << 4)
# define INT_GEN_1_SRC_YH (0x1 << 3)
# define INT_GEN_1_SRC_YL (0x1 << 2)
# define INT_GEN_1_SRC_XH (0x1 << 1)
# define INT_GEN_1_SRC_XL (0x1 << 0)
#define INT_GEN_1_THS 0x32
#define INT_GEN_1_DURATION 0x33
#define INT_GEN_2_REG 0x34
# define INT_GEN_2_REG_AOI (0x1 << 7)
# define INT_GEN_2_REG_6D (0x1 << 6)
# define INT_GEN_2_REG_ZHIE_ZUPE (0x1 << 5)
# define INT_GEN_2_REG_ZLIE_ZDOWNE (0x1 << 4)
# define INT_GEN_2_REG_YHIE_YUPE (0x1 << 3)
# define INT_GEN_2_REG_YLIE_YDOWNE (0x1 << 2)
# define INT_GEN_2_REG_XHIE_XUPE (0x1 << 1)
# define INT_GEN_2_REG_XLIE_XDOWNE (0x1 << 0)
#define INT_GEN_2_SRC 0x35
# define INT_GEN_2_SRC_IA (0x1 << 6)
# define INT_GEN_2_SRC_ZH (0x1 << 5)
# define INT_GEN_2_SRC_ZL (0x1 << 4)
# define INT_GEN_2_SRC_YH (0x1 << 3)
# define INT_GEN_2_SRC_YL (0x1 << 2)
# define INT_GEN_2_SRC_XH (0x1 << 1)
# define INT_GEN_2_SRC_XL (0x1 << 0)
#define INT_GEN_2_THS 0x36
#define INT_GEN_2_DURATION 0x37
#define CLICK_CFG 0x38
# define CLICK_CFG_ZD (0x1 << 5)
# define CLICK_CFG_ZS (0x1 << 4)
# define CLICK_CFG_YD (0x1 << 3)
# define CLICK_CFG_YS (0x1 << 2)
# define CLICK_CFG_XD (0x1 << 1)
# define CLICK_CFG_XS (0x1 << 0)
#define CLICK_SRC 0x39
# define CLICK_SRC_IA (0x1 << 6)
# define CLICK_SRC_DCLICK (0x1 << 5)
# define CLICK_SRC_SCLICK (0x1 << 4)
# define CLICK_SRC_SIGN (0x1 << 3)
# define CLICK_SRC_Z (0x1 << 2)
# define CLICK_SRC_Y (0x1 << 1)
# define CLICK_SRC_X (0x1 << 0)
#define CLICK_THS 0x3A
#define TIME_LIMIT 0x3B
#define TIME_LATENCY 0x3C
#define TIME_WINDOW 0x3D
#define ACT_THS 0x3E
#define ACT_DUR 0x3F
AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::Device> dev_accel,
int drdy_pin_num_a,
int drdy_pin_num_g,
enum Rotation rotation_a,
enum Rotation rotation_g,
enum Rotation rotation_gH)
: AP_InertialSensor_Backend(imu)
, _dev_gyro(std::move(dev_gyro))
, _dev_accel(std::move(dev_accel))
, _drdy_pin_num_a(drdy_pin_num_a)
, _drdy_pin_num_g(drdy_pin_num_g)
, _rotation_a(rotation_a)
, _rotation_g(rotation_g)
, _rotation_gH(rotation_gH)
, _temp_filter(400, 1)
{
}
AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::Device> dev_accel,
enum Rotation rotation_a,
enum Rotation rotation_g,
enum Rotation rotation_gH)
{
if (!dev_gyro || !dev_accel) {
return nullptr;
}
AP_InertialSensor_LSM9DS0 *sensor =
new AP_InertialSensor_LSM9DS0(_imu, std::move(dev_gyro), std::move(dev_accel),
LSM9DS0_DRY_X_PIN, LSM9DS0_DRY_G_PIN,
rotation_a, rotation_g, rotation_gH);
if (!sensor || !sensor->_init_sensor()) {
delete sensor;
return nullptr;
}
return sensor;
}
bool AP_InertialSensor_LSM9DS0::_init_sensor()
{
/*
* Same semaphore for both since they necessarily share the same bus (with
* different CS)
*/
_spi_sem = _dev_gyro->get_semaphore();
if (_drdy_pin_num_a >= 0) {
_drdy_pin_a = hal.gpio->channel(_drdy_pin_num_a);
if (_drdy_pin_a == nullptr) {
AP_HAL::panic("LSM9DS0: null accel data-ready GPIO channel\n");
}
_drdy_pin_a->mode(HAL_GPIO_INPUT);
}
if (_drdy_pin_num_g >= 0) {
_drdy_pin_g = hal.gpio->channel(_drdy_pin_num_g);
if (_drdy_pin_g == nullptr) {
AP_HAL::panic("LSM9DS0: null gyro data-ready GPIO channel\n");
}
_drdy_pin_g->mode(HAL_GPIO_INPUT);
}
bool success = _hardware_init();
#if LSM9DS0_DEBUG
_dump_registers();
#endif
return success;
}
bool AP_InertialSensor_LSM9DS0::_hardware_init()
{
_spi_sem->take_blocking();
uint8_t tries, whoami;
// set flag for reading registers
_dev_gyro->set_read_flag(0x80);
_dev_accel->set_read_flag(0x80);
whoami_g = _register_read_g(WHO_AM_I_G);
if (whoami_g != LSM9DS0_G_WHOAMI && whoami_g != LSM9DS0_G_WHOAMI_H) {
goto fail_whoami;
}
whoami = _register_read_xm(WHO_AM_I_XM);
if (whoami != LSM9DS0_XM_WHOAMI) {
goto fail_whoami;
}
// setup for register checking
_dev_gyro->setup_checked_registers(5, 20);
_dev_accel->setup_checked_registers(4, 20);
for (tries = 0; tries < 5; tries++) {
_dev_gyro->set_speed(AP_HAL::Device::SPEED_LOW);
_dev_accel->set_speed(AP_HAL::Device::SPEED_LOW);
_gyro_init();
_accel_init();
_dev_gyro->set_speed(AP_HAL::Device::SPEED_HIGH);
_dev_accel->set_speed(AP_HAL::Device::SPEED_HIGH);
hal.scheduler->delay(10);
if (_accel_data_ready() && _gyro_data_ready()) {
break;
}
#if LSM9DS0_DEBUG
_dump_registers();
#endif
}
if (tries == 5) {
hal.console->printf("Failed to boot LSM9DS0 5 times\n\n");
goto fail_tries;
}
_spi_sem->give();
return true;
fail_tries:
fail_whoami:
_spi_sem->give();
return false;
}
/*
start the sensor going
*/
void AP_InertialSensor_LSM9DS0::start(void)
{
_gyro_instance = _imu.register_gyro(760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20));
_accel_instance = _imu.register_accel(1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D));
if (whoami_g == LSM9DS0_G_WHOAMI_H) {
set_gyro_orientation(_gyro_instance, _rotation_gH);
} else {
set_gyro_orientation(_gyro_instance, _rotation_g);
}
set_accel_orientation(_accel_instance, _rotation_a);
_set_accel_max_abs_offset(_accel_instance, 5.0f);
/* start the timer process to read samples */
_dev_gyro->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void));
}
uint8_t AP_InertialSensor_LSM9DS0::_register_read_xm(uint8_t reg)
{
uint8_t val = 0;
_dev_accel->read_registers(reg, &val, 1);
return val;
}
uint8_t AP_InertialSensor_LSM9DS0::_register_read_g(uint8_t reg)
{
uint8_t val = 0;
_dev_gyro->read_registers(reg, &val, 1);
return val;
}
void AP_InertialSensor_LSM9DS0::_register_write_xm(uint8_t reg, uint8_t val, bool checked)
{
_dev_accel->write_register(reg, val, checked);
}
void AP_InertialSensor_LSM9DS0::_register_write_g(uint8_t reg, uint8_t val, bool checked)
{
_dev_gyro->write_register(reg, val, checked);
}
void AP_InertialSensor_LSM9DS0::_gyro_disable_i2c()
{
uint8_t retries = 10;
while (retries--) {
// add retries
uint8_t a = _register_read_g(0x05);
_register_write_g(0x05, (0x20 | a));
if (_register_read_g(0x05) == (a | 0x20)) {
return;
}
}
AP_HAL::panic("LSM9DS0_G: Unable to disable I2C");
}
void AP_InertialSensor_LSM9DS0::_accel_disable_i2c()
{
uint8_t a = _register_read_xm(0x02);
_register_write_xm(0x02, (0x10 | a));
a = _register_read_xm(0x02);
_register_write_xm(0x02, (0xF7 & a));
a = _register_read_xm(0x15);
_register_write_xm(0x15, (0x80 | a));
a = _register_read_xm(0x02);
_register_write_xm(0x02, (0xE7 & a));
}
void AP_InertialSensor_LSM9DS0::_gyro_init()
{
_gyro_disable_i2c();
hal.scheduler->delay(1);
_register_write_g(CTRL_REG1_G,
CTRL_REG1_G_DR_760Hz_BW_50Hz |
CTRL_REG1_G_PD |
CTRL_REG1_G_ZEN |
CTRL_REG1_G_YEN |
CTRL_REG1_G_XEN, true);
hal.scheduler->delay(1);
_register_write_g(CTRL_REG2_G, 0x00, true);
hal.scheduler->delay(1);
/*
* Gyro data ready on DRDY_G
*/
_register_write_g(CTRL_REG3_G, CTRL_REG3_G_I2_DRDY, true);
hal.scheduler->delay(1);
_register_write_g(CTRL_REG4_G,
CTRL_REG4_G_BDU |
CTRL_REG4_G_FS_2000DPS, true);
_set_gyro_scale(G_SCALE_2000DPS);
hal.scheduler->delay(1);
_register_write_g(CTRL_REG5_G, 0x00, true);
hal.scheduler->delay(1);
}
void AP_InertialSensor_LSM9DS0::_accel_init()
{
_accel_disable_i2c();
hal.scheduler->delay(1);
_register_write_xm(CTRL_REG0_XM, 0x00, true);
hal.scheduler->delay(1);
_register_write_xm(CTRL_REG1_XM,
CTRL_REG1_XM_AODR_1600Hz |
CTRL_REG1_XM_BDU |
CTRL_REG1_XM_AZEN |
CTRL_REG1_XM_AYEN |
CTRL_REG1_XM_AXEN, true);
hal.scheduler->delay(1);
_register_write_xm(CTRL_REG2_XM,
CTRL_REG2_XM_ABW_194Hz |
CTRL_REG2_XM_AFS_16G, true);
_set_accel_scale(A_SCALE_16G);
hal.scheduler->delay(1);
/* Accel data ready on INT1 */
_register_write_xm(CTRL_REG3_XM, CTRL_REG3_XM_P1_DRDYA, true);
hal.scheduler->delay(1);
// enable temperature sensor
_register_write_xm(CTRL_REG5_XM, _register_read_xm(CTRL_REG5_XM) | 0x80, false);
}
void AP_InertialSensor_LSM9DS0::_set_gyro_scale(gyro_scale scale)
{
/* scales values from datasheet in mdps/digit */
switch (scale) {
case G_SCALE_245DPS:
_gyro_scale = 8.75;
break;
case G_SCALE_500DPS:
_gyro_scale = 17.50;
break;
case G_SCALE_2000DPS:
_gyro_scale = 70;
break;
}
/* convert mdps/digit to dps/digit */
_gyro_scale /= 1000;
/* convert dps/digit to (rad/s)/digit */
_gyro_scale *= DEG_TO_RAD;
}
void AP_InertialSensor_LSM9DS0::_set_accel_scale(accel_scale scale)
{
/*
* Possible accelerometer scales (and their register bit settings) are:
* 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an
* algorithm to calculate g/(ADC tick) based on that 3-bit value:
*/
_accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f;
if (scale == A_SCALE_16G) {
/* the datasheet shows an exception for +-16G */
_accel_scale = 0.000732f;
}
/* convert to G/LSB to (m/s/s)/LSB */
_accel_scale *= GRAVITY_MSS;
}
/**
* Timer process to poll for new data from the LSM9DS0.
*/
void AP_InertialSensor_LSM9DS0::_poll_data()
{
if (_gyro_data_ready()) {
_read_data_transaction_g();
}
if (_accel_data_ready()) {
_read_data_transaction_a();
}
// check next register value for correctness
AP_HAL::Device::checkreg reg;
if (!_dev_gyro->check_next_register(reg)) {
log_register_change(_dev_gyro->get_bus_id(), reg);
_inc_gyro_error_count(_gyro_instance);
}
if (!_dev_accel->check_next_register(reg)) {
log_register_change(_dev_accel->get_bus_id(), reg);
_inc_accel_error_count(_accel_instance);
}
}
bool AP_InertialSensor_LSM9DS0::_accel_data_ready()
{
if (_drdy_pin_a != nullptr) {
return _drdy_pin_a->read() != 0;
}
uint8_t status = _register_read_xm(STATUS_REG_A);
return status & STATUS_REG_A_ZYXADA;
}
bool AP_InertialSensor_LSM9DS0::_gyro_data_ready()
{
if (_drdy_pin_g != nullptr) {
return _drdy_pin_g->read() != 0;
}
uint8_t status = _register_read_g(STATUS_REG_G);
return status & STATUS_REG_G_ZYXDA;
}
void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
{
struct sensor_raw_data raw_data = { };
const uint8_t reg = OUT_X_L_A | 0xC0;
if (!_dev_accel->transfer(®, 1, (uint8_t *) &raw_data, sizeof(raw_data))) {
return;
}
Vector3f accel_data(raw_data.x, -raw_data.y, -raw_data.z);
accel_data *= _accel_scale;
_rotate_and_correct_accel(_accel_instance, accel_data);
_notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64());
// read temperature every 10th sample
if (_temp_counter++ >= 10) {
int16_t traw;
const uint8_t regtemp = OUT_TEMP_L_XM | 0xC0;
_temp_counter = 0;
if (_dev_accel->transfer(®temp, 1, (uint8_t *)&traw, sizeof(traw))) {
_temperature = _temp_filter.apply(traw * 0.125 + 25);
}
}
}
/*
* read from the data registers and update filtered data
*/
void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
{
struct sensor_raw_data raw_data = { };
const uint8_t reg = OUT_X_L_G | 0xC0;
if (!_dev_gyro->transfer(®, 1, (uint8_t *) &raw_data, sizeof(raw_data))) {
return;
}
Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z);
gyro_data *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro_data);
_notify_new_gyro_raw_sample(_gyro_instance, gyro_data, AP_HAL::micros64());
}
bool AP_InertialSensor_LSM9DS0::update()
{
update_gyro(_gyro_instance);
update_accel(_accel_instance);
_publish_temperature(_accel_instance, _temperature);
return true;
}
#if LSM9DS0_DEBUG
/* dump all config registers - used for debug */
void AP_InertialSensor_LSM9DS0::_dump_registers(void)
{
hal.console->printf("LSM9DS0 registers:\n");
hal.console->printf("Gyroscope registers:\n");
const uint8_t first = OUT_TEMP_L_XM;
const uint8_t last = ACT_DUR;
for (uint8_t reg=first; reg<=last; reg++) {
uint8_t v = _register_read_g(reg);
hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg - (first-1)) % 16 == 0) {
hal.console->printf("\n");
}
}
hal.console->printf("\n");
hal.console->printf("Accelerometer and Magnetometers registers:\n");
for (uint8_t reg=first; reg<=last; reg++) {
uint8_t v = _register_read_xm(reg);
hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg - (first-1)) % 16 == 0) {
hal.console->printf("\n");
}
}
hal.console->printf("\n");
}
#endif