1////////////////////////////////////////////////////////////////////////////////
2// //
3// This software is supplied under the terms of a license agreement or //
4// nondisclosure agreement with Mitov Software and may not be copied //
5// or disclosed except in accordance with the terms of that agreement. //
6// Copyright(c) 2002-2016 Mitov Software. All Rights Reserved. //
7// //
8////////////////////////////////////////////////////////////////////////////////
9#ifndef _MITOV_MPU9250_h
11#define _MITOV_MPU9250_h
12#include <Mitov.h>
14#include <Wire.h> //I2C Arduino Library
15#include <Mitov_Basic_I2C.h>
16#include <Mitov_Compass_AK8963.h>
17namespace Mitov
19{
20//---------------------------------------------------------------------------
21 namespace MPU9250Const
22 {
23 const byte Adresses[ 2 ] = { 0x68, 0x68 };
24 const byte MPU9150_RA_MAG_ADDRESS = 0x0C;
26 const byte MPU9150_RA_MAG_ST1 = 0x02;
28 const byte MPU9150_RA_MAG_XOUT_L = 0x03;
29 const byte MPU9150_RA_MAG_XOUT_H = 0x04;
30 const byte MPU9150_RA_MAG_YOUT_L = 0x05;
31 const byte MPU9150_RA_MAG_YOUT_H = 0x06;
32 const byte MPU9150_RA_MAG_ZOUT_L = 0x07;
33 const byte MPU9150_RA_MAG_ZOUT_H = 0x08;
34 const byte MPU9150_RA_MAG_ST2 = 0x09;
35 const byte MPU9150_RA_MAG_CNTL1 = 0x0A;
36 const byte MPU9150_RA_MAG_CNTL2 = 0x0B;
37 const byte MPU9150_RA_MAG_ASAX = 0x10;
39 const byte MPU9150_RA_MAG_ASAY = 0x11;
40 const byte MPU9150_RA_MAG_ASAZ = 0x12;
41 };
42//---------------------------------------------------------------------------
43 enum MPU9250ClockSource { mcsAutoSelect = 6, mcsInternal = 0, mcsGyroX = 1, mcsGyroY = 2, mcsGyroZ = 3, mcsExt32K = 4, mcsExt19M = 5, mcsReset = 7 };
44//---------------------------------------------------------------------------
45 class MPU9250BasicSensor : public OpenWire::Object
46 {
47 public:
48 OpenWire::SourcePin OutputPins[ 3 ];
49 };
51//---------------------------------------------------------------------------
52 class MPU9250OptionalSensor
53 {
54 public:
55 bool Enabled : 1;
56 bool Queue : 1;
57 bool SelfTest : 1; // Added to save space as bitfield
58 public:
60 MPU9250OptionalSensor() :
61 Enabled( true ),
62 Queue( false ),
63 SelfTest( false )
64 {
65 }
66 };
68//---------------------------------------------------------------------------
69 typedef MPU9250OptionalSensor MPU9250OptionalSelfTestSensor;
70//---------------------------------------------------------------------------
71 class MPU9250OptionalAxesSensor : public MPU9250BasicSensor
72 {
73 public:
74 MPU9250OptionalSelfTestSensor X;
75 MPU9250OptionalSelfTestSensor Y;
76 MPU9250OptionalSelfTestSensor Z;
77 };
78//---------------------------------------------------------------------------
79 enum ArduinoMPU9250AccelerometerRange : uint8_t { ar2g, ar4g, ar8g, ar16g };
80//---------------------------------------------------------------------------
81 class MPU9250Accelerometer : public MPU9250OptionalAxesSensor
82 {
83 public:
84 ArduinoMPU9250AccelerometerRange FullScaleRange = ar2g;
85 };
87//---------------------------------------------------------------------------
88 enum TArduinoMPU9250GyroscopeRange : uint8_t { gr250dps, gr500dps, gr1000dps, gr2000dps };
89//---------------------------------------------------------------------------
90 class MPU9250Gyroscope : public MPU9250OptionalAxesSensor
91 {
92 public:
93 TArduinoMPU9250GyroscopeRange FullScaleRange = gr250dps;
94 };
96//---------------------------------------------------------------------------
97 class MPU9250Compass : public MPU9250BasicSensor
98 {
99 public:
100 CompassAK8963Mode Mode : 2;
101 bool Enabled : 1;
102 bool HighResolution : 1;
103 public:
105 OpenWire::SinkPin ResetInputPin;
106 OpenWire::SourcePin OverflowOutputPin;
107 protected:
109 void DoResetReceive( void *_Data )
110 {
111 I2C::WriteByte( MPU9250Const::MPU9150_RA_MAG_ADDRESS, MPU9250Const::MPU9150_RA_MAG_CNTL2, 0b00000001 );
112 }
113 public:
115 MPU9250Compass() :
116 Mode( cm100Hz ),
117 Enabled( true ),
118 HighResolution( true )
119 {
120 ResetInputPin.SetCallback( this, (OpenWire::TOnPinReceive)&MPU9250Compass::DoResetReceive );
121 }
122 };
123//---------------------------------------------------------------------------
124 class MPU9250Thermometer : public MPU9250OptionalSensor
125 {
126 public:
127 OpenWire::SourcePin OutputPin;
128 };
129//---------------------------------------------------------------------------
130 enum MPU9250GyroscopeThermometerFilter
131 {
132 gtf_GB_8800Hz_GF_32KHz_TB_4000Hz,
133 gtf_GB_3600Hz_GF_32KHz_TB_4000Hz,
134 gtf_GB_250Hz_GF_8KHz_TB_4000Hz,
135 gtf_GB_184Hz_GF_1KHz_TB_188Hz,
136 gtf_GB_92Hz_GF_1KHz_TB_98Hz,
137 gtf_GB_41Hz_GF_1KHz_TB_42Hz,
138 gtf_GB_20Hz_GF_1KHz_TB_20Hz,
139 gtf_GB_10Hz_GF_1KHz_TB_10Hz,
140 gtf_GB_5Hz_GF_1KHz_TB_5Hz,
141 gtf_GB_3600Hz_GF_8KHz_TB_4000Hz
142 };
143//---------------------------------------------------------------------------
144 enum MPU9250FrameSynchronizationLocation { fslDisabled, fslThermometer, fslGyroscopeX, fslGyroscopeY, fslGyroscopeZ, fslAccelerometerX, fslAccelerometerY, fslAccelerometerZ };
145//---------------------------------------------------------------------------
146 class MPU9250FrameSynchronization
147 {
148 public:
149 MPU9250FrameSynchronizationLocation Location : 3;
150 bool EnableInterrupt : 1;
151 bool InterruptOnLowLevel : 1;
152 public:
154 MPU9250FrameSynchronization() :
155 Location( fslDisabled ),
156 EnableInterrupt( false ),
157 InterruptOnLowLevel( false )
158 {
159 }
160 };
161//---------------------------------------------------------------------------
162 class MPU9250Interrupt
163 {
164 public:
165 bool Inverted : 1;
166 bool OpenDrain : 1;
167 bool Latch : 1;
168 public:
170 MPU9250Interrupt() :
171 Inverted( false ),
172 OpenDrain( false ),
173 Latch( true )
174 {
175 }
176 };
177//---------------------------------------------------------------------------
178 class MPU9250Queue
179 {
180 public:
181 bool BlockOnFull = false;
182 };
183//---------------------------------------------------------------------------
184 class MPU9250I2C : public Mitov::EnabledComponent, public Mitov::ClockingSupport
185 {
186 typedef Mitov::EnabledComponent inherited;
187 protected:
189 static const byte MPU9250_RA_SMPLRT_DIV = 0x19;
190 static const byte MPU9250_RA_CONFIG = 0x1A;
191 static const byte MPU9250_RA_GYRO_CONFIG = 0x1B;
192 static const byte MPU9250_RA_ACCEL_CONFIG = 0x1C;
193 static const byte MPU9250_RA_INT_PIN_CFG = 0x37;
194 static const byte MPU9250_RA_ACCEL_XOUT_H = 0x3B;
195 static const byte MPU9250_RA_ACCEL_XOUT_L = 0x3C;
196 static const byte MPU9250_RA_ACCEL_YOUT_H = 0x3D;
197 static const byte MPU9250_RA_ACCEL_YOUT_L = 0x3E;
198 static const byte MPU9250_RA_ACCEL_ZOUT_H = 0x3F;
199 static const byte MPU9250_RA_ACCEL_ZOUT_L = 0x40;
200 static const byte MPU9250_RA_PWR_MGMT_1 = 0x6B;
201 static const byte MPU9250_RA_PWR_MGMT_2 = 0x6C;
202 public:
204 OpenWire::SinkPin ResetInputPin;
205 public:
207 bool Address : 1;
208 bool Standby : 1;
209 MPU9250ClockSource ClockSource : 3;
210 MPU9250GyroscopeThermometerFilter GyroscopeThermometerFilter : 4;
211 uint8_t SampleRateDivider = 0;
212 public:
214 MPU9250Accelerometer Accelerometer;
215 MPU9250Gyroscope Gyroscope;
216 MPU9250Compass Compass;
217 MPU9250Thermometer Thermometer;
218 MPU9250FrameSynchronization FrameSynchronization;
219 MPU9250Queue Queue;
220 MPU9250Interrupt Interrupt;
221 protected:
223 float CompassAdjustmentValues[ 3 ];
224 protected:
226 virtual void DoClockReceive( void *_Data ) override
227 {
228 ReadSensors();
229 }
230 void DoResetReceive( void *_Data )
232 {
233 }
234 protected:
236 void ReadSensors()
237 {
238// Serial.println( "ReadSensors" );
239 const float AccelerometerCoefficients [] =
240 {
241 2.0f / 32768,
242 4.0f / 32768,
243 8.0f / 32768,
244 16.0f / 32768
245 };
246 const float GyroscopeCoefficients [] =
248 {
249 250.0f / 32768,
250 500.0f / 32768,
251 1000.0f / 32768,
252 2000.0f / 32768
253 };
254 const float CompassCoefficients[] =
256 {
257 10.0f *4219.0f / 8190.0f,
258 10.0f *4219.0f / 32760.0f
259 };
260 uint8_t AIntValues[ 7 * 2 ];
262 if( ReadBytes( MPU9250_RA_ACCEL_XOUT_H, sizeof( AIntValues ), AIntValues ))
264 {
265 for( int i = 0; i < 3; ++i )
266 {
267 float AValue = (( ((int16_t)AIntValues[ i * 2 ] ) << 8 ) | AIntValues[ i * 2 + 1 ] ) * AccelerometerCoefficients[ Accelerometer.FullScaleRange ];
268 Accelerometer.OutputPins[ i ].Notify( &AValue );
269 }
270 for( int i = 0; i < 3; ++i )
272 {
273 float AValue = (((int16_t)AIntValues[ ( i + 4 ) * 2 ] ) << 8 | AIntValues[ ( i + 4 ) * 2 + 1 ] ) * GyroscopeCoefficients[ Gyroscope.FullScaleRange & 0x11 ];
274 Gyroscope.OutputPins[ i ].Notify( &AValue );
275 }
276 float AValue = ((((int16_t)AIntValues[ 3 * 2 ] ) << 8 ) | AIntValues[ 3 * 2 + 1 ] ) / 333.87 + 21.0;
278 Thermometer.OutputPin.Notify( &AValue );
279 }
280 if( I2C::ReadBytes( MPU9250Const::MPU9150_RA_MAG_ADDRESS, MPU9250Const::MPU9150_RA_MAG_ST1, 1, AIntValues ))
282 {
283 Compass.OverflowOutputPin.SendValue( AIntValues[ 0 ] & 0b00000010 );
284 if( AIntValues[ 0 ] & 0b00000001 )
285 if( I2C::ReadBytes( MPU9250Const::MPU9150_RA_MAG_ADDRESS, MPU9250Const::MPU9150_RA_MAG_XOUT_L, 7, AIntValues ))
286 for( int i = 0; i < 3; ++i )
287 {
288 float AValue = (( ((int16_t)AIntValues[ i * 2 + 1 ] ) << 8 ) | AIntValues[ i * 2 ] ) * CompassCoefficients[ Compass.HighResolution & 1 ] * CompassAdjustmentValues[ i ];
289 Compass.OutputPins[ i ].Notify( &AValue );
290 }
291 }
292 }
294 protected:
296 inline void WriteTo( byte ARegister, byte AData )
297 {
298 I2C::WriteByte( MPU9250Const::Adresses[ Address ], ARegister, AData );
299 }
300 bool ReadBytes( uint8_t regAddr, uint8_t length, void *data )
302 {
303 return I2C::ReadBytes( MPU9250Const::Adresses[ Address ], regAddr, length, data );
304 }
305 protected:
307 void UpdatePowerManagementReg1()
308 {
309 uint8_t AValue;
310 if( ClockSource = mcsAutoSelect )
311 {
312 if( Gyroscope.X.Enabled )
313 AValue = mcsGyroX;
314 else if( Gyroscope.Y.Enabled )
316 AValue = mcsGyroY;
317 else if( Gyroscope.Z.Enabled )
319 AValue = mcsGyroZ;
320 else
322 AValue = mcsInternal;
323 }
324 else
326 AValue = ClockSource;
327 AValue |= ( Enabled ? 0 : 0b01000000 ) |
329 ( Standby ? 0b00010000 : 0 ) |
330 ( Thermometer.Enabled ? 0 : 0b00001000 );
331 WriteTo( MPU9250_RA_PWR_MGMT_1, AValue );
333 }
334 void UpdatePowerManagementReg2()
336 {
337 uint8_t AValue = ( Accelerometer.X.Enabled ? 0 : 0b00100000 ) |
338 ( Accelerometer.Y.Enabled ? 0 : 0b00010000 ) |
339 ( Accelerometer.Z.Enabled ? 0 : 0b00001000 ) |
340 ( Gyroscope.X.Enabled ? 0 : 0b00000100 ) |
341 ( Gyroscope.Y.Enabled ? 0 : 0b00000010 ) |
342 ( Gyroscope.Z.Enabled ? 0 : 0b00000001 );
343 WriteTo( MPU9250_RA_PWR_MGMT_2, AValue );
345 }
346 void UpdateConfigReg()
348 {
349 uint8_t AValue = (( GyroscopeThermometerFilter - 2 ) & 0b111 ) |
350 ( Queue.BlockOnFull ? 0b01000000 : 0 ) |
351 (( FrameSynchronization.Location & 0b111 ) << 4 );
352 WriteTo( MPU9250_RA_CONFIG, AValue );
354 }
355 void UpdateGyroConfigReg()
357 {
358 uint8_t AValue;
359 switch( GyroscopeThermometerFilter )
360 {
361 case gtf_GB_8800Hz_GF_32KHz_TB_4000Hz: AValue = 0b11; break;
362 case gtf_GB_3600Hz_GF_32KHz_TB_4000Hz: AValue = 0b10; break;
363 default : AValue = 0b00;
364 }
365 AValue |= (( Gyroscope.FullScaleRange & 0b11 ) << 3 ) |
367 ( Gyroscope.X.SelfTest ? 0b10000000 : 0 ) |
368 ( Gyroscope.Y.SelfTest ? 0b01000000 : 0 ) |
369 ( Gyroscope.Z.SelfTest ? 0b00100000 : 0 );
370 WriteTo( MPU9250_RA_GYRO_CONFIG, AValue );
372 }
373 void UpdateAccelerometerConfigReg()
375 {
376 byte AValue = (( Accelerometer.FullScaleRange & 0b11 ) << 3 ) |
377 ( Accelerometer.X.SelfTest ? 0b10000000 : 0 ) |
378 ( Accelerometer.Y.SelfTest ? 0b01000000 : 0 ) |
379 ( Accelerometer.Z.SelfTest ? 0b00100000 : 0 );
380 WriteTo( MPU9250_RA_ACCEL_CONFIG, AValue );
382 }
383 void UpdatenterruptPinAndBypassConfigReg( bool ADirectCompassAccess )
385 {
386 byte AValue = ( Interrupt.Inverted ? 0b10000000 : 0 ) |
387 ( Interrupt.OpenDrain ? 0b01000000 : 0 ) |
388 ( Interrupt.Latch ? 0b00100000 : 0 ) |
389 ( FrameSynchronization.InterruptOnLowLevel ? 0b00001000 : 0 ) |
390 ( FrameSynchronization.EnableInterrupt ? 0b00000100 : 0 ) |
391 ( Thermometer.Queue & ( ! ADirectCompassAccess ) ? 0 : 0b00000010 );
392// Serial.print( "UpdatenterruptPinAndBypassConfigReg: " ); Serial.println( AValue, BIN );
394 WriteTo( MPU9250_RA_INT_PIN_CFG, AValue );
396 }
397 void UpdateSampleRateDividerReg()
399 {
400 WriteTo( MPU9250_RA_SMPLRT_DIV, SampleRateDivider );
401 }
402 void UpdateCompassControlReg()
404 {
405 byte AValue;
406 if( Compass.Enabled )
407 AValue = CompassAK8963Const::CompassModes[ Compass.Mode ];
408 else
410 AValue = 0;
411 AValue |= ( Compass.HighResolution ? 0b00010000 : 0 );
413 I2C::WriteByte( MPU9250Const::MPU9150_RA_MAG_ADDRESS, MPU9250Const::MPU9150_RA_MAG_CNTL1, AValue );
415 }
416 void ReagCompassAdjustmentValues()
418 {
419 uint8_t AValues[ 3 ];
420 I2C::ReadBytes( MPU9250Const::MPU9150_RA_MAG_ADDRESS, MPU9250Const::MPU9150_RA_MAG_ASAX, sizeof( AValues ), AValues );
422 for( int i = 0; i < 3; ++i )
423 CompassAdjustmentValues[ i ] = (((float) AValues[ i ] ) - 128.0f) / 256.0f + 1.0f;
424 }
426 protected:
428 virtual void SystemStart() override
429 {
430 UpdatePowerManagementReg1();
431 UpdatePowerManagementReg2();
432 UpdateConfigReg();
433 UpdateGyroConfigReg();
434 UpdateAccelerometerConfigReg();
435 UpdateSampleRateDividerReg();
436 UpdatenterruptPinAndBypassConfigReg( true );
437 UpdateCompassControlReg();
438 ReagCompassAdjustmentValues();
439 UpdatenterruptPinAndBypassConfigReg( false );
440 inherited::SystemStart();
442 }
443 virtual void SystemLoopBegin( unsigned long currentMicros ) override
445 {
446 if( Enabled )
447 if( ! ClockInputPin.IsConnected() )
448 ReadSensors();
449 inherited::SystemLoopBegin( currentMicros );
451 }
452 public:
454 MPU9250I2C() :
455 ClockSource( mcsAutoSelect ),
456 GyroscopeThermometerFilter( gtf_GB_250Hz_GF_8KHz_TB_4000Hz ),
457 Address( false ),
458 Standby( false )
459 {
460 ResetInputPin.SetCallback( MAKE_CALLBACK( MPU9250I2C::DoResetReceive ));
461 }
462 };
463//---------------------------------------------------------------------------
464}
465#endif