-
-
Notifications
You must be signed in to change notification settings - Fork 95
/
Mpu6050.cs
798 lines (715 loc) · 30.9 KB
/
Mpu6050.cs
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
// Licensed to the .NET Foundation under one or more agreements.
// The .NET Foundation licenses this file to you under the MIT license.
using System;
using System.Buffers.Binary;
using System.Device;
using System.Device.I2c;
using System.Device.Model;
using System.IO;
using System.Numerics;
using System.Threading;
using UnitsNet;
namespace Iot.Device.Imu
{
/// <summary>
/// MPU6050 - gyroscope, accelerometer and temperature sensor
/// </summary>
[Interface("MPU6050 - gyroscope, accelerometer and temperature sensor")]
public class Mpu6050 : IDisposable
{
/// <summary>
/// Default address for MPU9250
/// </summary>
public const byte DefaultI2cAddress = 0x68;
/// <summary>
/// Second address for MPU9250
/// </summary>
public const byte SecondI2cAddress = 0x69;
private const float Adc = 0x8000;
private const float Gravity = 9.807f;
internal I2cDevice _i2cDevice;
private Vector3 _accelerometerBias = new Vector3();
private Vector3 _gyroscopeBias = new Vector3();
private AccelerometerRange _accelerometerRange;
private GyroscopeRange _gyroscopeRange;
private AccelerometerBandwidth _accelerometerBandwidth;
private GyroscopeBandwidth _gyroscopeBandwidth;
internal bool _wakeOnMotion;
/// <summary>
/// Initialize the MPU6050
/// </summary>
/// <param name="i2cDevice">The I2C device</param>
public Mpu6050(I2cDevice i2cDevice)
{
if (i2cDevice == null)
{
throw new ArgumentNullException(nameof(i2cDevice), $"Variable i2cDevice is null");
}
_i2cDevice = i2cDevice;
Reset();
PowerOn();
if (!CheckVersion())
{
throw new IOException($"This device does not contain the correct signature 0x68 for a MPU6050");
}
GyroscopeBandwidth = GyroscopeBandwidth.Bandwidth0250Hz;
GyroscopeRange = GyroscopeRange.Range0250Dps;
AccelerometerBandwidth = AccelerometerBandwidth.Bandwidth1130Hz;
AccelerometerRange = AccelerometerRange.Range02G;
}
/// <summary>
/// Used to create the class for the MPU9250. Initialization is a bit different than for the MPU6050
/// </summary>
internal Mpu6050(I2cDevice i2cDevice, bool isInternal)
{
_i2cDevice = i2cDevice ?? throw new ArgumentNullException(nameof(i2cDevice));
}
#region Accelerometer
/// <summary>
/// Accelerometer bias data
/// </summary>
[Property]
public Vector3 AccelerometerBias => _accelerometerBias;
/// <summary>
/// Get or set the accelerometer range
/// </summary>
[Property]
public AccelerometerRange AccelerometerRange
{
get => _accelerometerRange;
set
{
WriteRegister(Register.ACCEL_CONFIG, (byte)((byte)value << 3));
// We will cache the range to avoid i2c access every time this data is requested
// This allow as well to make sure the stored data is the same as the one read
_accelerometerRange = (AccelerometerRange)(ReadByte(Register.ACCEL_CONFIG) >> 3);
if (_accelerometerRange != value)
{
throw new IOException($"Can set {nameof(AccelerometerRange)}, desired value {value}, stored value {_accelerometerRange}");
}
}
}
/// <summary>
/// Get or set the accelerometer bandwidth
/// </summary>
[Property]
public AccelerometerBandwidth AccelerometerBandwidth
{
get => _accelerometerBandwidth;
set
{
WriteRegister(Register.ACCEL_CONFIG_2, (byte)value);
_accelerometerBandwidth = (AccelerometerBandwidth)ReadByte(Register.ACCEL_CONFIG_2);
if (_accelerometerBandwidth != value)
{
throw new IOException($"Can set {nameof(AccelerometerBandwidth)}, desired value {value}, stored value {_accelerometerBandwidth}");
}
}
}
/// <summary>
/// Get the real accelerometer bandwidth. This allows to calculate the real
/// degree per second
/// </summary>
[Property]
public float AccelerationScale
{
get
{
float val = AccelerometerRange switch
{
AccelerometerRange.Range02G => 2.0f,
AccelerometerRange.Range04G => 4.0f,
AccelerometerRange.Range08G => 8.0f,
AccelerometerRange.Range16G => 16.0f,
_ => 0,
};
val = (val * Gravity) / Adc;
return val / (1 + SampleRateDivider);
}
}
/// <summary>
/// Get the accelerometer in G
/// </summary>
/// <remarks>
/// Vector axes are the following:
/// +Z +Y
/// \ | /
/// \ | /
/// \|/
/// /|\
/// / | \
/// / | \
/// +X
/// </remarks>
[Telemetry("Acceleration")]
public Vector3 GetAccelerometer() => GetRawAccelerometer() * AccelerationScale;
/// <summary>
/// Gets the raw accelerometer data
/// </summary>
/// <returns></returns>
internal Vector3 GetRawAccelerometer()
{
SpanByte rawData = new byte[6]
{
0,
0,
0,
0,
0,
0
};
Vector3 ace = new Vector3();
ReadBytes(Register.ACCEL_XOUT_H, rawData);
ace.X = BinaryPrimitives.ReadInt16BigEndian(rawData);
ace.Y = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(2));
ace.Z = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(4));
return ace;
}
/// <summary>
/// Set or get the accelerometer low power mode
/// </summary>
[Property]
public AccelerometerLowPowerFrequency AccelerometerLowPowerFrequency
{
get => (AccelerometerLowPowerFrequency)ReadByte(Register.LP_ACCEL_ODR);
set => WriteRegister(Register.LP_ACCEL_ODR, (byte)value);
}
#endregion
#region Gyroscope
/// <summary>
/// Gyroscope bias data
/// </summary>
[Property]
public Vector3 GyroscopeBias => _gyroscopeBias;
/// <summary>
/// Get or set the gyroscope range
/// </summary>
[Property]
public GyroscopeRange GyroscopeRange
{
get => _gyroscopeRange;
set
{
WriteRegister(Register.GYRO_CONFIG, (byte)((byte)value << 3));
_gyroscopeRange = (GyroscopeRange)(ReadByte(Register.GYRO_CONFIG) >> 3);
if (_gyroscopeRange != value)
{
throw new IOException($"Can set {nameof(GyroscopeRange)}, desired value {value}, stored value {_gyroscopeRange}");
}
}
}
/// <summary>
/// Get or set the gyroscope bandwidth
/// </summary>
[Property]
public GyroscopeBandwidth GyroscopeBandwidth
{
get => _gyroscopeBandwidth;
set
{
if (value == GyroscopeBandwidth.Bandwidth8800HzFS32)
{
WriteRegister(Register.GYRO_CONFIG, (byte)((byte)GyroscopeRange | 0x01));
}
else if (value == GyroscopeBandwidth.Bandwidth3600HzFS32)
{
WriteRegister(Register.GYRO_CONFIG, (byte)((byte)GyroscopeRange | 0x02));
}
else
{
WriteRegister(Register.GYRO_CONFIG, (byte)GyroscopeRange);
WriteRegister(Register.CONFIG, (byte)value);
}
var retConf = ReadByte(Register.GYRO_CONFIG);
if ((retConf & 0x01) == 0x01)
{
_gyroscopeBandwidth = GyroscopeBandwidth.Bandwidth8800HzFS32;
}
else if ((retConf & 0x03) == 0x00)
{
_gyroscopeBandwidth = (GyroscopeBandwidth)ReadByte(Register.CONFIG);
}
else
{
_gyroscopeBandwidth = GyroscopeBandwidth.Bandwidth3600HzFS32;
}
if (_gyroscopeBandwidth != value)
{
throw new IOException($"Can set {nameof(GyroscopeBandwidth)}, desired value {value}, stored value {_gyroscopeBandwidth}");
}
}
}
/// <summary>
/// Get the real gyroscope bandwidth. This allows to calculate the real
/// angular rate in degree per second
/// </summary>
[Property]
public float GyroscopeScale
{
get
{
float val = GyroscopeRange switch
{
GyroscopeRange.Range0250Dps => 250.0f,
GyroscopeRange.Range0500Dps => 500.0f,
GyroscopeRange.Range1000Dps => 1000.0f,
GyroscopeRange.Range2000Dps => 2000.0f,
_ => 0,
};
val /= Adc;
// the sample rate diver only apply for the non FS modes
if ((GyroscopeBandwidth != GyroscopeBandwidth.Bandwidth3600HzFS32) &&
(GyroscopeBandwidth != GyroscopeBandwidth.Bandwidth8800HzFS32))
{
return val / (1 + SampleRateDivider);
}
return val;
}
}
/// <summary>
/// Get the gyroscope in degrees per seconds
/// </summary>
/// <remarks>
/// Vector axes are the following:
/// +Z +Y
/// \ | /
/// \ | /
/// \|/
/// /|\
/// / | \
/// / | \
/// +X
/// </remarks>
[Telemetry("AngularRate")]
public Vector3 GetGyroscopeReading() => GetRawGyroscope() * GyroscopeScale;
/// <summary>
/// Gets the raw gyroscope data
/// </summary>
/// <returns></returns>
internal Vector3 GetRawGyroscope()
{
SpanByte rawData = new byte[6]
{
0,
0,
0,
0,
0,
0
};
Vector3 gyro = new Vector3();
ReadBytes(Register.GYRO_XOUT_H, rawData);
gyro.X = BinaryPrimitives.ReadInt16BigEndian(rawData);
gyro.Y = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(2));
gyro.Z = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(4));
return gyro;
}
#endregion
#region Temperature
/// <summary>
/// Get the temperature
/// </summary>
[Telemetry("Temperature")]
public Temperature GetTemperature()
{
SpanByte rawData = new byte[2]
{
0,
0
};
ReadBytes(Register.TEMP_OUT_H, rawData);
// formula from the documentation
return Temperature.FromDegreesCelsius((BinaryPrimitives.ReadInt16BigEndian(rawData) - 21) / 333.87 + 21);
}
#endregion
#region Modes, constructor, Dispose
/// <summary>
/// Setup the Wake On Motion. This mode generate a rising signal on pin INT
/// You can catch it with a normal GPIO and place an interruption on it if supported
/// Reading the sensor won't give any value until it wakes up periodically
/// Only Accelerator data is available in this mode
/// </summary>
/// <param name="accelerometerThreshold">Threshold of magnetometer x/y/z axes. LSB = 4mg. Range is 0mg to 1020mg</param>
/// <param name="acceleratorLowPower">Frequency used to measure data for the low power consumption mode</param>
[Command]
public void SetWakeOnMotion(uint accelerometerThreshold, AccelerometerLowPowerFrequency acceleratorLowPower)
{
// Using documentation page 31 of Product Specification to setup
_wakeOnMotion = true;
if (accelerometerThreshold > 1020)
{
throw new ArgumentException($"Value has to be between 0mg and 1020mg", nameof(accelerometerThreshold));
}
// LSB = 4mg
accelerometerThreshold /= 4;
// Make sure we start from a clean soft reset
PowerOn();
// PWR_MGMT_1 (0x6B) make CYCLE =0, SLEEP = 0 and STANDBY = 0
WriteRegister(Register.PWR_MGMT_1, (byte)ClockSource.Internal20MHz);
// PWR_MGMT_2 (0x6C) set DIS_XA, DIS_YA, DIS_ZA = 0 and DIS_XG, DIS_YG, DIS_ZG = 1
// Remove the Gyroscope
WriteRegister(Register.PWR_MGMT_2, (byte)(DisableModes.DisableGyroscopeX | DisableModes.DisableGyroscopeY | DisableModes.DisableGyroscopeZ));
// ACCEL_CONFIG 2 (0x1D) set ACCEL_FCHOICE_B = 0 and A_DLPFCFG[2:0]=1(b001)
// Bandwidth for Accelerator to 184Hz
AccelerometerBandwidth = AccelerometerBandwidth.Bandwidth0184Hz;
// Enable Motion Interrupt
// In INT_ENABLE (0x38), set the whole register to 0x40 to enable motion interrupt only
WriteRegister(Register.INT_ENABLE, 0x40);
// Enable AccelHardware Intelligence:
// In MOT_DETECT_CTRL (0x69), set ACCEL_INTEL_EN = 1 and ACCEL_INTEL_MODE = 1
WriteRegister(Register.MOT_DETECT_CTRL, 0b1100_0000);
// Set Motion Threshold:
// In WOM_THR (0x1F), set the WOM_Threshold[7:0] to 1~255 LSBs (0~1020mg)
WriteRegister(Register.WOM_THR, (byte)accelerometerThreshold);
// Set Frequency of Wake-up:
// In LP_ACCEL_ODR (0x1E), set Lposc_clksel[3:0] = 0.24Hz ~ 500Hz
WriteRegister(Register.LP_ACCEL_ODR, (byte)acceleratorLowPower);
// Enable Cycle Mode (AccelLow Power Mode):
// In PWR_MGMT_1 (0x6B) make CYCLE =1
WriteRegister(Register.PWR_MGMT_1, 0b0010_0000);
// Motion Interrupt Configuration Completed
}
internal void Reset()
{
WriteRegister(Register.PWR_MGMT_1, 0x80);
// http://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf, section 4.23.
// Maximum risen time is 100 ms after VDD
Thread.Sleep(100);
_wakeOnMotion = false;
}
internal void PowerOn()
{
// this should be a soft reset
WriteRegister(Register.PWR_MGMT_1, 0x01);
// http://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf, section 4.23.
// Maximum risen time is 100 ms after VDD
Thread.Sleep(100);
_wakeOnMotion = false;
}
/// <summary>
/// Return true if the version of MPU6050 is the correct one
/// </summary>
/// <returns>True if success</returns>
// Check if the version is thee correct one
internal bool CheckVersion() => ReadByte(Register.WHO_AM_I) == 0x68;
/// <summary>
/// Get or set the sample diver mode
/// </summary>
[Property]
public byte SampleRateDivider
{
get => ReadByte(Register.SMPLRT_DIV);
set => WriteRegister(Register.SMPLRT_DIV, value);
}
/// <summary>
/// Get or set the elements to disable.
/// It can be any axes of the accelerometer and or the gyroscope
/// </summary>
public DisableModes DisableModes
{
get => (DisableModes)ReadByte(Register.PWR_MGMT_2);
set => WriteRegister(Register.PWR_MGMT_2, (byte)value);
}
#endregion
#region FIFO
/// <summary>
/// Get the number of elements to read from the FIFO (First In First Out) buffer
/// </summary>
public uint FifoCount
{
get
{
SpanByte rawData = new byte[2]
{
0,
0
};
ReadBytes(Register.FIFO_COUNTH, rawData);
return BinaryPrimitives.ReadUInt16BigEndian(rawData);
}
}
/// <summary>
/// Get or set the FIFO (First In First Out) modes
/// </summary>
public FifoModes FifoModes
{
get => (FifoModes)(ReadByte(Register.FIFO_EN));
set
{
if (value != FifoModes.None)
{
// Make sure the FIFO is enabled
var usrCtl = (UserControls)ReadByte(Register.USER_CTRL);
usrCtl = usrCtl | UserControls.FIFO_RST;
WriteRegister(Register.USER_CTRL, (byte)usrCtl);
}
else
{
// Deactivate FIFO
var usrCtl = (UserControls)ReadByte(Register.USER_CTRL);
usrCtl = usrCtl & ~UserControls.FIFO_RST;
WriteRegister(Register.USER_CTRL, (byte)usrCtl);
}
WriteRegister(Register.FIFO_EN, (byte)value);
}
}
/// <summary>
/// Read data in the FIFO (First In First Out) buffer, read as many data as the size of readData byte span
/// You should read the number of data available in the FifoCount property then
/// read them here.
/// You will read only data you have selected in FifoModes.
/// Data are in the order of the Register from 0x3B to 0x60.
/// ACCEL_XOUT_H and ACCEL_XOUT_L
/// ACCEL_YOUT_H and ACCEL_YOUT_L
/// ACCEL_ZOUT_H and ACCEL_ZOUT_L
/// TEMP_OUT_H and TEMP_OUT_L
/// GYRO_XOUT_H and GYRO_XOUT_L
/// GYRO_YOUT_H and GYRO_YOUT_L
/// GYRO_ZOUT_H and GYRO_ZOUT_L
/// EXT_SENS_DATA_00 to EXT_SENS_DATA_24
/// </summary>
/// <param name="readData">Data which will be read</param>
public void ReadFifo(SpanByte readData) => ReadBytes(Register.FIFO_R_W, readData);
#endregion
#region Calibration and tests
/// <summary>
/// Perform full calibration the gyroscope and the accelerometer
/// It will automatically adjust as well the offset stored in the device
/// The result bias will be stored in the AcceloremeterBias and GyroscopeBias
/// </summary>
/// <returns>Gyroscope and accelerometer bias</returns>
[Command]
public GyroscopeAccelerometer CalibrateGyroscopeAccelerometer()
{
// = 131 LSB/degrees/sec
const int GyroSensitivity = 131;
// = 16384 LSB/g
const int AccSensitivity = 16384;
byte i2cMaster;
byte userControls;
SpanByte rawData = new byte[12];
Vector3 gyroBias = new Vector3();
Vector3 acceBias = new Vector3();
Reset();
// Enable accelerator and gyroscope
DisableModes = DisableModes.DisableNone;
Thread.Sleep(200);
// Disable all interrupts
WriteRegister(Register.INT_ENABLE, 0x00);
// Disable FIFO
FifoModes = FifoModes.None;
// Disable I2C master
i2cMaster = ReadByte(Register.I2C_MST_CTRL);
WriteRegister(Register.I2C_MST_CTRL, 0x00);
// Disable FIFO and I2C master modes
userControls = ReadByte(Register.USER_CTRL);
WriteRegister(Register.USER_CTRL, (byte)UserControls.None);
// Reset FIFO and DMP
WriteRegister(Register.USER_CTRL, (byte)UserControls.FIFO_RST);
DelayHelper.DelayMilliseconds(15, false);
// Configure MPU6050 gyro and accelerometer for bias calculation
// Set low-pass filter to 184 Hz
GyroscopeBandwidth = GyroscopeBandwidth.Bandwidth0184Hz;
AccelerometerBandwidth = AccelerometerBandwidth.Bandwidth0184Hz;
// Set sample rate to 1 kHz
SampleRateDivider = 0;
// Set gyro to maximum sensitivity
GyroscopeRange = GyroscopeRange.Range0250Dps;
AccelerometerRange = AccelerometerRange.Range02G;
// Configure FIFO will be needed for bias calculation
FifoModes = FifoModes.GyroscopeX | FifoModes.GyroscopeY | FifoModes.GyroscopeZ | FifoModes.Accelerometer;
// accumulate 40 samples in 40 milliseconds = 480 bytes
// Do not exceed 512 bytes max buffer
DelayHelper.DelayMilliseconds(40, false);
// We have our data, deactivate FIFO
FifoModes = FifoModes.None;
// How many sets of full gyro and accelerometer data for averaging
var packetCount = FifoCount / 12;
for (uint reading = 0; reading < packetCount; reading++)
{
Vector3 accel_temp = new Vector3();
Vector3 gyro_temp = new Vector3();
// Read data
ReadBytes(Register.FIFO_R_W, rawData);
// Form signed 16-bit integer for each sample in FIFO
accel_temp.X = BinaryPrimitives.ReadInt16BigEndian(rawData);
accel_temp.Y = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(2));
accel_temp.Z = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(4));
gyro_temp.X = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(6));
gyro_temp.Y = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(8));
gyro_temp.Z = BinaryPrimitives.ReadInt16BigEndian(rawData.Slice(10));
acceBias += accel_temp;
gyroBias += gyro_temp;
}
// Make the average
acceBias /= packetCount;
gyroBias /= packetCount;
// bias on Z is cumulative
acceBias.Z += acceBias.Z > 0 ? -AccSensitivity : AccSensitivity;
// Divide by 4 to get 32.9 LSB per deg/s
// Biases are additive, so change sign on calculated average gyro biases
rawData[0] = (byte)(((int)(-gyroBias.X / 4) >> 8) & 0xFF);
rawData[1] = (byte)((int)(-gyroBias.X / 4) & 0xFF);
rawData[2] = (byte)(((int)(-gyroBias.Y / 4) >> 8) & 0xFF);
rawData[3] = (byte)((int)(-gyroBias.Y / 4) & 0xFF);
rawData[4] = (byte)(((int)(-gyroBias.Z / 4) >> 8) & 0xFF);
rawData[5] = (byte)((int)(-gyroBias.Z / 4) & 0xFF);
// Changes all Gyroscope offsets
WriteRegister(Register.XG_OFFSET_H, rawData[0]);
WriteRegister(Register.XG_OFFSET_L, rawData[1]);
WriteRegister(Register.YG_OFFSET_H, rawData[2]);
WriteRegister(Register.YG_OFFSET_L, rawData[3]);
WriteRegister(Register.ZG_OFFSET_H, rawData[4]);
WriteRegister(Register.ZG_OFFSET_L, rawData[5]);
// Output scaled gyro biases for display in the main program
_gyroscopeBias = gyroBias / GyroSensitivity;
// Construct the accelerometer biases for push to the hardware accelerometer
// bias registers. These registers contain factory trim values which must be
// added to the calculated accelerometer biases; on boot up these registers
// will hold non-zero values. In addition, bit 0 of the lower byte must be
// preserved since it is used for temperature compensation calculations.
// Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.
// A place to hold the factory accelerometer trim biases
Vector3 accel_bias_reg = new Vector3();
SpanByte accData = new byte[2];
// Read factory accelerometer trim values
ReadBytes(Register.XA_OFFSET_H, accData);
accel_bias_reg.X = BinaryPrimitives.ReadInt16BigEndian(accData);
ReadBytes(Register.YA_OFFSET_H, accData);
accel_bias_reg.Y = BinaryPrimitives.ReadInt16BigEndian(accData);
ReadBytes(Register.ZA_OFFSET_H, accData);
accel_bias_reg.Z = BinaryPrimitives.ReadInt16BigEndian(accData);
// Define mask for temperature compensation bit 0 of lower byte of
// accelerometer bias registers
uint mask = 0x01;
// Define array to hold mask bit for each accelerometer bias axis
SpanByte mask_bit = new byte[3];
// If temperature compensation bit is set, record that fact in mask_bit
mask_bit[0] = (((uint)accel_bias_reg.X & mask) == mask) ? (byte)0x01 : (byte)0x00;
mask_bit[1] = (((uint)accel_bias_reg.Y & mask) == mask) ? (byte)0x01 : (byte)0x00;
mask_bit[2] = (((uint)accel_bias_reg.Z & mask) == mask) ? (byte)0x01 : (byte)0x00;
// Construct total accelerometer bias, including calculated average
// accelerometer bias from above
// Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g
// (16 g full scale) and keep the mask
accel_bias_reg -= acceBias / 8;
// Add the "reserved" mask as it was
rawData[0] = (byte)(((int)accel_bias_reg.X >> 8) & 0xFF);
rawData[1] = (byte)(((int)accel_bias_reg.X & 0xFF) | mask_bit[0]);
rawData[2] = (byte)(((int)accel_bias_reg.Y >> 8) & 0xFF);
rawData[3] = (byte)(((int)accel_bias_reg.Y & 0xFF) | mask_bit[1]);
rawData[4] = (byte)(((int)accel_bias_reg.Z >> 8) & 0xFF);
rawData[5] = (byte)(((int)accel_bias_reg.Z & 0xFF) | mask_bit[2]);
// Push accelerometer biases to hardware registers
WriteRegister(Register.XA_OFFSET_H, rawData[0]);
WriteRegister(Register.XA_OFFSET_L, rawData[1]);
WriteRegister(Register.YA_OFFSET_H, rawData[2]);
WriteRegister(Register.YA_OFFSET_L, rawData[3]);
WriteRegister(Register.ZA_OFFSET_H, rawData[4]);
WriteRegister(Register.ZA_OFFSET_L, rawData[5]);
// Restore the previous modes
WriteRegister(Register.USER_CTRL, (byte)(userControls | (byte)UserControls.I2C_MST_EN));
i2cMaster = (byte)(i2cMaster & (~(byte)(I2cBusFrequency.Frequency348kHz) | (byte)I2cBusFrequency.Frequency400kHz));
WriteRegister(Register.I2C_MST_CTRL, i2cMaster);
DelayHelper.DelayMilliseconds(10, false);
// Finally store the acceleration bias
_accelerometerBias = acceBias / AccSensitivity;
return new GyroscopeAccelerometer(_gyroscopeBias, _accelerometerBias);
}
#endregion
#region I2C
/// <summary>
/// Write data on any of the I2C replica attached to the MPU9250
/// </summary>
/// <param name="i2cChannel">The replica channel to attached to the I2C device</param>
/// <param name="address">The I2C address of the replica I2C element</param>
/// <param name="register">The register to write to the replica I2C element</param>
/// <param name="data">The byte data to write to the replica I2C element</param>
public void WriteByteToReplicaDevice(I2cChannel i2cChannel, byte address, byte register, byte data)
{
// I2C_SLVx_ADDR += 3 * i2cChannel
byte slvAddress = (byte)((byte)Register.I2C_SLV0_ADDR + 3 * (byte)i2cChannel);
SpanByte dataout = new byte[2]
{
slvAddress,
address
};
_i2cDevice.Write(dataout);
// I2C_SLVx_REG = I2C_SLVx_ADDR + 1
dataout[0] = (byte)(slvAddress + 1);
dataout[1] = register;
_i2cDevice.Write(dataout);
// I2C_SLVx_D0 = I2C_SLV0_DO + i2cChannel
// Except Channel4
byte channelData = i2cChannel != I2cChannel.Slave4 ? (byte)((byte)Register.I2C_SLV0_DO + (byte)i2cChannel) : (byte)Register.I2C_SLV4_DO;
dataout[0] = channelData;
dataout[1] = data;
_i2cDevice.Write(dataout);
// I2C_SLVx_CTRL = I2C_SLVx_ADDR + 2
dataout[0] = (byte)(slvAddress + 2);
dataout[1] = 0x81;
_i2cDevice.Write(dataout);
}
/// <summary>
/// Read data from any of the I2C replica attached to the MPU9250
/// </summary>
/// <param name="i2cChannel">The replica channel to attached to the I2C device</param>
/// <param name="address">The I2C address of the replica I2C element</param>
/// <param name="register">The register to read from the replica I2C element</param>
/// <param name="readBytes">The read data</param>
public void ReadByteFromReplicaDevice(I2cChannel i2cChannel, byte address, byte register, SpanByte readBytes)
{
if (readBytes.Length > 24)
{
throw new ArgumentException("Value must be 24 bytes or less.", nameof(readBytes));
}
byte slvAddress = (byte)((byte)Register.I2C_SLV0_ADDR + 3 * (byte)i2cChannel);
SpanByte dataout = new byte[2]
{
slvAddress,
(byte)(address | 0x80)
};
_i2cDevice.Write(dataout);
dataout[0] = (byte)(slvAddress + 1);
dataout[1] = (byte)register;
_i2cDevice.Write(dataout);
dataout[0] = (byte)(slvAddress + 2);
dataout[1] = (byte)(0x80 | readBytes.Length);
_i2cDevice.Write(dataout);
// Just need to wait a very little bit
// For data transfer to happen and process on the MPU9250 side
DelayHelper.DelayMicroseconds(140 + readBytes.Length * 10, false);
_i2cDevice.WriteByte((byte)Register.EXT_SENS_DATA_00);
_i2cDevice.Read(readBytes);
}
internal void WriteRegister(Register reg, byte data)
{
SpanByte dataout = new byte[]
{
(byte)reg,
data
};
_i2cDevice.Write(dataout);
}
internal byte ReadByte(Register reg)
{
_i2cDevice.WriteByte((byte)reg);
return _i2cDevice.ReadByte();
}
internal void ReadBytes(Register reg, SpanByte readBytes)
{
_i2cDevice.WriteByte((byte)reg);
_i2cDevice.Read(readBytes);
}
/// <summary>
/// Cleanup everything
/// </summary>
public void Dispose()
{
_i2cDevice?.Dispose();
_i2cDevice = null!;
}
#endregion
}
}