Home
JAQForum Ver 24.01
Log In or Join  
Active Topics
Local Time 21:30 01 Jul 2025 Privacy Policy
Jump to

Notice. New forum software under development. It's going to miss a few functions and look a bit ugly for a while, but I'm working on it full time now as the old forum was too unstable. Couple days, all good. If you notice any issues, please contact me.

Forum Index : Microcontroller and PC projects : uM2(+): Tilt compensated compass

Author Message
matherp
Guru

Joined: 11/12/2012
Location: United Kingdom
Posts: 10180
Posted: 07:47am 11 Mar 2016
Copy link to clipboard 
Print this post

This has taken more effort than any code recently and yet it is all in Basic

However, finally I think it is working well. What is really interesting is how big the variance is in the uncompensated headings with just very small tilts of the magnetometer.

The code uses the MPU6050 and HMC5883 modules which must be orientated relative to each other like this.



The maths is done using quaternions to implement what is termed "sensor fusion". This integrates the output of the accelerometer, the gyro, and the magnetometer to produced a composite quaternion that represents the orientation of the sensors in 3D space. Then using some simple maths this can be translated into normal concepts like roll, pitch and heading (yaw).

Much of the code has been copied and ported from C based on work by Kris Winer which was in turn derived from the original math by Madgwick and Mahoney.

The code also uses some of the Basic routines previously posted by TassyJim.

Before using the code it is essential to calibrate the HMC5883 using this code.

When you run the code you will see the compensated heading slowly slew to the correct value as the fusion engine integrates the magnetic input with the arbitrary start position. This takes 10-15 seconds on a MX470 and somewhat more on a MX170.

Once correct the compensated heading will stay stable even when the sensors are tilted whereas the simple "atan2(y,x)" 2D heading will move significantly.

The code actually implements a complete AHRS system and includes the math for generating pitch and roll as well as heading.

Jim: I'm very interested in you giving this a go and letting me know your experiences.


option explicit
option default FLOAT
'
' Micromite AHRS using MPU6050
' Original source code from kriswiner (https://github.com/kriswiner/MPU-6050)
' and Sebastion Madgwick's Open source IMU and AHRS algorithms
'
' Use TFT display in landscape mode to display position
'
' MPU6050 connections
' GND
' VCC - 3.3V
' SCL to I2C clock
' SDA to I2C data
' NCS to 3.3V (needed to force I2C)
' INT to intPin
' AD0 to GND or 3.3V (see address below)
const intPin = 16
'const MPU6050_ADDRESS =&H69 ' Device address when ADO = 1
const MPU6050_ADDRESS =&H68 ' Device address when ADO = 0
'

'Magnetometer Registers and constants
'
' Calibration constants derived from running calibration algorithm
'
const hmc5883_Gauss_LSB_X = 381 'least significant bits per uT
const hmc5883_Gauss_LSB_Y = 403
const hmc5883_Gauss_LSB_Z = 349
const magbias_x = 12.2 ' User environmental correction (hard-iron) in MICROTESLA from calibration routine
const magbias_y = 20.6
const magbias_z = 2.7
'
const declination=-0.63 ' local magnetic declination in degrees
'
const HMC5883L_I2C = &h1e
const HMC5883_MAGGAIN_4_7 = &hA0' +/- 4.7
const GAUSS_TO_MICROTESLA = 100
Const HMC5883_CONFIG_A = 0
Const HMC5883_CONFIG_B = 1
Const HMC5883_MODE = 2
Const HMC5883_MSB_X = 3
Const HMC5883_ID_A = 10

' Gyro/accelerometer registers
const SMPLRT_DIV =&H19
const CONFIG =&H1A
const GYRO_CONFIG =&H1B
const ACCEL_CONFIG =&H1C
const INT_PIN_CFG =&H37
const INT_ENABLE =&H38
const ACCEL_XOUT_H =&H3B
const TEMP_OUT_H =&H41
const USER_CTRL =&H6A ' Bit 7 enable DMP, bit 3 reset DMP
const PWR_MGMT_1 =&H6B ' Device defaults to the SLEEP mode
const WHO_AM_I_MPU6050 =&H75 ' Should return =&H68
const AFS_2G = 0
const AFS_4G =1
const AFS_8G =2
const AFS_16G =3
const GFS_250DPS = 0
const GFS_500DPS =1
const GFS_1000DPS =2
const GFS_2000DPS =3
const sampleFreq=20.0 ' print output frequency in samples
'
' Global Variable definitions
'
dim float GyroMeasError = PI * (40.0 / 180.0) ' gyroscope measurement error in rads/s (start at 40 deg/s)
' There is a tradeoff in the beta parameter between accuracy and response speed.
' In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
' However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
' Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
' By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
' I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
' the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
' In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
dim float beta = sqr(3.0 / 4.0) * GyroMeasError ' compute beta
BETA = 0.2 'derived from testing
dim float q(3) = (0,0,0,0)' quaternion of sensor frame relative to auxiliary frame
q(0)=sqr(2)/2
q(1)=sqr(2)/2
DIM INTEGER Gscale = GFS_250DPS
DIM INTEGER Ascale = AFS_4G
DIM FLOAT magcount(2), accelCount(2),gyrocount(2),gRes,aRes
dim float ax,ay,az,gx,gy,gz,mx,my,mz
dim float pitch,yaw,roll,heading
dim integer gdata(13),temp%
DIM ID$ length 3
DIM FLOAT deltat = 1/sampleFreq
'
' Setup
'
i2c open 400,1000
setpin intPin,DIN
pause 100
writeByte(MPU6050_ADDRESS,PWR_MGMT_1,&H80)
pause 500
initMPU6050
temp%=readByte(MPU6050_ADDRESS,WHO_AM_I_MPU6050)
if temp%<>&H68 then
Print "MPU6050 not found"
end
endif
readByteString(HMC5883L_I2C,HMC5883_ID_A,3,ID$)
if ID$<>"H43" then
Print "HMC5883 not found"
end
endif
print "Internal temperature is ",readTempData()
initHMC5883
getAres
getGres
timer=0
do
readMPU6050andmag
deltat=timer/1000:timer=0
MadgwickQuaternionUpdate(gx, gy, gz, ax, ay, az, my, -mx, mz)
yaw = atan2(2.0 * (q(1) * q(2) + q(0) * q(3)), q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3))
pitch = -asin(2.0 * (q(1) * q(3) - q(0) * q(2)))
roll = atan2(2.0 * (q(0) * q(1) + q(2) * q(3)), q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3))
pitch=deg(pitch)
yaw = deg(yaw)
yaw = ((450-yaw)- declination) mod 360'
roll = deg(roll)
heading = DEG(atan2(magcount(1),magcount(0))) - declination
if (temp% mod sampleFreq) = 0 then
Print "2D Heading = "+str$(heading,3,1," ")+ " Compensated heading = "+str$(yaw,3,1," ")
'print "Pitch = "+str$(pitch,3,2," ")+" Yaw = "+str$(yaw,3,2," ")+" Roll = "+str$(roll,3,2," "),+" Heading = "+str$(heading,3,1," ")
endif
temp%=temp%+1
loop
end
'
sub readMPU6050andmag
do while not(pin(intPin)): loop
readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 14, gData())
readMagData(magCount())' Read the x/y/z adc values
readAccelData(gData(),accelCount()) ' Convert the x/y/z adc values
' Now we'll calculate the accleration value into actual g's
ax = accelCount(0)*aRes' get actual g value, this depends on scale being set
ay = accelCount(1)*aRes'
az = accelCount(2)*aRes'
' text 0,20,"Acc X="+str$(ax,3,2," ")+" Y="+str$(ay,3,2," ")+" Z="+str$(az,3,2," ")
readGyroData(gData(),gyroCount())' Convert the x/y/z adc values
' Calculate the gyro value into actual radians per second
gx = RAD(gyroCount(0)*gRes)' get actual gyro value, this depends on scale being set
gy = RAD(gyroCount(1)*gRes)
gz = RAD(gyroCount(2)*gRes)
' text 0,40,"Rot X="+str$(gx,3,2," ")+" Y="+str$(gy,3,2," ")+" Z="+str$(gz,3,2," ")
' Calculate the magnetometer values in milliGauss
' Include factory calibration per data sheet and user environmental corrections
mx = magCount(0)' get actual magnetometer value, this depends on scale being set
my = magCount(1)
mz = magCount(2)
' text 0,0, "Mag X="+str$(mx,3,2," ")+" Y="+str$(my,3,2," ")+" Z="+str$(mz,3,2," ")
end sub
'
sub initMPU6050
LOCAL INTEGER c
' wake up device
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, &H00) ' Clear sleep mode bit (6), enable all sensors
pause 100 ' Wait for all registers to reset
' get stable time source
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, &H01) ' Auto select clock source to be PLL gyroscope reference if ready else
pause 200
' Configure Gyro and Thermometer
' Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively
' minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
' be higher than 1 / 0.0059 = 170 Hz
' DLPF_CFG = bits 2:0 = 100 this limits the sample rate to 1000 Hz for both
' With the MPU6050, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
writeByte(MPU6050_ADDRESS, CONFIG, &H04)
' Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
writeByte(MPU6050_ADDRESS, SMPLRT_DIV, &H04) ' Use a 200 Hz rate a rate consistent with the filter update rate
' Set gyroscope full scale range
' Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
c = readByte(MPU6050_ADDRESS, GYRO_CONFIG)
c=c AND &B11100100
c = c OR (Gscale << 3) ' Set full scale range for the gyro
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c ) ' Write new GYRO_CONFIG value to register
' Set accelerometer full-scale range configuration
c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG)
c=c AND &B11100111
c = c OR (Ascale << 3) ' Set full scale range for the accelerometer
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c) ' Write new ACCEL_CONFIG register value
' Set accelerometer sample rate configuration
' It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
' accel_fchoice_b bit (3) in this case the bandwidth is 1.13 kHz
' The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
' but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
' Configure Interrupts and Bypass Enable
' Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
' clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips
' can join the I2C bus and all can be controlled by the Arduino as master
writeByte(MPU6050_ADDRESS, INT_PIN_CFG, &H22)
writeByte(MPU6050_ADDRESS, INT_ENABLE, &H01) ' Enable data ready (bit 0) interrupt
writeByte(MPU6050_ADDRESS, USER_CTRL, &H05)
pause 100
end sub

