-
Notifications
You must be signed in to change notification settings - Fork 152
/
Copy pathinv_mpu.c
3227 lines (2929 loc) · 91.2 KB
/
inv_mpu.c
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
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*
$License:
Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
See included License.txt for License information.
$
*/
/**
* @addtogroup DRIVERS Sensor Driver Layer
* @brief Hardware drivers to communicate with sensors via I2C.
*
* @{
* @file inv_mpu.c
* @brief An I2C-based driver for Invensense gyroscopes.
* @details This driver currently works for the following devices:
* MPU6050
* MPU6500
* MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus)
* MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus)
*/
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"
#ifdef ESP32
#define min(a,b) ((a)<(b)?(a):(b))
#endif
/* The following functions must be defined for this platform:
* i2c_write(unsigned char slave_addr, unsigned char reg_addr,
* unsigned char length, unsigned char const *data)
* i2c_read(unsigned char slave_addr, unsigned char reg_addr,
* unsigned char length, unsigned char *data)
* delay_ms(unsigned long num_ms)
* get_ms(unsigned long *count)
* reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
* labs(long x)
* fabsf(float x)
* min(int a, int b)
*/
#include <Arduino.h>
#define MPU9250
#include "arduino_mpu9250_i2c.h"
#include "arduino_mpu9250_clk.h"
#define i2c_write(a, b, c, d) arduino_i2c_write(a, b, c, d)
#define i2c_read(a, b, c, d) arduino_i2c_read(a, b, c, d)
#define delay_ms arduino_delay_ms
#define get_ms arduino_get_clock_ms
#define log_i _MLPrintLog
#define log_e _MLPrintLog
static inline int reg_int_cb(struct int_param_s *int_param)
{
}
#if !defined MPU6050 && !defined MPU9150 && !defined MPU6500 && !defined MPU9250
#error Which gyro are you using? Define MPUxxxx in your compiler options.
#endif
/* Time for some messy macro work. =]
* #define MPU9150
* is equivalent to..
* #define MPU6050
* #define AK8975_SECONDARY
*
* #define MPU9250
* is equivalent to..
* #define MPU6500
* #define AK8963_SECONDARY
*/
#if defined MPU9150
#ifndef MPU6050
#define MPU6050
#endif /* #ifndef MPU6050 */
#if defined AK8963_SECONDARY
#error "MPU9150 and AK8963_SECONDARY cannot both be defined."
#elif !defined AK8975_SECONDARY /* #if defined AK8963_SECONDARY */
#define AK8975_SECONDARY
#endif /* #if defined AK8963_SECONDARY */
#elif defined MPU9250 /* #if defined MPU9150 */
#ifndef MPU6500
#define MPU6500
#endif /* #ifndef MPU6500 */
#if defined AK8975_SECONDARY
#error "MPU9250 and AK8975_SECONDARY cannot both be defined."
#elif !defined AK8963_SECONDARY /* #if defined AK8975_SECONDARY */
#define AK8963_SECONDARY
#endif /* #if defined AK8975_SECONDARY */
#endif /* #if defined MPU9150 */
#if defined AK8975_SECONDARY || defined AK8963_SECONDARY
#define AK89xx_SECONDARY
#else
/* #warning "No compass = less profit for Invensense. Lame." */
#endif
//static int set_int_enable(unsigned char enable);
/* Hardware registers needed by driver. */
struct gyro_reg_s {
unsigned char who_am_i;
unsigned char rate_div;
unsigned char lpf;
unsigned char prod_id;
unsigned char user_ctrl;
unsigned char fifo_en;
unsigned char gyro_cfg;
unsigned char accel_cfg;
unsigned char accel_cfg2;
unsigned char lp_accel_odr;
unsigned char motion_thr;
unsigned char motion_dur;
unsigned char fifo_count_h;
unsigned char fifo_r_w;
unsigned char raw_gyro;
unsigned char raw_accel;
unsigned char temp;
unsigned char int_enable;
unsigned char dmp_int_status;
unsigned char int_status;
unsigned char accel_intel;
unsigned char pwr_mgmt_1;
unsigned char pwr_mgmt_2;
unsigned char int_pin_cfg;
unsigned char mem_r_w;
unsigned char accel_offs;
unsigned char i2c_mst;
unsigned char bank_sel;
unsigned char mem_start_addr;
unsigned char prgm_start_h;
#if defined AK89xx_SECONDARY
unsigned char s0_addr;
unsigned char s0_reg;
unsigned char s0_ctrl;
unsigned char s1_addr;
unsigned char s1_reg;
unsigned char s1_ctrl;
unsigned char s4_ctrl;
unsigned char s0_do;
unsigned char s1_do;
unsigned char i2c_delay_ctrl;
unsigned char raw_compass;
/* The I2C_MST_VDDIO bit is in this register. */
unsigned char yg_offs_tc;
#endif
};
/* Information specific to a particular device. */
struct hw_s {
unsigned char addr;
unsigned short max_fifo;
unsigned char num_reg;
unsigned short temp_sens;
short temp_offset;
unsigned short bank_size;
#if defined AK89xx_SECONDARY
unsigned short compass_fsr;
#endif
};
/* When entering motion interrupt mode, the driver keeps track of the
* previous state so that it can be restored at a later time.
* TODO: This is tacky. Fix it.
*/
struct motion_int_cache_s {
unsigned short gyro_fsr;
unsigned char accel_fsr;
unsigned short lpf;
unsigned short sample_rate;
unsigned char sensors_on;
unsigned char fifo_sensors;
unsigned char dmp_on;
};
/* Cached chip configuration data.
* TODO: A lot of these can be handled with a bitmask.
*/
struct chip_cfg_s {
/* Matches gyro_cfg >> 3 & 0x03 */
unsigned char gyro_fsr;
/* Matches accel_cfg >> 3 & 0x03 */
unsigned char accel_fsr;
/* Enabled sensors. Uses same masks as fifo_en, NOT pwr_mgmt_2. */
unsigned char sensors;
/* Matches config register. */
unsigned char lpf;
unsigned char clk_src;
/* Sample rate, NOT rate divider. */
unsigned short sample_rate;
/* Matches fifo_en register. */
unsigned char fifo_enable;
/* Matches int enable register. */
unsigned char int_enable;
/* 1 if devices on auxiliary I2C bus appear on the primary. */
unsigned char bypass_mode;
/* 1 if half-sensitivity.
* NOTE: This doesn't belong here, but everything else in hw_s is const,
* and this allows us to save some precious RAM.
*/
unsigned char accel_half;
/* 1 if device in low-power accel-only mode. */
unsigned char lp_accel_mode;
/* 1 if interrupts are only triggered on motion events. */
unsigned char int_motion_only;
struct motion_int_cache_s cache;
/* 1 for active low interrupts. */
unsigned char active_low_int;
/* 1 for latched interrupts. */
unsigned char latched_int;
/* 1 if DMP is enabled. */
unsigned char dmp_on;
/* Ensures that DMP will only be loaded once. */
unsigned char dmp_loaded;
/* Sampling rate used when DMP is enabled. */
unsigned short dmp_sample_rate;
#ifdef AK89xx_SECONDARY
/* Compass sample rate. */
unsigned short compass_sample_rate;
unsigned char compass_addr;
short mag_sens_adj[3];
#endif
};
/* Information for self-test. */
struct test_s {
unsigned long gyro_sens;
unsigned long accel_sens;
unsigned char reg_rate_div;
unsigned char reg_lpf;
unsigned char reg_gyro_fsr;
unsigned char reg_accel_fsr;
unsigned short wait_ms;
unsigned char packet_thresh;
float min_dps;
float max_dps;
float max_gyro_var;
float min_g;
float max_g;
float max_accel_var;
#ifdef MPU6500
float max_g_offset;
unsigned short sample_wait_ms;
#endif
};
/* Gyro driver state variables. */
struct gyro_state_s {
const struct gyro_reg_s *reg;
const struct hw_s *hw;
struct chip_cfg_s chip_cfg;
const struct test_s *test;
};
/* Filter configurations. */
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
NUM_FILTER
};
/* Full scale ranges. */
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
INV_FSR_1000DPS,
INV_FSR_2000DPS,
NUM_GYRO_FSR
};
/* Full scale ranges. */
enum accel_fsr_e {
INV_FSR_2G = 0,
INV_FSR_4G,
INV_FSR_8G,
INV_FSR_16G,
NUM_ACCEL_FSR
};
/* Clock sources. */
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
NUM_CLK
};
/* Low-power accel wakeup rates. */
enum lp_accel_rate_e {
#if defined MPU6050
INV_LPA_1_25HZ,
INV_LPA_5HZ,
INV_LPA_20HZ,
INV_LPA_40HZ
#elif defined MPU6500
INV_LPA_0_3125HZ,
INV_LPA_0_625HZ,
INV_LPA_1_25HZ,
INV_LPA_2_5HZ,
INV_LPA_5HZ,
INV_LPA_10HZ,
INV_LPA_20HZ,
INV_LPA_40HZ,
INV_LPA_80HZ,
INV_LPA_160HZ,
INV_LPA_320HZ,
INV_LPA_640HZ
#endif
};
#define BIT_I2C_MST_VDDIO (0x80)
#define BIT_FIFO_EN (0x40)
#define BIT_DMP_EN (0x80)
#define BIT_FIFO_RST (0x04)
#define BIT_DMP_RST (0x08)
#define BIT_FIFO_OVERFLOW (0x10)
#define BIT_DATA_RDY_EN (0x01)
#define BIT_DMP_INT_EN (0x02)
#define BIT_MOT_INT_EN (0x40)
#define BITS_FSR (0x18)
#define BITS_LPF (0x07)
#define BITS_HPF (0x07)
#define BITS_CLK (0x07)
#define BIT_FIFO_SIZE_1024 (0x40)
#define BIT_FIFO_SIZE_2048 (0x80)
#define BIT_FIFO_SIZE_4096 (0xC0)
#define BIT_RESET (0x80)
#define BIT_SLEEP (0x40)
#define BIT_S0_DELAY_EN (0x01)
#define BIT_S2_DELAY_EN (0x04)
#define BITS_SLAVE_LENGTH (0x0F)
#define BIT_SLAVE_BYTE_SW (0x40)
#define BIT_SLAVE_GROUP (0x10)
#define BIT_SLAVE_EN (0x80)
#define BIT_I2C_READ (0x80)
#define BITS_I2C_MASTER_DLY (0x1F)
#define BIT_AUX_IF_EN (0x20)
#define BIT_ACTL (0x80)
#define BIT_LATCH_EN (0x20)
#define BIT_ANY_RD_CLR (0x10)
#define BIT_BYPASS_EN (0x02)
#define BITS_WOM_EN (0xC0)
#define BIT_LPA_CYCLE (0x20)
#define BIT_STBY_XA (0x20)
#define BIT_STBY_YA (0x10)
#define BIT_STBY_ZA (0x08)
#define BIT_STBY_XG (0x04)
#define BIT_STBY_YG (0x02)
#define BIT_STBY_ZG (0x01)
#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
#if defined AK8975_SECONDARY
#define SUPPORTS_AK89xx_HIGH_SENS (0x00)
#define AK89xx_FSR (9830)
#elif defined AK8963_SECONDARY
#define SUPPORTS_AK89xx_HIGH_SENS (0x10)
#define AK89xx_FSR (4915)
#endif
#ifdef AK89xx_SECONDARY
#define AKM_REG_WHOAMI (0x00)
#define AKM_REG_ST1 (0x02)
#define AKM_REG_HXL (0x03)
#define AKM_REG_ST2 (0x09)
#define AKM_REG_CNTL (0x0A)
#define AKM_REG_ASTC (0x0C)
#define AKM_REG_ASAX (0x10)
#define AKM_REG_ASAY (0x11)
#define AKM_REG_ASAZ (0x12)
#define AKM_DATA_READY (0x01)
#define AKM_DATA_OVERRUN (0x02)
#define AKM_OVERFLOW (0x80)
#define AKM_DATA_ERROR (0x40)
#define AKM_BIT_SELF_TEST (0x40)
#define AKM_POWER_DOWN (0x00 | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_SINGLE_MEASUREMENT (0x01 | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_FUSE_ROM_ACCESS (0x0F | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_MODE_SELF_TEST (0x08 | SUPPORTS_AK89xx_HIGH_SENS)
#define AKM_WHOAMI (0x48)
#endif
#if defined MPU6050
const struct gyro_reg_s reg = {
.who_am_i = 0x75,
.rate_div = 0x19,
.lpf = 0x1A,
.prod_id = 0x0C,
.user_ctrl = 0x6A,
.fifo_en = 0x23,
.gyro_cfg = 0x1B,
.accel_cfg = 0x1C,
.motion_thr = 0x1F,
.motion_dur = 0x20,
.fifo_count_h = 0x72,
.fifo_r_w = 0x74,
.raw_gyro = 0x43,
.raw_accel = 0x3B,
.temp = 0x41,
.int_enable = 0x38,
.dmp_int_status = 0x39,
.int_status = 0x3A,
.pwr_mgmt_1 = 0x6B,
.pwr_mgmt_2 = 0x6C,
.int_pin_cfg = 0x37,
.mem_r_w = 0x6F,
.accel_offs = 0x06,
.i2c_mst = 0x24,
.bank_sel = 0x6D,
.mem_start_addr = 0x6E,
.prgm_start_h = 0x70
#ifdef AK89xx_SECONDARY
,.raw_compass = 0x49,
.yg_offs_tc = 0x01,
.s0_addr = 0x25,
.s0_reg = 0x26,
.s0_ctrl = 0x27,
.s1_addr = 0x28,
.s1_reg = 0x29,
.s1_ctrl = 0x2A,
.s4_ctrl = 0x34,
.s0_do = 0x63,
.s1_do = 0x64,
.i2c_delay_ctrl = 0x67
#endif
};
const struct hw_s hw = {
.addr = 0x68,
.max_fifo = 1024,
.num_reg = 118,
.temp_sens = 340,
.temp_offset = -521,
.bank_size = 256
#if defined AK89xx_SECONDARY
,.compass_fsr = AK89xx_FSR
#endif
};
const struct test_s test = {
.gyro_sens = 32768/250,
.accel_sens = 32768/16,
.reg_rate_div = 0, /* 1kHz. */
.reg_lpf = 1, /* 188Hz. */
.reg_gyro_fsr = 0, /* 250dps. */
.reg_accel_fsr = 0x18, /* 16g. */
.wait_ms = 50,
.packet_thresh = 5, /* 5% */
.min_dps = 10.f,
.max_dps = 105.f,
.max_gyro_var = 0.14f,
.min_g = 0.3f,
.max_g = 0.95f,
.max_accel_var = 0.14f
};
static struct gyro_state_s st = {
.reg = ®,
.hw = &hw,
.test = &test
};
#elif defined MPU6500
const struct gyro_reg_s reg = {
.who_am_i = 0x75,
.rate_div = 0x19,
.lpf = 0x1A,
.prod_id = 0x0C,
.user_ctrl = 0x6A,
.fifo_en = 0x23,
.gyro_cfg = 0x1B,
.accel_cfg = 0x1C,
.accel_cfg2 = 0x1D,
.lp_accel_odr = 0x1E,
.motion_thr = 0x1F,
.motion_dur = 0x20,
.fifo_count_h = 0x72,
.fifo_r_w = 0x74,
.raw_gyro = 0x43,
.raw_accel = 0x3B,
.temp = 0x41,
.int_enable = 0x38,
.dmp_int_status = 0x39,
.int_status = 0x3A,
.accel_intel = 0x69,
.pwr_mgmt_1 = 0x6B,
.pwr_mgmt_2 = 0x6C,
.int_pin_cfg = 0x37,
.mem_r_w = 0x6F,
.accel_offs = 0x77,
.i2c_mst = 0x24,
.bank_sel = 0x6D,
.mem_start_addr = 0x6E,
.prgm_start_h = 0x70
#ifdef AK89xx_SECONDARY
,.raw_compass = 0x49,
.s0_addr = 0x25,
.s0_reg = 0x26,
.s0_ctrl = 0x27,
.s1_addr = 0x28,
.s1_reg = 0x29,
.s1_ctrl = 0x2A,
.s4_ctrl = 0x34,
.s0_do = 0x63,
.s1_do = 0x64,
.i2c_delay_ctrl = 0x67
#endif
};
const struct hw_s hw = {
.addr = 0x68,
.max_fifo = 1024,
.num_reg = 128,
.temp_sens = 321,
.temp_offset = 0,
.bank_size = 256
#if defined AK89xx_SECONDARY
,.compass_fsr = AK89xx_FSR
#endif
};
const struct test_s test = {
.gyro_sens = 32768/250,
.accel_sens = 32768/2, //FSR = +-2G = 16384 LSB/G
.reg_rate_div = 0, /* 1kHz. */
.reg_lpf = 2, /* 92Hz low pass filter*/
.reg_gyro_fsr = 0, /* 250dps. */
.reg_accel_fsr = 0x0, /* Accel FSR setting = 2g. */
.wait_ms = 200, //200ms stabilization time
.packet_thresh = 200, /* 200 samples */
.min_dps = 20.f, //20 dps for Gyro Criteria C
.max_dps = 60.f, //Must exceed 60 dps threshold for Gyro Criteria B
.max_gyro_var = .5f, //Must exceed +50% variation for Gyro Criteria A
.min_g = .225f, //Accel must exceed Min 225 mg for Criteria B
.max_g = .675f, //Accel cannot exceed Max 675 mg for Criteria B
.max_accel_var = .5f, //Accel must be within 50% variation for Criteria A
.max_g_offset = .5f, //500 mg for Accel Criteria C
.sample_wait_ms = 10 //10ms sample time wait
};
static struct gyro_state_s st = {
.reg = ®,
.hw = &hw,
.test = &test
};
#endif
#define MAX_PACKET_LENGTH (12)
#ifdef MPU6500
#define HWST_MAX_PACKET_LENGTH (512)
#endif
#ifdef AK89xx_SECONDARY
static int setup_compass(void);
#define MAX_COMPASS_SAMPLE_RATE (100)
#endif
/**
* @brief Enable/disable data ready interrupt.
* If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
* interrupt is used.
* @param[in] enable 1 to enable interrupt.
* @return 0 if successful.
*/
int set_int_enable(unsigned char enable)
{
unsigned char tmp;
if (st.chip_cfg.dmp_on) {
if (enable)
tmp = BIT_DMP_INT_EN;
else
tmp = 0x00;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
return -1;
st.chip_cfg.int_enable = tmp;
} else {
if (!st.chip_cfg.sensors)
return -1;
if (enable && st.chip_cfg.int_enable)
return 0;
if (enable)
tmp = BIT_DATA_RDY_EN;
else
tmp = 0x00;
if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
return -1;
st.chip_cfg.int_enable = tmp;
}
return 0;
}
/**
* @brief Register dump for testing.
* @return 0 if successful.
*/
int mpu_reg_dump(void)
{
unsigned char ii;
unsigned char data;
for (ii = 0; ii < st.hw->num_reg; ii++) {
if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w)
continue;
if (i2c_read(st.hw->addr, ii, 1, &data))
return -1;
log_i("%#5x: %#5x\r\n", ii, data);
}
return 0;
}
/**
* @brief Read from a single register.
* NOTE: The memory and FIFO read/write registers cannot be accessed.
* @param[in] reg Register address.
* @param[out] data Register data.
* @return 0 if successful.
*/
int mpu_read_reg(unsigned char reg, unsigned char *data)
{
if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w)
return -1;
if (reg >= st.hw->num_reg)
return -1;
return i2c_read(st.hw->addr, reg, 1, data);
}
/**
* @brief Initialize hardware.
* Initial configuration:\n
* Gyro FSR: +/- 2000DPS\n
* Accel FSR +/- 2G\n
* DLPF: 42Hz\n
* FIFO rate: 50Hz\n
* Clock source: Gyro PLL\n
* FIFO: Disabled.\n
* Data ready interrupt: Disabled, active low, unlatched.
* @param[in] int_param Platform-specific parameters to interrupt API.
* @return 0 if successful.
*/
int mpu_init(struct int_param_s *int_param)
{
unsigned char data[6];
/* Reset device. */
data[0] = BIT_RESET;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
return -1;
delay_ms(100);
/* Wake up chip. */
data[0] = 0x00;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
return -1;
st.chip_cfg.accel_half = 0;
#ifdef MPU6500
/* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
* first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
*/
data[0] = BIT_FIFO_SIZE_1024 | 0x8;
if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data))
return -1;
#endif
/* Set to invalid values to ensure no I2C writes are skipped. */
st.chip_cfg.sensors = 0xFF;
st.chip_cfg.gyro_fsr = 0xFF;
st.chip_cfg.accel_fsr = 0xFF;
st.chip_cfg.lpf = 0xFF;
st.chip_cfg.sample_rate = 0xFFFF;
st.chip_cfg.fifo_enable = 0xFF;
st.chip_cfg.bypass_mode = 0xFF;
#ifdef AK89xx_SECONDARY
st.chip_cfg.compass_sample_rate = 0xFFFF;
#endif
/* mpu_set_sensors always preserves this setting. */
st.chip_cfg.clk_src = INV_CLK_PLL;
/* Handled in next call to mpu_set_bypass. */
st.chip_cfg.active_low_int = 1;
st.chip_cfg.latched_int = 0;
st.chip_cfg.int_motion_only = 0;
st.chip_cfg.lp_accel_mode = 0;
memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
st.chip_cfg.dmp_on = 0;
st.chip_cfg.dmp_loaded = 0;
st.chip_cfg.dmp_sample_rate = 0;
if (mpu_set_gyro_fsr(2000))
return -1;
if (mpu_set_accel_fsr(2))
return -1;
if (mpu_set_lpf(42))
return -1;
if (mpu_set_sample_rate(50))
return -1;
if (mpu_configure_fifo(0))
return -1;
#ifndef EMPL_TARGET_STM32F4
if (int_param)
reg_int_cb(int_param);
#endif
#ifdef AK89xx_SECONDARY
setup_compass();
if (mpu_set_compass_sample_rate(10))
return -1;
#else
/* Already disabled by setup_compass. */
if (mpu_set_bypass(0))
return -1;
#endif
mpu_set_sensors(0);
return 0;
}
/**
* @brief Enter low-power accel-only mode.
* In low-power accel mode, the chip goes to sleep and only wakes up to sample
* the accelerometer at one of the following frequencies:
* \n MPU6050: 1.25Hz, 5Hz, 20Hz, 40Hz
* \n MPU6500: 1.25Hz, 2.5Hz, 5Hz, 10Hz, 20Hz, 40Hz, 80Hz, 160Hz, 320Hz, 640Hz
* \n If the requested rate is not one listed above, the device will be set to
* the next highest rate. Requesting a rate above the maximum supported
* frequency will result in an error.
* \n To select a fractional wake-up frequency, round down the value passed to
* @e rate.
* @param[in] rate Minimum sampling rate, or zero to disable LP
* accel mode.
* @return 0 if successful.
*/
int mpu_lp_accel_mode(unsigned short rate)
{
unsigned char tmp[2];
if (rate > 40)
return -1;
if (!rate) {
mpu_set_int_latched(0);
tmp[0] = 0;
tmp[1] = BIT_STBY_XYZG;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
return -1;
st.chip_cfg.lp_accel_mode = 0;
return 0;
}
/* For LP accel, we automatically configure the hardware to produce latched
* interrupts. In LP accel mode, the hardware cycles into sleep mode before
* it gets a chance to deassert the interrupt pin; therefore, we shift this
* responsibility over to the MCU.
*
* Any register read will clear the interrupt.
*/
mpu_set_int_latched(1);
#if defined MPU6050
tmp[0] = BIT_LPA_CYCLE;
if (rate == 1) {
tmp[1] = INV_LPA_1_25HZ;
mpu_set_lpf(5);
} else if (rate <= 5) {
tmp[1] = INV_LPA_5HZ;
mpu_set_lpf(5);
} else if (rate <= 20) {
tmp[1] = INV_LPA_20HZ;
mpu_set_lpf(10);
} else {
tmp[1] = INV_LPA_40HZ;
mpu_set_lpf(20);
}
tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
return -1;
#elif defined MPU6500
/* Set wake frequency. */
if (rate == 1)
tmp[0] = INV_LPA_1_25HZ;
else if (rate == 2)
tmp[0] = INV_LPA_2_5HZ;
else if (rate <= 5)
tmp[0] = INV_LPA_5HZ;
else if (rate <= 10)
tmp[0] = INV_LPA_10HZ;
else if (rate <= 20)
tmp[0] = INV_LPA_20HZ;
else if (rate <= 40)
tmp[0] = INV_LPA_40HZ;
else if (rate <= 80)
tmp[0] = INV_LPA_80HZ;
else if (rate <= 160)
tmp[0] = INV_LPA_160HZ;
else if (rate <= 320)
tmp[0] = INV_LPA_320HZ;
else
tmp[0] = INV_LPA_640HZ;
if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
return -1;
tmp[0] = BIT_LPA_CYCLE;
if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
return -1;
#endif
st.chip_cfg.sensors = INV_XYZ_ACCEL;
st.chip_cfg.clk_src = 0;
st.chip_cfg.lp_accel_mode = 1;
mpu_configure_fifo(0);
return 0;
}
/**
* @brief Read raw gyro data directly from the registers.
* @param[out] data Raw data in hardware units.
* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
* @return 0 if successful.
*/
int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
{
unsigned char tmp[6];
if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
return -1;
if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
return -1;
data[0] = (tmp[0] << 8) | tmp[1];
data[1] = (tmp[2] << 8) | tmp[3];
data[2] = (tmp[4] << 8) | tmp[5];
if (timestamp)
get_ms(timestamp);
return 0;
}
/**
* @brief Read raw accel data directly from the registers.
* @param[out] data Raw data in hardware units.
* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
* @return 0 if successful.
*/
int mpu_get_accel_reg(short *data, unsigned long *timestamp)
{
unsigned char tmp[6];
if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
return -1;
if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
return -1;
data[0] = (tmp[0] << 8) | tmp[1];
data[1] = (tmp[2] << 8) | tmp[3];
data[2] = (tmp[4] << 8) | tmp[5];
if (timestamp)
get_ms(timestamp);
return 0;
}
/**
* @brief Read temperature data directly from the registers.
* @param[out] data Data in q16 format.
* @param[out] timestamp Timestamp in milliseconds. Null if not needed.
* @return 0 if successful.
*/
int mpu_get_temperature(long *data, unsigned long *timestamp)
{
unsigned char tmp[2];
short raw;
if (!(st.chip_cfg.sensors))
return -1;
if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp))
return -1;
raw = (tmp[0] << 8) | tmp[1];
if (timestamp)
get_ms(timestamp);
data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
return 0;
}
/**
* @brief Read biases to the accel bias 6500 registers.
* This function reads from the MPU6500 accel offset cancellations registers.
* The format are G in +-8G format. The register is initialized with OTP
* factory trim values.
* @param[in] accel_bias returned structure with the accel bias
* @return 0 if successful.
*/
int mpu_read_6500_accel_bias(long *accel_bias) {
unsigned char data[6];
if (i2c_read(st.hw->addr, 0x77, 2, &data[0]))
return -1;
if (i2c_read(st.hw->addr, 0x7A, 2, &data[2]))
return -1;
if (i2c_read(st.hw->addr, 0x7D, 2, &data[4]))
return -1;
accel_bias[0] = ((long)data[0]<<8) | data[1];
accel_bias[1] = ((long)data[2]<<8) | data[3];
accel_bias[2] = ((long)data[4]<<8) | data[5];
return 0;
}
/**
* @brief Read biases to the accel bias 6050 registers.
* This function reads from the MPU6050 accel offset cancellations registers.
* The format are G in +-8G format. The register is initialized with OTP
* factory trim values.
* @param[in] accel_bias returned structure with the accel bias
* @return 0 if successful.
*/
int mpu_read_6050_accel_bias(long *accel_bias) {
unsigned char data[6];
if (i2c_read(st.hw->addr, 0x06, 2, &data[0]))
return -1;
if (i2c_read(st.hw->addr, 0x08, 2, &data[2]))
return -1;
if (i2c_read(st.hw->addr, 0x0A, 2, &data[4]))
return -1;
accel_bias[0] = ((long)data[0]<<8) | data[1];
accel_bias[1] = ((long)data[2]<<8) | data[3];
accel_bias[2] = ((long)data[4]<<8) | data[5];
return 0;
}
int mpu_read_6500_gyro_bias(long *gyro_bias) {
unsigned char data[6];
if (i2c_read(st.hw->addr, 0x13, 2, &data[0]))
return -1;
if (i2c_read(st.hw->addr, 0x15, 2, &data[2]))
return -1;
if (i2c_read(st.hw->addr, 0x17, 2, &data[4]))
return -1;
gyro_bias[0] = ((long)data[0]<<8) | data[1];
gyro_bias[1] = ((long)data[2]<<8) | data[3];
gyro_bias[2] = ((long)data[4]<<8) | data[5];
return 0;
}
/**
* @brief Push biases to the gyro bias 6500/6050 registers.
* This function expects biases relative to the current sensor output, and
* these biases will be added to the factory-supplied values. Bias inputs are LSB
* in +-1000dps format.
* @param[in] gyro_bias New biases.
* @return 0 if successful.
*/
int mpu_set_gyro_bias_reg(long *gyro_bias)
{
unsigned char data[6] = {0, 0, 0, 0, 0, 0};
int i=0;
for(i=0;i<3;i++) {
gyro_bias[i]= (-gyro_bias[i]);
}
data[0] = (gyro_bias[0] >> 8) & 0xff;
data[1] = (gyro_bias[0]) & 0xff;
data[2] = (gyro_bias[1] >> 8) & 0xff;
data[3] = (gyro_bias[1]) & 0xff;
data[4] = (gyro_bias[2] >> 8) & 0xff;
data[5] = (gyro_bias[2]) & 0xff;
if (i2c_write(st.hw->addr, 0x13, 2, &data[0]))
return -1;
if (i2c_write(st.hw->addr, 0x15, 2, &data[2]))
return -1;
if (i2c_write(st.hw->addr, 0x17, 2, &data[4]))
return -1;
return 0;
}
/**
* @brief Push biases to the accel bias 6050 registers.
* This function expects biases relative to the current sensor output, and
* these biases will be added to the factory-supplied values. Bias inputs are LSB
* in +-16G format.
* @param[in] accel_bias New biases.
* @return 0 if successful.
*/
int mpu_set_accel_bias_6050_reg(const long *accel_bias) {
unsigned char data[6] = {0, 0, 0, 0, 0, 0};
long accel_reg_bias[3] = {0, 0, 0};
if(mpu_read_6050_accel_bias(accel_reg_bias))
return -1;
accel_reg_bias[0] -= (accel_bias[0] & ~1);
accel_reg_bias[1] -= (accel_bias[1] & ~1);
accel_reg_bias[2] -= (accel_bias[2] & ~1);
data[0] = (accel_reg_bias[0] >> 8) & 0xff;