This is a simple project using the Orientation Sensor MPU9250.
This is a 9-axis device containing 3 accelerometers, 3 gyroscopes and 3 magnetometers.
The driver is written directly in Annex script but it rely on the internal fusion algorithm to compute the 3 orientation angles (pitch, roll and yaw).
The files in attachment contains all the required files for this project.
Even if not drawn on the schematics, the code drives also an OLED display that can simply connected in parallel on the same I2C pins.
Wiring
The output window
The Code
CODE: mpu9250.bas
' MPU6050 Test program
' cicciocb 2018
' the values shown are in 'g' for the accelerometers
' in °/sec for the gyros and in °C for the temperature
' in milliGauss for the Magnetometer
' important : the axis for the magnetometers different from the accelerometers and the gyros
' X magnetometer -> Y gyro + accel
' Y magnetometer -> X gyro + accel
' Z magnetometer -> -Z gyro + accel
' refer to datasheet : http://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
' for the registers :
' refer to datasheet : https://www.invensense.com/wp-content/uploads/2015/02/RM-MPU-9250A-00-v1.6.pdf
MPU6050SlaveAddress = &h68
AccelScaleFactor = 32768 / 4 ' 2G full scale -- sensitivity scale factor respective to full scale setting provided in datasheet
GyroScaleFactor = 32768 / 1000' 500 °/sec full scale
MagnScaleFactor = 32768 / (10 * 4912)' 49120 milliGauss full scale
MPU6050_REG_SMPLRT_DIV = &h19
MPU6050_REG_CONFIG = &h1A
MPU6050_REG_GYRO_CONFIG = &h1B
MPU6050_REG_ACCEL_CONFIG = &h1C
MPU6050_REG_FIFO_EN = &h23
MPU9250_INT_PIN_CFG = &h37
MPU6050_REG_INT_ENABLE = &h38
MPU6050_REG_ACCEL_XOUT_H = &h3B
MPU6050_REG_SIGNAL_PATH_RESET = &h68
MPU6050_REG_USER_CTRL = &h6A
MPU6050_REG_PWR_MGMT_1 = &h6B
MPU6050_REG_PWR_MGMT_2 = &h6C
MPU6050_REG_WHO_AM_I = &h75
' parameters for the MPU6050
dim IMU_data(20)
AccelX = 0 : AccelY = 0 : AccelZ = 0
GyroX = 0: GyroY = 0: GyroZ = 0
Temperature = 0
'paramters for the magnetometer
AK8963_ADDRESS = &h0C
AK8963_WHO_AM_I = &h00
AK8963_CNTL = &h0A 'Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
AK8963_ASAX = &h10 'Fuse ROM x-axis sensitivity adjustment value
dim MAGN_data(20)
dim MAGN_calib(3)
MagnX = 0 : MagnY = 0 : MagnZ = 0
' parameters for 6 DoF sensor fusion calculations
pitch = 0 : yaw = 0 : roll = 0
GyroMeasError = PI * (40.0 / 180.0) ' gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
beta = sqr(3.0 / 4.0) * GyroMeasError ' compute beta
GyroMeasDrift = PI * (0.0 / 180.0) ' gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
zeta = sqr(3.0 / 4.0) * GyroMeasDrift ' compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
deltat = 0.0 ' integration interval for both filter schemes
deltat_p = millis
' the algo Madgwick 6 DOF uses beta and zeta (default 0.6045 , 0)
' the algo Madgwick 9 DOF uses beta (default 0.6045)
' the algo Mahony 9 DOF uses Kp and Ki (default = 10 , 0)
beta = 0.5
counter.setup 1, 12, 2
fusion.init
fusion.beta = beta
fusion.zeta = zeta
onUrlMessage message
'opens the html page
jscall "window.location = 'mpu6050_demo.html'"
i2c.setup 4, 5
'check the presence of the MPU6050
ident = 0
'I2C_ReadDataByte MPU6050_REG_WHO_AM_I, ident
ident = i2c.ReadRegByte(MPU6050SlaveAddress, MPU6050_REG_WHO_AM_I)
if ident <> &h71 then print "MPU9250 not found." : end
MPU6050_Ini
ident = i2c.ReadRegByte(AK8963_ADDRESS, AK8963_WHO_AM_I )
if ident <> &h48 then print "AK8963 not found." : end
AK8963_Ini
for j = 1 to 1000000
i2c.readregarray MPU6050SlaveAddress, MPU6050_REG_ACCEL_XOUT_H, 14, IMU_data() ' read all registers in one shot
'extract all the parameters from the buffer
extract_param AccelX, 0
extract_param AccelY, 2
extract_param AccelZ, 4
extract_param GyroX, 8
extract_param GyroY, 10
extract_param GyroZ, 12
extract_param Temperature, 6
'scale all the parameters in Engineering Unit
AccelX = AccelX/AccelScaleFactor' + 0.03
AccelY = AccelY/AccelScaleFactor' + 0.01
AccelZ = AccelZ/AccelScaleFactor' + 0.08
GyroX = GyroX/GyroScaleFactor' + 0.55
GyroY = GyroY/GyroScaleFactor' + 0.8
GyroZ = GyroZ/GyroScaleFactor '- 0.25
Temperature = Temperature/340 + 36.53
ident = i2c.ReadRegByte( AK8963_ADDRESS, &h02)
if (ident and 1) = 0 then print "mag data missing"
i2c.readregarray AK8963_ADDRESS, &h03, 7, MAGN_data()
extract_AK8963_param MagnX, 0
extract_AK8963_param MagnY, 2
extract_AK8963_param MagnZ, 4
MagnX = MagnX / MagnScaleFactor * MAGN_calib(0)
MagnY = MagnY / MagnScaleFactor * MAGN_calib(1)
MagnZ = MagnZ / MagnScaleFactor * MAGN_calib(2)
deltat = (millis - deltat_p) / 1000
deltat_p = millis
'print deltat
fusion.Madgwick AccelX, AccelY, AccelZ, (GyroX * PI/180), (GyroY * Pi/180), (GyroZ * Pi/180), MagnY, MagnX, -MagnZ
'fusion.Madgwick AccelX, AccelY, AccelZ, (GyroX * Pi/180), (GyroY * Pi/180), -(GyroZ * Pi/180)
'fusion.mahony AccelX, AccelY, AccelZ, (GyroX * Pi/180), (GyroY * Pi/180), (GyroZ * Pi/180), MagnY, MagnX, -MagnZ
pitch = -fusion.angle(1)
roll = fusion.angle(2)
yaw = -fusion.angle(3)
print str$(pitch, "%3.1f"), str$(roll, "%3.1f"), str$(yaw, "%3.1f"), deltat
next j
end
message:
UrlMsgReturn str$(pitch) + "," + str$(roll) + "," + str$(yaw) + ",1"
return
'------------------------------------------------------------
'extract a parameter from the buffer at the given address
'correct the sign of the value using the Most significant bit
sub extract_param(ret, address)
ret = (IMU_data(address) <<8) + IMU_data(address+1)
if ret > 32768 then ret = ret - 65536
end sub
'configuration of the MPU6050
sub MPU6050_Ini
pause 150
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_PWR_MGMT_1, &h01 'PLL with X axis gyroscope reference
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_PWR_MGMT_2, &h00 ' no sensors standby
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_SMPLRT_DIV, 9 ' sample rate = 8Khz / (1 + 7) -> 1KHz
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_CONFIG, &h01 ' bandwith 5Hz (delay 19msec) Fs 1Kz
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_GYRO_CONFIG, &h10 'set +/-1000 degree/second full scale
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_ACCEL_CONFIG, &h01 ' set +/- 4g full scale
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_FIFO_EN, &h00 ' fifo disabled
'enables the Data Ready interrupt, which occurs each
'time a write operation to all of the sensor registers has been completed.
'put a led on the pin INT to see the sample rate
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_INT_ENABLE, &h01
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_SIGNAL_PATH_RESET, &h07 ' reset sensors
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_USER_CTRL, &h00 ' do not reset sensor registers (put 1 if required)
i2c.WriteRegByte MPU6050SlaveAddress, MPU9250_INT_PIN_CFG, &h02 ' enable bypass mode
end sub
sub AK8963_Ini
'First extract the factory calibration for each magnetometer axis
i2c.WriteRegByte AK8963_ADDRESS, AK8963_CNTL, &h00 'Power down magnetometer
pause 10
i2c.WriteRegByte AK8963_ADDRESS, AK8963_CNTL, &h0f 'Enter Fuse ROM access mode
pause 10
'I2C_ReadAK8963DataByteMulti AK8963_ASAX, 3 'Read the x-, y-, and z-axis calibration values
i2c.readregarray AK8963_ADDRESS, AK8963_ASAX, 3, MAGN_data()
MAGN_calib(0) = (MAGN_data(0) - 128) / 256 + 1 'Return x-axis sensitivity adjustment values, etc.
MAGN_calib(1) = (MAGN_data(1) - 128) / 256 + 1
MAGN_calib(2) = (MAGN_data(2) - 128) / 256 + 1
i2c.WriteRegByte AK8963_ADDRESS, AK8963_CNTL, &h00 'Power down magnetometer
pause 10
'Configure the magnetometer for continuous read and highest resolution
'set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
'and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
i2c.WriteRegByte AK8963_ADDRESS, &h0a, &h16 ' Set magnetometer data resolution and sample ODR
end sub
'extract a parameter from the buffer at the given address
'correct the sign of the value using the Most significant bit
sub extract_AK8963_param(ret, address)
ret = (MAGN_data(address+1) <<8) + MAGN_data(address)
if ret > 32768 then ret = ret - 65536
end sub
' cicciocb 2018
' the values shown are in 'g' for the accelerometers
' in °/sec for the gyros and in °C for the temperature
' in milliGauss for the Magnetometer
' important : the axis for the magnetometers different from the accelerometers and the gyros
' X magnetometer -> Y gyro + accel
' Y magnetometer -> X gyro + accel
' Z magnetometer -> -Z gyro + accel
' refer to datasheet : http://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
' for the registers :
' refer to datasheet : https://www.invensense.com/wp-content/uploads/2015/02/RM-MPU-9250A-00-v1.6.pdf
MPU6050SlaveAddress = &h68
AccelScaleFactor = 32768 / 4 ' 2G full scale -- sensitivity scale factor respective to full scale setting provided in datasheet
GyroScaleFactor = 32768 / 1000' 500 °/sec full scale
MagnScaleFactor = 32768 / (10 * 4912)' 49120 milliGauss full scale
MPU6050_REG_SMPLRT_DIV = &h19
MPU6050_REG_CONFIG = &h1A
MPU6050_REG_GYRO_CONFIG = &h1B
MPU6050_REG_ACCEL_CONFIG = &h1C
MPU6050_REG_FIFO_EN = &h23
MPU9250_INT_PIN_CFG = &h37
MPU6050_REG_INT_ENABLE = &h38
MPU6050_REG_ACCEL_XOUT_H = &h3B
MPU6050_REG_SIGNAL_PATH_RESET = &h68
MPU6050_REG_USER_CTRL = &h6A
MPU6050_REG_PWR_MGMT_1 = &h6B
MPU6050_REG_PWR_MGMT_2 = &h6C
MPU6050_REG_WHO_AM_I = &h75
' parameters for the MPU6050
dim IMU_data(20)
AccelX = 0 : AccelY = 0 : AccelZ = 0
GyroX = 0: GyroY = 0: GyroZ = 0
Temperature = 0
'paramters for the magnetometer
AK8963_ADDRESS = &h0C
AK8963_WHO_AM_I = &h00
AK8963_CNTL = &h0A 'Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
AK8963_ASAX = &h10 'Fuse ROM x-axis sensitivity adjustment value
dim MAGN_data(20)
dim MAGN_calib(3)
MagnX = 0 : MagnY = 0 : MagnZ = 0
' parameters for 6 DoF sensor fusion calculations
pitch = 0 : yaw = 0 : roll = 0
GyroMeasError = PI * (40.0 / 180.0) ' gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
beta = sqr(3.0 / 4.0) * GyroMeasError ' compute beta
GyroMeasDrift = PI * (0.0 / 180.0) ' gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
zeta = sqr(3.0 / 4.0) * GyroMeasDrift ' compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
deltat = 0.0 ' integration interval for both filter schemes
deltat_p = millis
' the algo Madgwick 6 DOF uses beta and zeta (default 0.6045 , 0)
' the algo Madgwick 9 DOF uses beta (default 0.6045)
' the algo Mahony 9 DOF uses Kp and Ki (default = 10 , 0)
beta = 0.5
counter.setup 1, 12, 2
fusion.init
fusion.beta = beta
fusion.zeta = zeta
onUrlMessage message
'opens the html page
jscall "window.location = 'mpu6050_demo.html'"
i2c.setup 4, 5
'check the presence of the MPU6050
ident = 0
'I2C_ReadDataByte MPU6050_REG_WHO_AM_I, ident
ident = i2c.ReadRegByte(MPU6050SlaveAddress, MPU6050_REG_WHO_AM_I)
if ident <> &h71 then print "MPU9250 not found." : end
MPU6050_Ini
ident = i2c.ReadRegByte(AK8963_ADDRESS, AK8963_WHO_AM_I )
if ident <> &h48 then print "AK8963 not found." : end
AK8963_Ini
for j = 1 to 1000000
i2c.readregarray MPU6050SlaveAddress, MPU6050_REG_ACCEL_XOUT_H, 14, IMU_data() ' read all registers in one shot
'extract all the parameters from the buffer
extract_param AccelX, 0
extract_param AccelY, 2
extract_param AccelZ, 4
extract_param GyroX, 8
extract_param GyroY, 10
extract_param GyroZ, 12
extract_param Temperature, 6
'scale all the parameters in Engineering Unit
AccelX = AccelX/AccelScaleFactor' + 0.03
AccelY = AccelY/AccelScaleFactor' + 0.01
AccelZ = AccelZ/AccelScaleFactor' + 0.08
GyroX = GyroX/GyroScaleFactor' + 0.55
GyroY = GyroY/GyroScaleFactor' + 0.8
GyroZ = GyroZ/GyroScaleFactor '- 0.25
Temperature = Temperature/340 + 36.53
ident = i2c.ReadRegByte( AK8963_ADDRESS, &h02)
if (ident and 1) = 0 then print "mag data missing"
i2c.readregarray AK8963_ADDRESS, &h03, 7, MAGN_data()
extract_AK8963_param MagnX, 0
extract_AK8963_param MagnY, 2
extract_AK8963_param MagnZ, 4
MagnX = MagnX / MagnScaleFactor * MAGN_calib(0)
MagnY = MagnY / MagnScaleFactor * MAGN_calib(1)
MagnZ = MagnZ / MagnScaleFactor * MAGN_calib(2)
deltat = (millis - deltat_p) / 1000
deltat_p = millis
'print deltat
fusion.Madgwick AccelX, AccelY, AccelZ, (GyroX * PI/180), (GyroY * Pi/180), (GyroZ * Pi/180), MagnY, MagnX, -MagnZ
'fusion.Madgwick AccelX, AccelY, AccelZ, (GyroX * Pi/180), (GyroY * Pi/180), -(GyroZ * Pi/180)
'fusion.mahony AccelX, AccelY, AccelZ, (GyroX * Pi/180), (GyroY * Pi/180), (GyroZ * Pi/180), MagnY, MagnX, -MagnZ
pitch = -fusion.angle(1)
roll = fusion.angle(2)
yaw = -fusion.angle(3)
print str$(pitch, "%3.1f"), str$(roll, "%3.1f"), str$(yaw, "%3.1f"), deltat
next j
end
message:
UrlMsgReturn str$(pitch) + "," + str$(roll) + "," + str$(yaw) + ",1"
return
'------------------------------------------------------------
'extract a parameter from the buffer at the given address
'correct the sign of the value using the Most significant bit
sub extract_param(ret, address)
ret = (IMU_data(address) <<8) + IMU_data(address+1)
if ret > 32768 then ret = ret - 65536
end sub
'configuration of the MPU6050
sub MPU6050_Ini
pause 150
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_PWR_MGMT_1, &h01 'PLL with X axis gyroscope reference
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_PWR_MGMT_2, &h00 ' no sensors standby
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_SMPLRT_DIV, 9 ' sample rate = 8Khz / (1 + 7) -> 1KHz
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_CONFIG, &h01 ' bandwith 5Hz (delay 19msec) Fs 1Kz
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_GYRO_CONFIG, &h10 'set +/-1000 degree/second full scale
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_ACCEL_CONFIG, &h01 ' set +/- 4g full scale
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_FIFO_EN, &h00 ' fifo disabled
'enables the Data Ready interrupt, which occurs each
'time a write operation to all of the sensor registers has been completed.
'put a led on the pin INT to see the sample rate
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_INT_ENABLE, &h01
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_SIGNAL_PATH_RESET, &h07 ' reset sensors
i2c.WriteRegByte MPU6050SlaveAddress, MPU6050_REG_USER_CTRL, &h00 ' do not reset sensor registers (put 1 if required)
i2c.WriteRegByte MPU6050SlaveAddress, MPU9250_INT_PIN_CFG, &h02 ' enable bypass mode
end sub
sub AK8963_Ini
'First extract the factory calibration for each magnetometer axis
i2c.WriteRegByte AK8963_ADDRESS, AK8963_CNTL, &h00 'Power down magnetometer
pause 10
i2c.WriteRegByte AK8963_ADDRESS, AK8963_CNTL, &h0f 'Enter Fuse ROM access mode
pause 10
'I2C_ReadAK8963DataByteMulti AK8963_ASAX, 3 'Read the x-, y-, and z-axis calibration values
i2c.readregarray AK8963_ADDRESS, AK8963_ASAX, 3, MAGN_data()
MAGN_calib(0) = (MAGN_data(0) - 128) / 256 + 1 'Return x-axis sensitivity adjustment values, etc.
MAGN_calib(1) = (MAGN_data(1) - 128) / 256 + 1
MAGN_calib(2) = (MAGN_data(2) - 128) / 256 + 1
i2c.WriteRegByte AK8963_ADDRESS, AK8963_CNTL, &h00 'Power down magnetometer
pause 10
'Configure the magnetometer for continuous read and highest resolution
'set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
'and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
i2c.WriteRegByte AK8963_ADDRESS, &h0a, &h16 ' Set magnetometer data resolution and sample ODR
end sub
'extract a parameter from the buffer at the given address
'correct the sign of the value using the Most significant bit
sub extract_AK8963_param(ret, address)
ret = (MAGN_data(address+1) <<8) + MAGN_data(address)
if ret > 32768 then ret = ret - 65536
end sub