sub initHMC5883()
I2C WRITE HMC5883L_i2c, 0, 2, HMC5883_CONFIG_A, &h14 '1-average, 30 Hz, normal measurement
I2C WRITE HMC5883L_i2c, 0, 2, HMC5883_CONFIG_B, HMC5883_MAGGAIN_4_7 'Gain=4, or any other desired gain
I2C WRITE HMC5883L_i2c, 0, 2, HMC5883_MODE, &h00 'Single-measurement mode
end sub

sub writeByte(address%,reg%,value%)
i2c write address%,0,2,reg%,value%
end sub
'
function readByte(address%,reg%) as integer
i2c write address%,1,1,reg%
i2c read address%,0,1,readByte
end function
'
sub readBytes(address%,reg%,n%,a%())
i2c write address%,1,1,reg%
i2c read address%,0,n%,a%()
end sub
'
sub readByteString(address%,reg%,n%,s$)
i2c write address%,1,1,reg%
i2c read address%,0,n%,S$
end sub
'
sub getGres
select case Gscale
' Possible gyro scales (and their register bit settings) are:
'250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
' Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case GFS_250DPS
gRes = 250.0/32768.0
case GFS_500DPS
gRes = 500.0/32768.0
case GFS_1000DPS
gRes = 1000.0/32768.0
case GFS_2000DPS
gRes = 2000.0/32768.0
end select
end sub
'
sub getAres
select case Ascale
' Possible accelerometer scales (and their register bit settings) are:
' 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
' Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G
aRes = 2.0/32768.0
case AFS_4G
aRes = 4.0/32768.0
case AFS_8G
aRes = 8.0/32768.0
case AFS_16G:
aRes = 16.0/32768.0
end select
end sub
'
sub readAccelData(rawData() as integer,destination() as float)
destination(0) = sint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
destination(1) = sint16((rawData(2) << 8) OR rawData(3))
destination(2) = sint16((rawData(4) << 8) OR rawData(5))
end sub
'
sub readGyroData(rawData() as integer,destination() as float)
destination(0) = sint16((rawData(8) << 8) OR rawData(9)) 'Turn the MSB and LSB into a signed 16-bit value
destination(1) = sint16((rawData(10) << 8) OR rawData(11))
destination(2) = sint16((rawData(12) << 8) OR rawData(13))
end sub
'
sub readMagData(destination() as float)
local integer rawData(5)' x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
readBytes(HMC5883L_I2C, HMC5883_MSB_X, 6, rawData())' Read the six raw data
destination(0) = (sint16((rawData(0) << 8) OR rawData(1)))/hmc5883_Gauss_LSB_X * GAUSS_TO_MICROTESLA + magbias_x
destination(1) = (sint16((rawData(4) << 8) OR rawData(5)))/hmc5883_Gauss_LSB_Y * GAUSS_TO_MICROTESLA + magbias_y
destination(2) = (sint16((rawData(2) << 8) OR rawData(3)))/hmc5883_Gauss_LSB_Z * GAUSS_TO_MICROTESLA + magbias_z
end sub
'
function readTempData() as float
local integer rawData(1) ' x/y/z gyro register data stored here
readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, rawData()) ' Read the two raw data registers sequentially into data array
readTempData=sint16((rawData(0) << 8) OR rawData(1)) / 340 + 36.53
end function
'
FUNCTION sint16(x as integer) as float ' convert to signed 16 bit number
local a%=x
IF a% AND &H8000 then a%=a% OR &HFFFFFFFFFFFF0000
sint16 = a%
END FUNCTION
'

FUNCTION atan2(y as float,x as float) as float
IF x > 0 THEN
atan2 = ATN(y/x)
ELSEIF y >= 0 AND x < 0 THEN
atan2 = PI + ATN(y/x)
ELSEIF y < 0 AND x < 0 THEN
atan2 = ATN(y/x) - PI
ELSEIF y > 0 AND x = 0 THEN
atan2 = PI / 2
ELSEIF y < 0 AND x = 0 THEN
atan2 = PI / -2
ENDIF
IF atan2 < 0 THEN
atan2 = atan2 + 2 * PI
ENDIF
END FUNCTION

function asin(x!) as float
asin=atn(x!/sqr(1-x!*x!))
end function

sub MadgwickQuaternionUpdate(gx as float, gy as float, gz as float, ax as float, ay as float, az as float, mx as float, my as float, mz as float)

LOCAL float q1 = q(0), q2 = q(1), q3 = q(2), q4 = q(3)' short name local variable for readability
LOCAL float norm
LOCAL float hx, hy, _2bx, _2bz
LOCAL float s1, s2, s3, s4
LOCAL float qDot1, qDot2, qDot3, qDot4

' Auxiliary variables to avoid repeated arithmetic
LOCAL float _2q1mx
LOCAL float _2q1my
LOCAL float _2q1mz
LOCAL float _2q2mx
LOCAL float _4bx
LOCAL float _4bz
LOCAL float _2q1 = 2.0 * q1
LOCAL float _2q2 = 2.0 * q2
LOCAL float _2q3 = 2.0 * q3
LOCAL float _2q4 = 2.0 * q4
LOCAL float _2q1q3 = 2.0 * q1 * q3
LOCAL float _2q3q4 = 2.0 * q3 * q4
LOCAL float q1q1 = q1 * q1
LOCAL float q1q2 = q1 * q2
LOCAL float q1q3 = q1 * q3
LOCAL float q1q4 = q1 * q4
LOCAL float q2q2 = q2 * q2
LOCAL float q2q3 = q2 * q3
LOCAL float q2q4 = q2 * q4
LOCAL float q3q3 = q3 * q3
LOCAL float q3q4 = q3 * q4
LOCAL float q4q4 = q4 * q4

' Normalise accelerometer measurement
norm = sqr(ax * ax + ay * ay + az * az)
if (norm = 0.0) then exit sub' handle NaN
norm = 1.0/norm
ax = ax * norm
ay = ay * norm
az = az * norm


' Normalise magnetometer measurement
norm = sqr(mx * mx + my * my + mz * mz)
if (norm = 0.0) then exit sub' handle NaN
norm = 1.0/norm
mx = mx * norm
my = my * norm
mz = mz * norm


' Reference direction of Earth's magnetic field
_2q1mx = 2.0 * q1 * mx
_2q1my = 2.0 * q1 * my
_2q1mz = 2.0 * q1 * mz
_2q2mx = 2.0 * q2 * mx
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4
_2bx = sqr(hx * hx + hy * hy)
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4
_4bx = 2.0 * _2bx
_4bz = 2.0 * _2bz

' Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0 * q2q4 - _2q1q3 - ax) + _2q2 * (2.0 * q1q2 + _2q3q4 - ay)
s1 = s1 - _2bz * q3 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
s1 = s1 + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
s1 = s1 + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)
s2 = _2q4 * (2.0 * q2q4 - _2q1q3 - ax) + _2q1 * (2.0 * q1q2 + _2q3q4 - ay)
s2 = s2 - 4.0 * q2 * (1.0 - 2.0 * q2q2 - 2.0 * q3q3 - az) + _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4)+ _2bz * (q2q4 - q1q3) - mx)
s2 = s2 + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
s2 = s2 + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)
s3 = -_2q1 * (2.0 * q2q4 - _2q1q3 - ax) + _2q4 * (2.0 * q1q2 + _2q3q4 - ay)
s3 = s3 - 4.0 * q3 * (1.0 - 2.0 * q2q2 - 2.0 * q3q3 - az)
s3 = s3 + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
s3 = s3 + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
s3 = s3 + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)
s4 = _2q2 * (2.0 * q2q4 - _2q1q3 - ax) + _2q3 * (2.0 * q1q2 + _2q3q4 - ay)
s4 = s4 + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
s4 = s4 + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
s4 = s4 + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz)
norm = sqr(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4) ' normalise step magnitude
norm = 1.0/norm
s1 = s1 * norm
s2 = s2 * norm
s3 = s3 * norm
s4 = s4 * norm

' Compute rate of change of quaternion
qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1
qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - beta * s2
qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - beta * s3
qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - beta * s4

' Integrate to yield quaternion
q1 = q1 + qDot1 * deltat
q2 = q2 + qDot2 * deltat
q3 = q3 + qDot3 * deltat
q4 = q4 + qDot4 * deltat
norm = sqr(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) ' normalise quaternion
norm = 1.0/norm
q(0) = q1 * norm
q(1) = q2 * norm
q(2) = q3 * norm
q(3) = q4 * norm
end sub


Edited by matherp 2016-03-12
 
plasma
Guru

Joined: 08/04/2012
Location: Germany
Posts: 437
Posted: 07:49pm 11 Mar 2016
Copy link to clipboard 
Print this post

its great matherp ,thx a lot
 
TassyJim

Guru

Joined: 07/08/2011
Location: Australia
Posts: 6266
Posted: 08:26pm 11 Mar 2016
Copy link to clipboard 
Print this post

Just to make sure we are seeing the same thing,
My sensors are mounted together the same way as yours but...
Using your image of the two sensors, which way is forward and is the photo taken from above?

I have the X arrow on the HMC5883 pointing forwards and the Y arrow pointing to the left.

Next questions.
Does positive pitch has you climbing?
Is positive roll right wing down?

Jim
VK7JH
MMedit
 
matherp
Guru

Joined: 11/12/2012
Location: United Kingdom
Posts: 10180
Posted: 10:29pm 11 Mar 2016
Copy link to clipboard 
Print this post

Jim

The heading will be 0 when X on the HMC5883 points north. Because the MPU6050 and HMC5883 are at 90 degrees to each other this means pitch and roll are reversed just swap them to be consistent and change signs, add/subtract 180 degrees etc. to make them scale as you want.

You will also note that the calculation returns the yaw angle (where is north from me) rather than the heading so I use this calculation

((450-yaw)- declination) mod 360
to convert from the yaw angle to a heading and also to take into account the 90 degree orientation difference and local magnetic declination to give a true heading

Interestingly, the MPU9250 which contains a 9-axis sensor also has the magnetometer at 90-degrees - I've no idea why.

Edited by matherp 2016-03-13
 
WhiteWizzard
Guru

Joined: 05/04/2013
Location: United Kingdom
Posts: 2931
Posted: 11:21pm 11 Mar 2016
Copy link to clipboard 
Print this post

Peter, Jim,

Can I just ask something regarding the compass-chip (only) setup. IF it were used on a robot buggy so that it is horizontal to the 'floor', will the relative heading data be useable? i.e. If I wanted to turn right (90degrees), can I simply take note of 'current' heading, then turn the buggy until the difference in the 'new' heading is 90deg? I don't care about true north in this application. I just want to be able to turn accurately remembering that wheel/track slippage is preventing me from using 'timed' motor techniques for turning.

Thanks . . . .

WW
 
matherp
Guru

Joined: 11/12/2012
Location: United Kingdom
Posts: 10180
Posted: 12:57am 12 Mar 2016
Copy link to clipboard 
Print this post

  Quote  Can I just ask something regarding the compass-chip (only) setup. IF it were used on a robot buggy so that it is horizontal to the 'floor', will the relative heading data be useable? i.e. If I wanted to turn right (90degrees), can I simply take note of 'current' heading, then turn the buggy until the difference in the 'new' heading is 90deg? I don't care about true north in this application. I just want to be able to turn accurately remembering that wheel/track slippage is preventing me from using 'timed' motor techniques for turning.


This will work in theory but won't work very well in practice as the magnetic fields from the electric motors will b.....r the compass readings. A better solution would be to use the gyro in a MPU6050. Sample the z-axis frequently during the turn and add up the rate output * sample time until they equal 90 deg
 
WhiteWizzard
Guru

Joined: 05/04/2013
Location: United Kingdom
Posts: 2931
Posted: 01:37am 12 Mar 2016
Copy link to clipboard 
Print this post

  matherp said  . . . A better solution would be to use the gyro in a MPU6050. . . .


I see your point - but I am looking at 'volume' so the cost of the MCU being more than 5x the cost of the HMC compass part causes me an 'issue' for this particular application

I was hoping that the HMC be ok near the motors; especially after reading the data sheet's 'features' - but then became concerned when palcal posted issues regarding the reading errors when near his desk fan!

For now, assuming the HMC is ok(ish), then with the buggy going in a straight line (and hopefully the compass heading data being 'consistent'); then if the buggy were to go up (or down) and incline, are we saying the heading data would vary and hence the reason for the Gyro and the accelerometer to 'correctly stabilise' the heading data? Assume the buggy's wheels will remain on the ground/incline and hence no 'roll/spin' will be present to 'upset' the sensor.

Hope this makes sense - ultimately just tying to get accurate turns on a robot buggy at a low cost. I will need something because I am using low-cost tracks & motors that together have a small amount of 'slippage' on certain surfaces.

The other thing I will be trying an optical encoder on each motor shaft and then counting pulses; but I am not sure how this will perform against the slight slippage . . .

In the meantime I have ordered an MCU to test this all out; and will be keeping a very keen eye on this thread . . . . .
 
matherp
Guru

Joined: 11/12/2012
Location: United Kingdom
Posts: 10180
Posted: 01:53am 12 Mar 2016
Copy link to clipboard 
Print this post

I'm suggesting a gyro but no magnetometer.

Gyros modules are cheap enough e.g. Edited by matherp 2016-03-13
 
matherp
Guru

Joined: 11/12/2012
Location: United Kingdom
Posts: 10180
Posted: 06:45am 14 Mar 2016
Copy link to clipboard 
Print this post

I've updated the MPU6050 + HMC5883 code with the CFunction sensor fusion

option explicit
option default FLOAT
'
' Micromite AHRS using MPU6050 and HMC5883L
' Original source code from kriswiner (https://github.com/kriswiner/MPU-6050)
' and Sebastion Madgwick's Open source IMU and AHRS algorithms
'
' Use TFT display in landscape mode to display position
'
' MPU6050 connections
' GND
' VCC - 3.3V
' SCL to I2C clock
' SDA to I2C data
' INT to intPin
' AD0 to GND or 3.3V (see address below)
'
const intPin = 16
const screenIO=1 'set to 1 to do fast output on TFT screen, otherwise 0
'
'const MPU6050_ADDRESS =&H69 ' Device address when ADO = 1
const MPU6050_ADDRESS =&H68 ' Device address when ADO = 0
'

'Magnetometer Registers and constants
'
' Calibration constants derived from running HMC5883 calibration algorithm
'
const hmc5883_Gauss_LSB_X = 381 'least significant bits per uT
const hmc5883_Gauss_LSB_Y = 403
const hmc5883_Gauss_LSB_Z = 349
const magbias_x = 12.2 ' User environmental correction (hard-iron) in MICROTESLA from calibration routine
const magbias_y = 20.6
const magbias_z = 2.7
'
const declination=-0.63 ' local magnetic declination in degrees
'
const HMC5883L_I2C = &h1e
const HMC5883_MAGGAIN_4_7 = &hA0' +/- 4.7
const GAUSS_TO_MICROTESLA = 100
Const HMC5883_CONFIG_A = 0
Const HMC5883_CONFIG_B = 1
Const HMC5883_MODE = 2
Const HMC5883_MSB_X = 3
Const HMC5883_ID_A = 10

' Gyro/accelerometer registers
const SMPLRT_DIV =&H19
const CONFIG =&H1A
const GYRO_CONFIG =&H1B
const ACCEL_CONFIG =&H1C
const INT_PIN_CFG =&H37
const INT_ENABLE =&H38
const ACCEL_XOUT_H =&H3B
const TEMP_OUT_H =&H41
const USER_CTRL =&H6A ' Bit 7 enable DMP, bit 3 reset DMP
const PWR_MGMT_1 =&H6B ' Device defaults to the SLEEP mode
const WHO_AM_I_MPU6050 =&H75 ' Should return =&H68
const AFS_2G = 0
const AFS_4G =1
const AFS_8G =2
const AFS_16G =3
const GFS_250DPS = 0
const GFS_500DPS =1
const GFS_1000DPS =2
const GFS_2000DPS =3
const outputFreq=50 ' print output frequency in samples
'
'Calibration parameters derived by inspection of debug values with sensor fixed and level
'
dim float accelbias(2)=(0.009,-0.024,0.0), gyrobias(2)=(0.032,-0.015,0.0)
'
'
' Global Variable definitions
'
DIM FLOAT BETA = 0.2 ' gain of algorithm derived from testing
dim float q(3) = (0,0,0,0)' quaternion of sensor frame relative to auxiliary frame set to North
q(0)=sqr(2)/2
q(1)=sqr(2)/2
DIM INTEGER Gscale = GFS_250DPS
DIM INTEGER Ascale = AFS_4G
DIM FLOAT gRes,aRes
dim float ax,ay,az,gx,gy,gz,mx,my,mz 'Global variables to hold current sensor readings
dim float pitch,yaw,roll,heading ,tiltheading
dim integer gdata(13),mData(5),temp%
DIM ID$ length 3
DIM INTEGER lastrun
DIM FLOAT accs(2),mags(2),gees(2)
'
' Setup
'
cls
i2c open 400,1000
setpin intPin,DIN
pause 100
writeByte(MPU6050_ADDRESS,PWR_MGMT_1,&H80)
pause 500
initMPU6050
temp%=readByte(MPU6050_ADDRESS,WHO_AM_I_MPU6050)
if temp%<>&H68 then
Print "MPU6050 not found"
end
endif
readByteString(HMC5883L_I2C,HMC5883_ID_A,3,ID$)
if ID$<>"H43" then
Print "HMC5883 not found"
end
endif
print "Internal temperature is ",readTempData()
initHMC5883
getAres
getGres

do
readMPU6050andmag
accs(0)=ax:accs(1)=ay:accs(2)=az
gees(0)=gx:gees(1)=gy:gees(2)=gz
mags(0)=my:mags(1)=-mx:mags(2)=mz 'swap/reverse mag reading to match orientation of accelerometer
MadgwickQuaternionUpdate(q(),accs(),gees(),mags(),beta, lastrun, yaw ,pitch, roll)
tiltheading = ((450-yaw)- declination)
if tiltheading>360 then tiltheading=tiltheading-360
heading = DEG(atan2(my,mx)) - declination
if (temp% mod outputFreq) = 0 then
Print "2D Heading = "+str$(heading,3,1," ")+ " Compensated heading = "+str$(tiltheading,3,1," ")
if screenIO then text 0,60, "Pitch = "+str$(pitch,3,2," ")+" Yaw = "+str$(yaw,3,2," ")+" Roll = "+str$(roll,3,2," ")+" Heading = "+str$(tiltheading,3,1," ")
endif
temp%=temp%+1
if screenIO then text 0,60,"pitch="+str$(pitch,3,2," ")+" roll="+str$(roll,3,2," ")+" yaw="+str$(yaw,3,2," ")

loop
end
'
sub readMPU6050andmag
do while not(pin(intPin)): loop
readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 14, gData())
readBytes(HMC5883L_I2C, HMC5883_MSB_X, 6, mData())' Read the six raw data
' Calculate the magnetometer values in milliGauss
' Include factory calibration per data sheet and user environmental corrections
mx = (sint16((mData(0) << 8) OR mData(1)))/hmc5883_Gauss_LSB_X * GAUSS_TO_MICROTESLA + magbias_x
my = (sint16((mData(4) << 8) OR mData(5)))/hmc5883_Gauss_LSB_Y * GAUSS_TO_MICROTESLA + magbias_y
mz = (sint16((mData(2) << 8) OR mData(3)))/hmc5883_Gauss_LSB_Z * GAUSS_TO_MICROTESLA + magbias_z
' Now we'll calculate the acceleration value into actual g's
' this depends on scale being set
ax = sint16((gData(0) << 8) OR gData(1))*aRes +accelbias(0)' Turn the MSB and LSB into a signed 16-bit value
ay = sint16((gData(2) << 8) OR gData(3))*aRes +accelbias(1)
az = sint16((gData(4) << 8) OR gData(5))*aRes +accelbias(2)
' Calculate the gyro value into actual radians per second
gx = RAD(sint16((gData(8) << 8) OR gData(9))* gRes) +gyrobias(0) 'Turn the MSB and LSB into a signed 16-bit value
gy = RAD(sint16((gData(10) << 8) OR gData(11))* gRes) +gyrobias(1)
gz = RAD(sint16((gData(12) << 8) OR gData(13))* gRes) +gyrobias(2)
if screenIO then text 0,20,"Acc X="+str$(ax,3,3," ")+" Y="+str$(ay,3,3," ")+" Z="+str$(az,3,3," ")
if screenIO then text 0,40,"Rot X="+str$(gx,3,3," ")+" Y="+str$(gy,3,3," ")+" Z="+str$(gz,3,3," ")
if screenIO then text 0,0, "Mag X="+str$(mx,3,3," ")+" Y="+str$(my,3,3," ")+" Z="+str$(mz,3,3," ")
end sub
'
sub initMPU6050
LOCAL INTEGER c
' wake up device
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, &H00) ' Clear sleep mode bit (6), enable all sensors
pause 100 ' Wait for all registers to reset
' get stable time source
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, &H01) ' Auto select clock source to be PLL gyroscope reference if ready else
pause 200
' Configure Gyro and Thermometer
' Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively
' minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
' be higher than 1 / 0.0059 = 170 Hz
' DLPF_CFG = bits 2:0 = 100 this limits the sample rate to 1000 Hz for both
' With the MPU6050, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
writeByte(MPU6050_ADDRESS, CONFIG, &H04)
' Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
writeByte(MPU6050_ADDRESS, SMPLRT_DIV, &H04) ' Use a 200 Hz rate a rate consistent with the filter update rate
' Set gyroscope full scale range
' Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
c = readByte(MPU6050_ADDRESS, GYRO_CONFIG)
c=c AND &B11100100
c = c OR (Gscale << 3) ' Set full scale range for the gyro
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c ) ' Write new GYRO_CONFIG value to register
' Set accelerometer full-scale range configuration
c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG)
c=c AND &B11100111
c = c OR (Ascale << 3) ' Set full scale range for the accelerometer
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c) ' Write new ACCEL_CONFIG register value
' Set accelerometer sample rate configuration
' It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
' accel_fchoice_b bit (3) in this case the bandwidth is 1.13 kHz
' The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
' but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
' Configure Interrupts and Bypass Enable
' Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
' clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips
' can join the I2C bus and all can be controlled by the Arduino as master
writeByte(MPU6050_ADDRESS, INT_PIN_CFG, &H32) 'set interrupt cleared by any read
writeByte(MPU6050_ADDRESS, INT_ENABLE, &H01) ' Enable data ready (bit 0) interrupt
writeByte(MPU6050_ADDRESS, USER_CTRL, &H05)
pause 100
end sub

sub initHMC5883()
I2C WRITE HMC5883L_i2c, 0, 2, HMC5883_CONFIG_A, &h18 '1-average, 75 Hz, normal measurement
I2C WRITE HMC5883L_i2c, 0, 2, HMC5883_CONFIG_B, HMC5883_MAGGAIN_4_7 'Gain=4, or any other desired gain
I2C WRITE HMC5883L_i2c, 0, 2, HMC5883_MODE, &h00 'Single-measurement mode
end sub

sub writeByte(address%,reg%,value%)
i2c write address%,0,2,reg%,value%
end sub
'
function readByte(address%,reg%) as integer
i2c write address%,1,1,reg%
i2c read address%,0,1,readByte
end function
'
sub readBytes(address%,reg%,n%,a%())
i2c write address%,1,1,reg%
i2c read address%,0,n%,a%()
end sub
'
sub readByteString(address%,reg%,n%,s$)
i2c write address%,1,1,reg%
i2c read address%,0,n%,S$
end sub
'
sub getGres
select case Gscale
' Possible gyro scales (and their register bit settings) are:
'250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
' Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case GFS_250DPS
gRes = 250.0/32768.0
case GFS_500DPS
gRes = 500.0/32768.0
case GFS_1000DPS
gRes = 1000.0/32768.0
case GFS_2000DPS
gRes = 2000.0/32768.0
end select
end sub
'
sub getAres
select case Ascale
' Possible accelerometer scales (and their register bit settings) are:
' 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
' Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G
aRes = 2.0/32768.0
case AFS_4G
aRes = 4.0/32768.0
case AFS_8G
aRes = 8.0/32768.0
case AFS_16G:
aRes = 16.0/32768.0
end select
end sub
'
sub convertAccelData(rawData() as integer)
local float d(2)
d(0) = sint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
d(1) = sint16((rawData(2) << 8) OR rawData(3))
d(2) = sint16((rawData(4) << 8) OR rawData(5))
' Now we'll calculate the accleration value into actual g's
ax = d(0)*aRes+accelbias(0)' get actual g value, this depends on scale being set
ay = d(1)*aRes+accelbias(1)'
az = d(2)*aRes+accelbias(2)'
end sub
'
sub convertGyroData(rawData() as integer)
local float d(2)
d(0) = sint16((rawData(8) << 8) OR rawData(9)) 'Turn the MSB and LSB into a signed 16-bit value
d(1) = sint16((rawData(10) << 8) OR rawData(11))
d(2) = sint16((rawData(12) << 8) OR rawData(13))
' Calculate the gyro value into actual radians per second
gx = RAD(d(0)*gRes)+gyrobias(0)' get actual gyro value, this depends on scale being set
gy = RAD(d(1)*gRes)+gyrobias(1)
gz = RAD(d(2)*gRes)+gyrobias(2)
end sub
'
sub readMagData
local integer rawData(5)' x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
readBytes(HMC5883L_I2C, HMC5883_MSB_X, 6, rawData())' Read the six raw data
' Calculate the magnetometer values in milliGauss
' Include factory calibration per data sheet and user environmental corrections
mx = (sint16((rawData(0) << 8) OR rawData(1)))/hmc5883_Gauss_LSB_X * GAUSS_TO_MICROTESLA + magbias_x
my = (sint16((rawData(4) << 8) OR rawData(5)))/hmc5883_Gauss_LSB_Y * GAUSS_TO_MICROTESLA + magbias_y
mz = (sint16((rawData(2) << 8) OR rawData(3)))/hmc5883_Gauss_LSB_Z * GAUSS_TO_MICROTESLA + magbias_z
end sub
'
function readTempData() as float
local integer rawData(1) ' x/y/z gyro register data stored here
readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, rawData()) ' Read the two raw data registers sequentially into data array
readTempData=sint16((rawData(0) << 8) OR rawData(1)) / 340 + 36.53
end function
'
FUNCTION sint16(x as integer) as float ' convert to signed 16 bit number
local a%=x
IF a% AND &H8000 then a%=a% OR &HFFFFFFFFFFFF0000
sint16 = a%
END FUNCTION
'
FUNCTION atan2(y as float,x as float) as float
IF x > 0 THEN
atan2 = ATN(y/x)
ELSEIF y >= 0 AND x < 0 THEN
atan2 = PI + ATN(y/x)
ELSEIF y < 0 AND x < 0 THEN
atan2 = ATN(y/x) - PI
ELSEIF y > 0 AND x = 0 THEN
atan2 = PI / 2
ELSEIF y < 0 AND x = 0 THEN
atan2 = PI / -2
ENDIF
IF atan2 < 0 THEN
atan2 = atan2 + 2 * PI
ENDIF
END FUNCTION

CSUB MadgwickQuaternionUpdate
000000DA
'atan2
27BDFFC8 AFBF0034 AFBE0030 AFB7002C AFB60028 AFB50024 AFB40020 AFB3001C
AFB20018 AFB10014 AFB00010 00809021 00A0A021 3C109D00 8E02009C 3C044049
0040F809 24840FDA 0040B021 8E02009C 3C043FC9 0040F809 24840FDA 0040B821
8E02009C 3C04BFC9 0040F809 24840FDA 0040F021 8E02009C 3C0440C9 0040F809
24840FDA 0040A821 8E02009C 0040F809 00002021 00408821 8E02009C 0040F809
00002021 00409821 8E020068 02802021 0040F809 00002821 24030001 1443000A
3C029D00 8C500078 8C420064 02402021 0040F809 02802821 0200F809 00402021
10000054 00409821 8C420068 02402021 0040F809 02202821 2403FFFF 10430015
3C029D00 8C420068 02802021 0040F809 02202821 2403FFFF 1443000E 3C029D00
8C53005C 8C500078 8C420064 02402021 0040F809 02802821 0200F809 00402021
02C02021 0260F809 00402821 10000039 00409821 8C420068 02402021 0040F809
02202821 2403FFFF 14430015 3C029D00 8C420068 02802021 0040F809 02202821
2403FFFF 1443000E 3C029D00 8C530060 8C500078 8C420064 02402021 0040F809
02802821 0200F809 00402021 00402021 0260F809 02C02821 1000001E 00409821
8C420068 02402021 0040F809 02202821 24030001 14430008 3C029D00 8C420068
02802021 0040F809 02202821 50400011 02E09821 3C029D00 8C420068 02402021
0040F809 02202821 2403FFFF 1443000A 3C029D00 8C420068 02802021 0040F809
02202821 50400003 03C09821 10000002 3C029D00 3C029D00 8C420068 02602021
0040F809 02202821 2403FFFF 14430008 02601021 3C029D00 8C42005C 02602021
0040F809 02A02821 00409821 02601021 8FBF0034 8FBE0030 8FB7002C 8FB60028
8FB50024 8FB40020 8FB3001C 8FB20018 8FB10014 8FB00010 03E00008 27BD0038
'asin
27BDFFC8 AFBF0034 AFB70030 AFB6002C AFB50028 AFB40024 AFB30020 AFB2001C
AFB10018 AFB00014 00808821 3C109D00 8E02009C 0040F809 3C043F00 0040A021
8E02009C 0040F809 3C043F80 00409021 8E170078 8E160064 8E150074 8E130060
8E020058 02202021 0040F809 02202821 02402021 0260F809 00402821 00402021
02A0F809 02802821 02202021 02C0F809 00402821 02E0F809 00402021 8FBF0034
8FB70030 8FB6002C 8FB50028 8FB40024 8FB30020 8FB2001C 8FB10018 8FB00014
03E00008 27BD0038
'main
27BDFF30 AFBF00CC AFBE00C8 AFB700C4 AFB600C0 AFB500BC AFB400B8 AFB300B4
AFB200B0 AFB100AC AFB000A8 0080B021 8FA200E4 40034800 8C440000 AFA40018
8C440004 14800007 00608021 54800007 AC500000 8FA80018 0068202B 50800003
AC500000 00608021 AC500000 AC400004 8ED40000 8ED70004 8ED20008 8ED3000C
8CBE0000 8CA20004 AFA20010 8CA50008 AFA50020 8CC30000 AFA30054 8CC40004
AFA40058 8CC60008 AFA6005C 8CE80000 AFA80028 8CE20004 AFA20030 8CE70008
AFA70024 3C119D00 8E22009C 0040F809 3C044000 0040A821 8E22009C 0040F809
3C043F00 AFA20038 8E22009C 0040F809 3C043F80 AFA20044 8E22009C 0040F809
00002021 AFA20040 8E22009C 0040F809 3C044080 AFA20050 8E22009C 3C044265
0040F809 24842EE2 AFA20078 8FA200E0 8C420000 AFA20060 8E230058 AFA3001C
8E240064 AFA40014 8FA80018 02088023 8E220080 02002021 0040F809 00102FC3
00408021 8E230000 8E220080 8C640000 0040F809 00002821 02002021 8FA30014
0060F809 00402821 00402021 8FA8001C 0100F809 02A02821 AFA20064 8E220058
02A02021 0040F809 02802821 AFA2006C 8E220058 02A02021 0040F809 02E02821
AFA20048 8E220058 02A02021 0040F809 02402821 AFA20068 8E220058 02A02021
0040F809 02602821 AFA20084 8E300058 02A02021 0200F809 02802821 00402021
0200F809 02402821 AFA20088 8E300058 02A02021 0200F809 02402821 00402021
0200F809 02602821 AFA2008C 8E220058 02802021 0040F809 02802821 AFA2004C
8E220058 02802021 0040F809 02E02821 AFA20070 8E220058 02802021 0040F809
02402821 AFA20074 8E220058 02802021 0040F809 02602821 AFA2007C 8E220058
02E02021 0040F809 02E02821 AFA2002C 8E220058 02E02021 0040F809 02402821
AFA20090 8E220058 02E02021 0040F809 02602821 AFA2003C 8E220058 02402021
0040F809 02402821 AFA20014 8E220058 02402021 0040F809 02602821 AFA20080
8E220058 02602021 0040F809 02602821 AFA20034 8E220074 AFA20018 8E30005C
8E220058 03C02021 0040F809 03C02821 AFA2001C 8E220058 8FA40010 0040F809
00802821 8FA4001C 0200F809 00402821 AFA2001C 8E220058 8FA40020 0040F809
00802821 8FA4001C 0200F809 00402821 00402021 8FA30018 0060F809 8FA50038
AFA20018 8E220068 8FA40018 0040F809 8FA50040 104004FD 8FBF00CC 3C109D00
8E020064 8FA40044 0040F809 8FA50018 00408821 8E020058 03C02021 0040F809
02202821 AFA2009C 8E020058 8FA40010 0040F809 02202821 AFA200A0 8E020058
8FA40020 0040F809 02202821 AFA200A4 8E040074 AFA40010 8E11005C 8E020058
8FA40028 0040F809 00802821 0040F021 8E020058 8FA40030 0040F809 00802821
03C02021 0220F809 00402821 0040F021 8E020058 8FA40024 0040F809 00802821
03C02021 0220F809 00402821 00402021 8FA80010 0100F809 8FA50038 0040F021
8E020068 03C02021 0040F809 8FA50040 104004C6 8FBF00CC 3C119D00 8E220064
8FA40044 0040F809 03C02821 00408021 8E220058 8FA40028 0040F809 02002821
AFA2001C 8E220058 8FA40030 0040F809 02002821 AFA20020 8E220058 8FA40024
0040F809 02002821 AFA20018 8E300058 02A02021 0200F809 02802821 00402021
0200F809 8FA5001C AFA20028 8E300058 02A02021 0200F809 02802821 00402021
0200F809 8FA50020 AFA20030 8E300058 02A02021 0200F809 02802821 00402021
0200F809 8FA50018 AFA20024 8E300058 02A02021 0200F809 02E02821 00402021
0200F809 8FA5001C AFA20094 8E3E005C 8E220060 AFA20010 8E220058 8FA4001C
0040F809 8FA5004C 00408021 8E220058 8FA40030 0040F809 02602821 02002021
8FA30010 0060F809 00402821 00408021 8E220058 8FA40024 0040F809 02402821
02002021 03C0F809 00402821 00408021 8E220058 8FA4001C 0040F809 8FA5002C
02002021 03C0F809 00402821 AFA20098 8E300058 8FA40048 0200F809 8FA50020
00402021 0200F809 02402821 8FA40098 03C0F809 00402821 AFA20098 8E300058
8FA40048 0200F809 8FA50018 00402021 0200F809 02602821 8FA40098 03C0F809
00402821 00408021 8E220058 8FA4001C 0040F809 8FA50014 02002021 8FA80010
0100F809 00402821 00408021 8E220058 8FA4001C 0040F809 8FA50034 02002021
8FA30010 0060F809 00402821 0040F021 8E220058 8FA40028 0040F809 02602821
00408021 8E24005C AFA40010 8E220058 8FA40020 0040F809 8FA5004C 02002021
8FA80010 0100F809 00402821 00408021 8E220060 AFA20010 8E220058 8FA40024
0040F809 02E02821 02002021 8FA30010 0060F809 00402821 00408021 8E24005C
AFA40010 8E220058 8FA40094 0040F809 02402821 02002021 8FA80010 0100F809
00402821 00408021 8E220060 AFA20010 8E220058 8FA40020 0040F809 8FA5002C
02002021 8FA30010 0060F809 00402821 00408021 8E24005C AFA40010 8E220058
8FA40020 0040F809 8FA50014 02002021 8FA80010 0100F809 00402821 AFA20010
8E22005C AFA20024 8E300058 8FA40068 0200F809 8FA50018 00402021 0200F809
02602821 8FA40010 8FA30024 0060F809 00402821 00408021 8E240060 AFA40010
8E220058 8FA40020 0040F809 8FA50034 02002021 8FA80010 0100F809 00402821
00408021 8E220074 AFA20010 8E23005C AFA30024 8E220058 03C02021 0040F809
03C02821 0040F021 8E220058 02002021 0040F809 02002821 03C02021 8FA80024
0100F809 00402821 00402021 8FA30010 0060F809 8FA50038 AFA20010 8E220058
8FA40030 0040F809 02E02821 00408021 8E3E0060 8E220058 8FA40028 0040F809
02402821 02002021 03C0F809 00402821 00408021 8E3E005C 8E220058 8FA40018
0040F809 8FA5004C 02002021 03C0F809 00402821 00408021 8E3E005C 8E220058
8FA40094 0040F809 02602821 02002021 03C0F809 00402821 00408021 8E3E0060
8E220058 8FA40018 0040F809 8FA5002C 02002021 03C0F809 00402821 0040F021
8E24005C AFA40028 8E300058 8FA40068 0200F809 8FA50020 00402021 0200F809
02602821 03C02021 8FA80028 0100F809 00402821 00408021 8E3E0060 8E220058
8FA40018 0040F809 8FA50014 02002021 03C0F809 00402821 00408021 8E3E005C
8E220058 8FA40018 0040F809 8FA50034 02002021 03C0F809 00402821 0040F021
8E220058 02A02021 0040F809 8FA50010 AFA2004C 8E220058 02A02021 0040F809
03C02821 AFA20094 8E220058 AFA20028 8E300060 8FA40038 0200F809 8FA5002C
00402021 0200F809 8FA50014 03C02021 8FA30028 0060F809 00402821 AFA20030
8E300058 8E22005C 8FA40070 0040F809 8FA50080 03C02021 0200F809 00402821
AFA20024 8E300058 8E220060 8FA4003C 0040F809 8FA50074 03C02021 0200F809
00402821 AFA20080 8E240058 AFA40028 8E300060 8FA40038 0200F809 8FA50014
00402021 0200F809 8FA50034 8FA40010 8FA80028 0100F809 00402821 AFA20034
8E300058 8E220060 8FA40090 0040F809 8FA5007C 8FA40010 0200F809 00402821
AFA2007C 8E300058 8E22005C 8FA40074 0040F809 8FA5003C 8FA40010 0200F809
00402821 AFA20074 8E300060 8E220058 02A02021 0040F809 8FA5003C 00402021
0200F809 8FA50088 00402021 0200F809 8FA5009C AFA20028 8E300060 8E22005C
8FA4007C 0040F809 8FA50024 00402021 0200F809 8FA50020 AFA20020 8E300060
8E22005C 8FA40074 0040F809 8FA50030 00402021 0200F809 8FA50018 AFA20018
8E300060 8E22005C 8FA40034 0040F809 8FA50080 00402021 0200F809 8FA5001C
AFA2001C 8E220060 AFA20030 8E30005C 8E220058 02A02021 0040F809 8FA50070
00402021 0200F809 8FA5008C 00402021 8FA30030 0060F809 8FA500A0 AFA20030
8E300060 8E220058 02A02021 0040F809 8FA5002C 8FA40044 0200F809 00402821
AFA20024 8E220058 02A02021 0040F809 8FA50014 8FA40024 0200F809 00402821
00402021 0200F809 8FA500A4 AFA20034 8E24005C AFA40014 8E280060 AFA80024
8E220058 8FA40048 0040F809 8FA50030 00408021 8E220058 8FA40068 0040F809
8FA50028 02002021 8FA30024 0060F809 00402821 AFA2002C 8E300058 03C02021
0200F809 02402821 00402021 0200F809 8FA5001C 8FA4002C 8FA80024 0100F809
00402821 AFA20024 8E220060 AFA2002C 8E220058 03C02021 0040F809 02E02821
AFA2003C 8E300058 8FA40010 0200F809 02602821 8FA4003C 8FA3002C 0060F809
00402821 8FA40020 0200F809 00402821 8FA40024 8FA80014 0100F809 00402821
AFA20024 8E300058 8FA40010 0200F809 02402821 00402021 0200F809 8FA50018
8FA40024 8FA30014 0060F809 00402821 AFA20024 8E240060 AFA4002C 8E28005C
AFA80014 8E220058 8FA40084 0040F809 8FA50028 00408021 8E220058 8FA4006C
0040F809 8FA50030 02002021 8FA30014 0060F809 00402821 AFA2003C 8E300058
8FA40050 0200F809 02E02821 00402021 0200F809 8FA50034 8FA4003C 8FA8002C
0100F809 00402821 AFA2002C 8E300058 03C02021 0200F809 02602821 00402021
0200F809 8FA5001C 8FA4002C 8FA30014 0060F809 00402821 AFA2002C 8E24005C
AFA40014 8E220058 8FA40010 0040F809 02402821 AFA2003C 8E300058 03C02021
0200F809 02802821 8FA4003C 8FA80014 0100F809 00402821 8FA40020 0200F809
00402821 8FA4002C 8FA30014 0060F809 00402821 AFA2002C 8E240060 AFA4003C
8E220058 8FA40010 0040F809 02602821 AFA20070 8E300058 8FA40094 0200F809
02E02821 8FA40070 8FA8003C 0100F809 00402821 8FA40018 0200F809 00402821
8FA4002C 8FA30014 0060F809 00402821 AFA2002C 8E24005C AFA4003C 8E280060
AFA80014 8E220058 8FA40084 0040F809 8FA50030 00408021 8E220058 8FA4006C
0040F809 8FA50028 02002021 8FA30014 0060F809 00402821 AFA2006C 8E300058
8FA40050 0200F809 02402821 00402021 0200F809 8FA50034 8FA4006C 8FA80014
0100F809 00402821 AFA20034 8E300058 8E220060 AFA20014 8FA40040 0040F809
8FA5004C 00402021 0200F809 02402821 AFA20050 8E300058 03C02021 0200F809
02802821 8FA40050 8FA30014 0060F809 00402821 8FA4001C 0200F809 00402821
8FA40034 8FA8003C 0100F809 00402821 AFA20034 8E22005C AFA20014 8E220058
8FA40010 0040F809 02E02821 AFA2003C 8E300058 03C02021 0200F809 02602821
8FA4003C 8FA30014 0060F809 00402821 8FA40020 0200F809 00402821 8FA40034
8FA80014 0100F809 00402821 AFA20034 8E220060 AFA2003C 8E220058 8FA40010
0040F809 02802821 AFA20050 8E300058 8FA40094 0200F809 02402821 8FA40050
8FA3003C 0060F809 00402821 8FA40018 0200F809 00402821 8FA40034 8FA80014
0100F809 00402821 AFA20034 8E22005C AFA20014 8E220058 8FA40048 0040F809
8FA50028 00408021 8E220058 8FA40068 0040F809 8FA50030 02002021 8FA30014
0060F809 00402821 AFA20028 8E240060 AFA40030 8E220058 03C02021 0040F809
02E02821 AFA20048 8E300058 8FA4004C 0200F809 02602821 8FA40048 8FA80030
0100F809 00402821 8FA4001C 0200F809 00402821 8FA40028 8FA30014 0060F809
00402821 AFA2001C 8E240060 AFA40028 8E220058 03C02021 0040F809 02402821
0040F021 8E300058 8FA40010 0200F809 02802821 03C02021 8FA80028 0100F809
00402821 8FA40020 0200F809 00402821 8FA4001C 8FA30014 0060F809 00402821
0040F021 8E300058 8FA40010 0200F809 02E02821 00402021 0200F809 8FA50018
03C02021 8FA80014 0100F809 00402821 0040F021 8E220074 AFA20010 8E30005C
8E220058 8FA40024 0040F809 00802821 AFA20020 8E220058 8FA4002C 0040F809
00802821 8FA40020 0200F809 00402821 AFA20020 8E220058 8FA40034 0040F809
00802821 8FA40020 0200F809 00402821 AFA20020 8E220058 03C02021 0040F809
03C02821 8FA40020 0200F809 00402821 00402021 8FA30010 0060F809 8FA50038
8E230064 8FA40044 0060F809 00402821 00408021 8E220058 8FA40024 0040F809
02002821 AFA20010 8E220058 8FA4002C 0040F809 02002821 AFA20020 8E220058
8FA40034 0040F809 02002821 AFA20018 8E220058 03C02021 0040F809 02002821
AFA2001C 3C029D00 8C500058 8E3E0060 8FA40040 03C0F809 02E02821 00402021
0200F809 8FA50054 00408021 8E220058 02402021 0040F809 8FA50058 02002021
03C0F809 00402821 AFA20014 8E300058 02602021 0200F809 8FA5005C 8FA40014
03C0F809 00402821 8FA40038 0200F809 00402821 00408021 8E220058 8FA40060
0040F809 8FA50010 02002021 03C0F809 00402821 AFA20010 8E3E0060 8E24005C
AFA40014 8E220058 02802021 0040F809 8FA50054 00408021 8E220058 02402021
0040F809 8FA5005C 02002021 8FA80014 0100F809 00402821 AFA20014 8E300058
02602021 0200F809 8FA50058 8FA40014 03C0F809 00402821 8FA40038 0200F809
00402821 00408021 8E220058 8FA40060 0040F809 8FA50020 02002021 03C0F809
00402821 AFA20020 8E22005C AFA20014 8E3E0060 8E220058 02802021 0040F809
8FA50058 00408021 8E220058 02E02021 0040F809 8FA5005C 02002021 03C0F809
00402821 AFA20028 8E300058 02602021 0200F809 8FA50054 8FA40028 8FA30014
0060F809 00402821 8FA40038 0200F809 00402821 00408021 8E220058 8FA40060
0040F809 8FA50018 02002021 03C0F809 00402821 AFA20018 8E3E0060 8E24005C
AFA40014 8E220058 02802021 0040F809 8FA5005C 00408021 8E220058 02E02021
0040F809 8FA50058 02002021 8FA80014 0100F809 00402821 AFA20014 8E300058
02402021 0200F809 8FA50054 8FA40014 03C0F809 00402821 8FA40038 0200F809
00402821 00408021 8E220058 8FA40060 0040F809 8FA5001C 02002021 03C0F809
00402821 AFA2001C 8E30005C 8E220058 8FA40010 0040F809 8FA50064 02802021
0200F809 00402821 0040F021 8E30005C 8E220058 8FA40020 0040F809 8FA50064
02E02021 0200F809 00402821 0040B821 8E30005C 8E220058 8FA40018 0040F809
8FA50064 02402021 0200F809 00402821 0040A021 8E30005C 8E220058 8FA4001C
0040F809 8FA50064 02602021 0200F809 00402821 00409021 8E220074 AFA20010
8E30005C 8E220058 03C02021 0040F809 03C02821 00409821 8E220058 02E02021
0040F809 02E02821 02602021 0200F809 00402821 00409821 8E220058 02802021
0040F809 02802821 02602021 0200F809 00402821 00409821 8E220058 02402021
0040F809 02402821 02602021 0200F809 00402821 00402021 8FA30010 0060F809
8FA50038 8E230064 8FA40044 0060F809 00402821 00408021 8E220058 03C02021
0040F809 02002821 AEC20000 8E220058 02E02021 0040F809 02002821 AEC20004
8E220058 02802021 0040F809 02002821 AEC20008 8E220058 02402021 0040F809
02002821 AEC2000C 8E33005C 8E220058 8EC40004 0040F809 8EC50008 00409021
8E300058 8EC40000 0200F809 8EC5000C 02402021 0260F809 00402821 02A02021
0200F809 00402821 0040A021 8E300060 8E33005C 8EC50000 8E220058 0040F809
00A02021 00409021 8EC50004 8E220058 0040F809 00A02021 02402021 0260F809
00402821 00409021 8EC50008 8E220058 0040F809 00A02021 02402021 0200F809
00402821 00409821 8E320058 8EC5000C 0240F809 00A02021 02602021 0200F809
00402821 02802021 0411F9A3 00402821 8FA40078 0240F809 00402821 8FA300E8
AC620000 8E320060 8E220058 8EC40004 0040F809 8EC5000C 00409821 8E300058
8EC40000 0200F809 8EC50008 02602021 0240F809 00402821 02A02021 0200F809
00402821 0411FA34 00402021 8FA40040 0240F809 00402821 8FA40078 0200F809
00402821 8FA300EC AC620000 8E33005C 8E220058 8EC40000 0040F809 8EC50004
00409021 8E300058 8EC40008 0200F809 8EC5000C 02402021 0260F809 00402821
02A02021 0200F809 00402821 0040A021 8E33005C 8E300060 8EC50000 8E220058
0040F809 00A02021 00409021 8EC50004 8E220058 0040F809 00A02021 02402021
0200F809 00402821 00409021 8EC50008 8E220058 0040F809 00A02021 02402021
0200F809 00402821 00409021 8E300058 8EC5000C 0200F809 00A02021 02402021
0260F809 00402821 02802021 0411F952 00402821 8FA40078 0200F809 00402821
8FA300F0 AC620000 8FBF00CC 8FBE00C8 8FB700C4 8FB600C0 8FB500BC 8FB400B8
8FB300B4 8FB200B0 8FB100AC 8FB000A8 03E00008 27BD00D0
End CSUB
 
WhiteWizzard
Guru

Joined: 05/04/2013
Location: United Kingdom
Posts: 2931
Posted: 07:47am 14 Mar 2016
Copy link to clipboard 
Print this post

Takes me bl***y ages to keep typing in your 'updated' code

Still awaiting delivery of the MPU6050 (should have arrived today, but it didn't)

Out of interest, do you know the current consumption 'window' of the two modules? After min-max mA's to determine which LDO to use. I am close to 450mA with my complete circuit and want to add these modules. Thanks . . . .

WW

 
matherp
Guru

Joined: 11/12/2012
Location: United Kingdom
Posts: 10180
Posted: 08:03am 14 Mar 2016
Copy link to clipboard 
Print this post

The two together use 6.3mA when active of which 1.5mA seems to be the LED on the MPU6050 module
 
Print this page


To reply to this topic, you need to log in.

The Back Shed's forum code is written, and hosted, in Australia.
© JAQ Software 2025