' Calibration parameters for gyro and accelerometer
dim float gyro_Bias(2)=(40,15,100), accel_Bias(2)=(340,55,-390)
'
' calibration parameters for magnetometer derived from running AK8963calibrate
DIM float magbias(2)=(17.2,-23,28), magscale(2)=(0.905,0.92,0.90)

' Uncomment this section to run the self test algorithm of the accelerometer and gyro
' Ensure the module is on a level surface and is completely still
' MPU9250SelfTest(selftest())
' print "Errors in %"
' print " Gyro Accel"
' Print "X ",str$(selftest(3),3,3," "),STR$(selftest(0),4,3," ")
' Print "Y ",str$(selftest(4),3,3," "),STR$(selftest(1),4,3," ")
' Print "Z ",str$(selftest(5),3,3," "),STR$(selftest(2),4,3," ")
' end

deltat=timer/1000:timer=0
readMPU9250(60)
MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz, my, mx, -mz)
yaw = DEG(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 = DEG(-asin(2.0 * (q(1) * q(3) - q(0) * q(2))))
roll = DEG(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)))

'Magnetometer Registers
const AK8963_ADDRESS =&H0C
const WHO_AM_I_AK8963 =&H00 ' should return =&H48
const INFO =&H01
const AK8963_ST1 =&H02 ' data ready status bit 0
const AK8963_XOUT_L =&H03 ' data
const AK8963_XOUT_H =&H04
const AK8963_YOUT_L =&H05
const AK8963_YOUT_H =&H06
const AK8963_ZOUT_L =&H07
const AK8963_ZOUT_H =&H08
const AK8963_ST2 =&H09 ' Data overflow bit 3 and data read error status bit 2
const AK8963_CNTL =&H0A ' Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
const AK8963_ASTC =&H0C ' Self test control
const AK8963_I2CDIS =&H0F ' I2C disable
const AK8963_ASAX =&H10 ' Fuse ROM x-axis sensitivity adjustment value
const AK8963_ASAY =&H11 ' Fuse ROM y-axis sensitivity adjustment value
const AK8963_ASAZ =&H12 ' Fuse ROM z-axis sensitivity adjustment value
const MFS_14BITS = 0' 0.6 uT per LSB
const MFS_16BITS =1' 0.15 uT per LSB
const AK8963_Gauss_Scale_X = 1.0
const AK8963_Gauss_Scale_Y = 1.0
const AK8963_Gauss_Scale_Z = 1.0

DIM INTEGER Mmode = &H06,temp%
DIM INTEGER Mscale = MFS_16BITS ' Choose either 14-bit or 16-bit magnetometer resolution
DIM FLOAT MagCalibration(2),mmult(2),mRes,magcals(5)
'
dim float maxx=0, minx=0, maxy=0, miny=0, maxz=0, minz=0, fields(2), x(1000),y(1000),z(1000)
dim float magbias(2)=(0,0,0)
dim integer iterations=1000
dim maxfield ' maximum uT reading for this location
i2c open 400,1000
temp%=readByte(AK8963_ADDRESS,WHO_AM_I_AK8963)
if temp%<>&H48 then
Print "AK8963 not found"
end
endif
initAK8963(MagCalibration())
mmult(0)=mRes*magCalibration(0)
mmult(1)=mRes*magCalibration(1)
mmult(2)=mRes*magCalibration(2)
print "Rotate the sensor around all axis whilst the program is sampling"
input "local field strength in uT ? ",maxfield
for iterations=0 to 999
readMagData(fields())
x(iterations)=fields(0)
y(iterations)=fields(1)
z(iterations)=fields(2)
if (iterations mod 100)=0 then print iterations/10,"% ";
next iterations
print "100%"

floatsort(x(),1000)
floatsort(y(),1000)
floatsort(z(),1000)
print "x-min = ",x(10)," x-max = ",x(990)
print "y-min = ",y(10)," y-max = ",y(990)
print "z-min = ",z(10)," z-max = ",z(990)
magcals(3) = AK8963_Gauss_Scale_X * ((abs(x(10))+abs(x(990)))/2)/maxfield 'scale at 99th percentiles
magcals(4) = AK8963_Gauss_Scale_Y * ((abs(y(10))+abs(y(990)))/2)/maxfield
magcals(5) = AK8963_Gauss_Scale_Z * ((abs(z(10))+abs(z(990)))/2)/maxfield
magcals(0)=((x(990)-x(10))/2-x(990))*AK8963_Gauss_Scale_X/magcals(3)
magcals(1)=((y(990)-y(10))/2-y(990))*AK8963_Gauss_Scale_Y/magcals(4)
magcals(2)=((z(990)-z(10))/2-z(990))*AK8963_Gauss_Scale_Z/magcals(5)
print " magbias magscale"
Print "X ",str$(magcals(0),3,3," ")," ",STR$(magcals(3),4,3," ")
Print "Y ",str$(magcals(1),3,3," ")," ",STR$(magcals(4),4,3," ")
Print "Z ",str$(magcals(2),3,3," ")," ",STR$(magcals(5),4,3," ")
end
'
sub getMres
select case Mscale
' Possible magnetometer scales (and their register bit settings) are:
' 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS
mRes = 0.60. ' Proper scale to return uT
case MFS_16BITS
mRes = 0.15 ' Proper scale to return uT
end select
end sub
'
sub initAK8963(destination() as float)
getMres
' First extract the factory calibration for each magnetometer axis
local integer rawData(2); ' x/y/z gyro calibration data stored here
writeByte(AK8963_ADDRESS, AK8963_CNTL, &H00); ' Power down magnetometer
Pause 10
writeByte(AK8963_ADDRESS, AK8963_CNTL, &H0F); ' Enter Fuse ROM access mode
readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, rawData()); ' Read the x-, y-, and z-axis calibration values
destination(0) = (rawData(0) - 128)/256 + 1 ' Return x-axis sensitivity adjustment values, etc.
destination(1) = (rawData(1) - 128)/256 + 1
destination(2) = (rawData(2) - 128)/256 + 1
writeByte(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
writeByte(AK8963_ADDRESS, AK8963_CNTL, (Mscale << 4) OR Mmode) 'Set magnetometer data resolution and sample ODR
Pause 10
end sub

sub readMagData(destination() as float)
local integer rawData(6),c,i ' x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
if readByte(AK8963_ADDRESS, AK8963_ST1) and 1 then ' wait for magnetometer data ready bit to be set
readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, rawData())' Read the six raw data and ST2 registers sequentially into data array
c = rawData(6)' End data read by reading ST2 register
if((c AND &H08)=0) THEN ' Check if magnetic sensor overflow set, if not then report data
destination(0) = sint16((rawData(1) << 8) OR rawData(0))*mmult(0) ' Turn the MSB and LSB into a signed 16-bit value
destination(1) = sint16((rawData(3) << 8) OR rawData(2))*mmult(1) ' Data stored as little Endian
destination(2) = sint16((rawData(5) << 8) OR rawData(4))*mmult(2)
text 0,26,"Mag X="+str$(destination(0),5,0," ")+" Y="+str$(destination(1),5,0," ")+" Z="+str$(destination(2),5,0," ")
endif
endif
pause 30
end sub

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

sub writeByte(address%,reg%,value%)
i2c write address%,0,2,reg%,value%
end sub
'
function readByte(address%,reg%) as integer
local r%
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

CSUB floatsort
00000000
27BDFFA8 AFBF0054 AFBE0050 AFB7004C AFB60048 AFB50044 AFB40040 AFB3003C
AFB20038 AFB10034 AFB00030 AFA40058 8CA50000 AFA50018 00A09021 10000034
3C159D00 8FA20020 8C5E0000 8FA3001C 0072102A 14400019 0060B821 8FB30020
8FB10028 10000009 8FB00024 AE620000 8FA40014 02048021 02368821 02901021
0052102A 1440000C 02769821 0214B821 AFB00010 8EA20068 03C02021 0040F809
8E250000 2403FFFF 5043FFF0 8E220000 10000003 0017B880 8FB70010 0017B880
8FA40058 0097B821 AEFE0000 8FA2001C 24420001 AFA2001C 8FA30024 24630001
AFA30024 8FA40020 24840004 AFA40020 8FA20028 24420004 AFA20028 8FA30018
8FA4001C 0083102A 1440FFCF 8FA20020 001217C2 00529021 00129043 12400010
8FA30018 0243102A 1040FFFA 001217C2 0240A021 00121080 8FA40058 00821021
AFA20020 AFA40028 AFB2001C AFA00024 00121023 AFA20014 1000FFBA 0002B080
00001021 00001821 8FBF0054 8FBE0050 8FB7004C 8FB60048 8FB50044 8FB40040
8FB3003C 8FB20038 8FB10034 8FB00030 03E00008 27BD0058
End CSUB

option explicit
option default none
'
' Micromite AHRS using MPU9250
' Original source code from kriswiner (https://github.com/kriswiner/MPU-9250)
' and Sebastion Madgwick's Open source IMU and AHRS algorithms
'
' Use TFT display in landscape mode to display position
'
' MPU9250 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 screenIO=1 'set to 1 to do fast output on TFT screen, otherwise 0

'const MPU9250_ADDRESS =&H69 ' Device address when ADO = 1
const MPU9250_ADDRESS =&H68 ' Device address when ADO = 0
'
'Magnetometer Registers
const AK8963_ADDRESS =&H0C
const WHO_AM_I_AK8963 =&H00 ' should return =&H48
const AK8963_ST1 =&H02 ' data ready status bit 0
const AK8963_XOUT_L =&H03 ' data
const AK8963_CNTL =&H0A ' Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
const AK8963_ASAX =&H10 ' Fuse ROM x-axis sensitivity adjustment value
' Gyro/accelerometer registers
const SELF_TEST_X_GYRO =&H00
const SELF_TEST_Y_GYRO =&H01
const SELF_TEST_Z_GYRO =&H02
const SELF_TEST_X_ACCEL =&H0D
const SELF_TEST_Y_ACCEL =&H0E
const SELF_TEST_Z_ACCEL =&H0F
const XG_OFFSET_H =&H13 ' User-defined trim values for gyroscope
const XG_OFFSET_L =&H14
const YG_OFFSET_H =&H15
const YG_OFFSET_L =&H16
const ZG_OFFSET_H =&H17
const ZG_OFFSET_L =&H18
const SMPLRT_DIV =&H19
const CONFIG =&H1A
const GYRO_CONFIG =&H1B
const ACCEL_CONFIG =&H1C
const ACCEL_CONFIG2 =&H1D
const FIFO_EN =&H23
const I2C_MST_CTRL =&H24
const INT_PIN_CFG =&H37
const INT_ENABLE =&H38
const ACCEL_XOUT_H =&H3B
const TEMP_OUT_H =&H41
const GYRO_XOUT_H =&H43
const USER_CTRL =&H6A ' Bit 7 enable DMP, bit 3 reset DMP
const PWR_MGMT_1 =&H6B ' Device defaults to the SLEEP mode
const PWR_MGMT_2 =&H6C
const FIFO_COUNTH =&H72
const FIFO_R_W =&H74
const WHO_AM_I_MPU9250 =&H75 ' Should return =&H71
const XA_OFFSET_H =&H77
const XA_OFFSET_L =&H78
const YA_OFFSET_H =&H7A
const YA_OFFSET_L =&H7B
const ZA_OFFSET_H =&H7D
const ZA_OFFSET_L =&H7E
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 MFS_14BITS = 0' 0.6 uT per LSB
const MFS_16BITS =1' 0.15 uT0.6 per LSB
const outputFreq=20.0 ' sample frequency in Hz
const declination=-0.63 ' local magnetic declination in degrees
'
' Global Variable definitions
'
DIM INTEGER Mmode = &H01 ' 1 for single shot, 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
DIM INTEGER Gscale = GFS_250DPS
DIM INTEGER Ascale = AFS_4G
DIM INTEGER Mscale = MFS_16BITS ' Choose either 14-bit or 16-bit magnetometer resolution
DIM float magcount(2)
DIM FLOAT MagCalibration(2),accelCount(2), gyrocount(2), gRes, mRes, aRes
dim float ax, ay, az, gx, gy, gz, mx, my, mz, SelfTest(5), mmult(2)
dim float pitch,yaw,roll,heading
dim float beta = 0.3 'derived from testing
dim integer gdata(13),temp%
DIM FLOAT deltat = 1/outputFreq
dim float q(3) = (0,0,0,0)' quaternion of sensor frame relative to auxiliary frame
q(0)=sqr(2)/2 'set intial quaternion to north
q(1)=sqr(2)/2
'
' Calibration parameters for gyro and accelerometer
dim float gyro_Bias(2)=(40,15,100), accel_Bias(2)=(340,55,-390)
'
' calibration parameters for magnetometer derived from running AK8963calibrate
DIM float magbias(2)=(17.2,-23,28), magscale(2)=(0.905,0.92,0.90)
'
' Setup
'
i2c open 400,1000
setpin intPin,DIN
pause 100
writeByte(MPU9250_ADDRESS,PWR_MGMT_1,&H80)
pause 500
initMPU9250
temp%=readByte(MPU9250_ADDRESS,WHO_AM_I_MPU9250)
if temp%<>&H71 then
Print "MPU9250 not found"
end
endif
temp%=readByte(AK8963_ADDRESS,WHO_AM_I_AK8963)
'if temp%<>&H48 then
' Print "AK8963 not found"
' end
'endif
print "Internal temperature is ",readTempData()
if screenIO then cls
initAK8963(MagCalibration())
getAres
getGres
getMres
if screenIO then readMPU9250(0)
' Uncomment this section to run the self test algorithm of the accelerometer and gyro
' Ensure the module is on a level surface and is completely still
'MPU9250SelfTest(selftest())
'print "Errors in %"
'print " Gyro Accel"
'Print "X ",str$(selftest(3),3,3," "),STR$(selftest(0),4,3," ")
'Print "Y ",str$(selftest(4),3,3," "),STR$(selftest(1),4,3," ")
'Print "Z ",str$(selftest(5),3,3," "),STR$(selftest(2),4,3," ")
'end
'
' Uncomment this section to derive calibration parameters for gyro and accelerometer
' Ensure the module is on a level surface and is completely still
' note the parameters and set them to load into the arrays gyroBias and accelBias above
' initMPU9250
' calibrateMPU9250(gyroBias(), accelBias())
' print " Gyro Accel"
' Print "X ",str$(gyroBias(0),3,3," "),STR$(accelbias(0),4,3," ")
' Print "Y ",str$(gyroBias(1),3,3," "),STR$(accelbias(1),4,3," ")
' Print "Z ",str$(gyroBias(2),3,3," "),STR$(accelbias(2),4,3," ")
' end
'

initMPU9250
loadcalibrations
'Print "AK8963 mag calibration X=",MagCalibration(0)," Y=",MagCalibration(1)," Z=",MagCalibration(2)
mmult(0)=mRes*magCalibration(0)/magscale(0)
mmult(1)=mRes*magCalibration(1)/magscale(1)
mmult(2)=mRes*magCalibration(2)/magscale(2)
temp%=0
timer=0
'
' Main program
'
do
deltat=timer/1000:timer=0
readMPU9250(60)
MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz, my, mx, -mz)
yaw = DEG(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 = DEG(-asin(2.0 * (q(1) * q(3) - q(0) * q(2))))
roll = DEG(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)))
heading = ((720.0-yaw)- declination)
if heading>360.0 then heading = heading-360.0
if (temp% mod outputFreq) = 0 then
Print "Heading = "+str$(heading,3,1," ")
endif
temp%=temp%+1
if screenIO then text 0,99,"pitch="+str$(pitch,3,2," ")+" roll="+str$(roll,3,2," ")+" yaw="+str$(yaw,3,2," ")
loop
end
'
sub readMPU9250(offset%)
do while not(pin(intPin)): loop
readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, gData()) 'read in the gyro and accelerometer data
readMagData(magCount())' Read the x/y/z adc values
convertAccelData(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'
if screenIO then text 0,0+offset%,"Acc X="+str$(ax,3,2," ")+" Y="+str$(ay,3,2," ")+" Z="+str$(az,3,2," ")
convertGyroData(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)'
if screenIO then text 0,13+offset%,"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) + magbias(0)' get actual magnetometer value, this depends on scale being set
my = magCount(1) + magbias(1)
mz = magCount(2) + magbias(2)
if screenIO then text 0,26+offset%,"Mag X="+str$(mx,3,2," ")+" Y="+str$(my,3,2," ")+" Z="+str$(mz,3,2," ")
end sub
'
sub initMPU9250
LOCAL INTEGER c
' wake up device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, &H80) ' Reset
pause 100 ' Wait for all registers to reset
writeByte(MPU9250_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(MPU9250_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 = 011 this limits the sample rate to 1000 Hz for both
' With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
writeByte(MPU9250_ADDRESS, CONFIG, &H03)
' Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
writeByte(MPU9250_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(MPU9250_ADDRESS, GYRO_CONFIG)
c=c AND &B11100100
c = c OR (Gscale << 3) ' Set full scale range for the gyro
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ) ' Write new GYRO_CONFIG value to register
' Set accelerometer full-scale range configuration
c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG)
c=c AND &B11100111
c = c OR (Ascale << 3) ' Set full scale range for the accelerometer
writeByte(MPU9250_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
c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2)
c = c AND &B11110000
c = c OR &H03 ' Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c) ' Write new ACCEL_CONFIG2 register value
' 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(MPU9250_ADDRESS, INT_PIN_CFG, &H22)
writeByte(MPU9250_ADDRESS, INT_ENABLE, &H01) ' Enable data ready (bit 0) interrupt
writeByte(MPU9250_ADDRESS, USER_CTRL, &H05)
pause 100
end sub

sub initAK8963(destination() as float)
' First extract the factory calibration for each magnetometer axis
local integer rawData(2); ' x/y/z gyro calibration data stored here
writeByte(AK8963_ADDRESS, AK8963_CNTL, &H00); ' Power down magnetometer
Pause 10
writeByte(AK8963_ADDRESS, AK8963_CNTL, &H0F); ' Enter Fuse ROM access mode
readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, rawData()); ' Read the x-, y-, and z-axis calibration values
destination(0) = (rawData(0) - 128)/256 + 1 ' Return x-axis sensitivity adjustment values, etc.
destination(1) = (rawData(1) - 128)/256 + 1
destination(2) = (rawData(2) - 128)/256 + 1
writeByte(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
writeByte(AK8963_ADDRESS, AK8963_CNTL, (Mscale << 4) OR Mmode) 'Set magnetometer data resolution and sample ODR
Pause 10
end sub

sub writeByte(address%,reg%,value%)
i2c write address%,0,2,reg%,value%
end sub
'
function readByte(address%,reg%) as integer
local r%
i2c write address%,1,1,reg%
i2c read address%,0,1,readByte
end function
'
sub readBytes(address%,reg%,n%,a%()) as integer
local r%
i2c write address%,1,1,reg%
i2c read address%,0,n%,a%()
end sub
'
sub getMres
select case Mscale
' Possible magnetometer scales (and their register bit settings) are:
' 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS
mRes = 0.6 ' Proper scale to return milliGauss
case MFS_16BITS
mRes = 0.15 ' Proper scale to return milliGauss
end select
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,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 convertGyroData(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(6),c' x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
do while NOT(readByte(AK8963_ADDRESS, AK8963_ST1) and 1)
loop ' wait for magnetometer data ready bit to be set
readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, rawData())' Read the six raw data and ST2 registers sequentially into data array
writeByte(AK8963_ADDRESS, AK8963_CNTL, (Mscale << 4) OR Mmode) 'Set magnetometer data resolution and sample ODR
c = rawData(6)' End data read by reading ST2 register
if((c AND &H08)=0) THEN ' Check if magnetic sensor overflow set, if not then report data
destination(0) = sint16((rawData(1) << 8) OR rawData(0))*mmult(0) ' Turn the MSB and LSB into a signed 16-bit value
destination(1) = sint16((rawData(3) << 8) OR rawData(2))*mmult(1) ' Data stored as little Endian
destination(2) = sint16((rawData(5) << 8) OR rawData(4))*mmult(2)
endif
end sub
'
function readTempData() as float
local integer rawData(1) ' x/y/z gyro register data stored here
readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, rawData()) ' Read the two raw data registers sequentially into data array
readTempData=sint16((rawData(0) << 8) OR rawData(1)) / 333.87 + 21.0 '
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 iint16(x as integer) as integer ' convert to signed 16 bit number
local a%=x
IF a% AND &H8000 then a%=a% OR &HFFFFFFFFFFFF0000
iint16 = a%
END FUNCTION
'
FUNCTION iint15(x as integer) as integer ' convert to signed 16 bit number
local a%=x
IF a% AND &H4000 then a%=a% OR &HFFFFFFFFFFFF8000
iint15 = a%
END FUNCTION
'
sub MPU9250SelfTest(destination() as float) ' Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
local integer rawData(5) = (0, 0, 0, 0, 0, 0)
local integer selfTest(5)
local integer gAvg(2)=(0,0,0), aAvg(2)=(0,0,0), aSTAvg(2)=(0,0,0), gSTAvg(2)=(0,0,0)
local float factoryTrim(5)
local integer FS = 0,ii
writeByte(MPU9250_ADDRESS, SMPLRT_DIV, &H00) ' Set gyro sample rate to 1 kHz
writeByte(MPU9250_ADDRESS, CONFIG, &H02) ' Set gyro sample rate to 1 kHz and DLPF to 92 Hz
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, (1<<FS)) ' Set full scale range for the gyro to 250 dps
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, &H02) ' Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, (1<<FS)) ' Set full scale range for the accelerometer to 2 g
'
for ii = 0 to 199 ' get average current values of gyro and acclerometer
readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, rawData()) ' Read the six raw data registers into data array
aAvg(0) = aAvg(0) + iint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
aAvg(1) = aAvg(1) + iint16((rawData(2) << 8) OR rawData(3))
aAvg(2) = aAvg(2) + iint16((rawData(4) << 8) OR rawData(5))
readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, rawData()) ' Read the six raw data registers sequentially into data array
gAvg(0) = gAvg(0) + iint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
gAvg(1) = gAvg(1) + iint16((rawData(2) << 8) OR rawData(3))
gAvg(2) = gAvg(2) + iint16((rawData(4) << 8) OR rawData(5))
next ii
'
for ii =0 to 2 ' Get average of 200 values and store as average current readings
aAvg(ii) = aAvg(ii)/ 200
gAvg(ii) = gAvg(ii)/ 200
next ii
' Configure the accelerometer for self-test
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, &HE0) ' Enable self test on all three axes and set accelerometer range to +/- 2 g
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, &HE0) ' Enable self test on all three axes and set gyro range to +/- 250 degrees/s
pause 25 ' Delay a while to let the device stabilize
for ii = 0 to 199 ' get average self-test values of gyro and acclerometer
readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, rawData()) ' Read the six raw data registers into data array
aSTAvg(0) = aSTAvg(0) + iint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
aSTAvg(1) = aSTAvg(1) + iint16((rawData(2) << 8) OR rawData(3))
aSTAvg(2) = aSTAvg(2) + iint16((rawData(4) << 8) OR rawData(5))
readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, rawData()) ' Read the six raw data registers sequentially into data array
gSTAvg(0) = gSTAvg(0) + iint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
gSTAvg(1) = gSTAvg(1) + iint16((rawData(2) << 8) OR rawData(3))
gSTAvg(2) = gSTAvg(2) + iint16((rawData(4) << 8) OR rawData(5))
next ii
'
for ii =0 to 2 ' Get average of 200 values and store as average current readings
aSTAvg(ii) =aSTAvg(ii)/200
gSTAvg(ii) =gSTAvg(ii)/200
next ii
'
' Configure the gyro and accelerometer for normal operation
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, &H00)
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, &H00)
pause 25 ' Delay a while to let the device stabilize
'
' Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
selfTest(0) = (readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL)) ' X-axis accel self-test results
selfTest(1) = (readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL)) ' Y-axis accel self-test results
selfTest(2) = (readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL)) ' Z-axis accel self-test results
selfTest(3) = (readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO) ) ' X-axis gyro self-test results
selfTest(4) = (readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO) ) ' Y-axis gyro self-test results
selfTest(5) = (readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO) ) ' Z-axis gyro self-test results
'
' Retrieve factory self-test value from self-test code reads
factoryTrim(0) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(0) - 1.0) )) ' FT(Xa) factory trim calculation
factoryTrim(1) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(1) - 1.0) )) ' FT(Ya) factory trim calculation
factoryTrim(2) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(2) - 1.0) )) ' FT(Za) factory trim calculation
factoryTrim(3) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(3) - 1.0) )) ' FT(Xg) factory trim calculation
factoryTrim(4) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(4) - 1.0) )) ' FT(Yg) factory trim calculation
factoryTrim(5) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(5) - 1.0) )) ' FT(Zg) factory trim calculation
'
' Report results as a ratio of (STR - FT)/FT the change from Factory Trim of the Self-Test Response
' To get percent, must multiply by 100
for ii=0 to 2
destination(ii) = (aAvg(ii)-aSTAvg(ii)+factoryTrim(ii)) / factoryTrim(ii)*100 ' Report percent differences
destination(ii+3) = (gAvg(ii)-gSTAvg(ii)+factoryTrim(ii+3)) / factoryTrim(ii+3)*100 ' Report percent differences
next ii
end sub
'
sub calibrateMPU9250(dest1() as float, dest2() as float)
LOCAL INTEGER rData(11) ' data array to hold accelerometer and gyro x, y, z, data
LOCAL INTEGER ii, jj, packet_count, fifo_count
LOCAL INTEGER gyro_bias(2) = (0, 0, 0), accel_bias(2) = (0, 0, 0),accel_temp(2),gyro_temp(2)
LOCAL INTEGER accel_bias_reg(2) = (0, 0, 0)' A place to hold the factory accelerometer trim biases
LOCAL INTEGER mask_bit(2) = (0, 0, 0) ' Define array to hold mask bit for each accelerometer bias axis
LOCAL INTEGER accelsensitivity = 16384 ' = 16384 LSB/g
'
' reset device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, &H80) ' Write a one to bit 7 reset bit toggle reset device
pause 100
'
' get stable time source Auto select clock source to be PLL gyroscope reference if ready
' else use the internal oscillator, bits 2:0 = 001
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, &H01)
writeByte(MPU9250_ADDRESS, PWR_MGMT_2, &H00)
pause 200
'
' Configure device for bias calculation
writeByte(MPU9250_ADDRESS, INT_ENABLE, &H00) ' Disable all interrupts
writeByte(MPU9250_ADDRESS, FIFO_EN, &H00) ' Disable FIFO
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, &H00) ' Turn on internal clock source
writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, &H00) ' Disable I2C master
writeByte(MPU9250_ADDRESS, USER_CTRL, &H00) ' Disable FIFO and I2C master modes
writeByte(MPU9250_ADDRESS, USER_CTRL, &H0C) ' Reset FIFO and DMP
pause 15
'
' Configure MPU6050 gyro and accelerometer for bias calculation
writeByte(MPU9250_ADDRESS, CONFIG, &H01) ' Set low-pass filter to 188 Hz
writeByte(MPU9250_ADDRESS, SMPLRT_DIV, &H00) ' Set sample rate to 1 kHz
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, &H00) ' Set gyro full-scale to 250 degrees per second, maximum sensitivity
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, &H00) ' Set accelerometer full-scale to 2 g, maximum sensitivity

' Configure FIFO to capture accelerometer and gyro data for bias calculation
writeByte(MPU9250_ADDRESS, USER_CTRL, &H40) ' Enable FIFO
writeByte(MPU9250_ADDRESS, FIFO_EN, &H78) ' Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
pause 40 ' accumulate 40 samples in 40 milliseconds = 480 bytes
'
' At end of sample accumulation, turn off FIFO sensor read
writeByte(MPU9250_ADDRESS, FIFO_EN, &H00) ' Disable gyro and accelerometer sensors for FIFO
readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, rData()) ' read FIFO sample count
fifo_count = ((rData(0) << 8) OR rData(1))
packet_count = fifo_count/12' How many sets of full gyro and accelerometer data for averaging
for ii=0 to packet_count-1
for jj=0 to 2
accel_temp(jj)=0
gyro_temp(jj)=0
next jj
readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, rData()) ' read data for averaging
accel_temp(0) = iint16((rData(0) << 8) OR rData(1) ) ' Form signed 16-bit integer for each sample in FIFO
accel_temp(1) = iint16((rData(2) << 8) OR rData(3) )
accel_temp(2) = iint16((rData(4) << 8) OR rData(5) )
gyro_temp(0) = iint16((rData(6) << 8) OR rData(7) )
gyro_temp(1) = iint16((rData(8) << 8) OR rData(9) )
gyro_temp(2) = iint16((rData(10) << 8) OR rData(11))

accel_bias(0) = accel_bias(0)+ accel_temp(0) ' Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias(1) = accel_bias(1)+ accel_temp(1)
accel_bias(2) = accel_bias(2)+ accel_temp(2)
gyro_bias(0) = gyro_bias(0)+ gyro_temp(0)
gyro_bias(1) = gyro_bias(1)+ gyro_temp(1)
gyro_bias(2) = gyro_bias(2)+ gyro_temp(2)
next ii
accel_bias(0) = accel_bias(0)/ packet_count ' Normalize sums to get average count biases
accel_bias(1) = accel_bias(1)/ packet_count
accel_bias(2) = accel_bias(2)/ packet_count
gyro_bias(0) = gyro_bias(0)/ packet_count
gyro_bias(1) = gyro_bias(1)/ packet_count
gyro_bias(2) = gyro_bias(2)/ packet_count
if(accel_bias(2) > 0) THEN
accel_bias(2) = accel_bias(2) - accelsensitivity ' Remove gravity from the z-axis accelerometer bias calculation
else
accel_bias(2) = accel_bias(2) + accelsensitivity
endif
gyro_bias(0)=gyro_bias(0)/-4
gyro_bias(1)=gyro_bias(1)/-4
gyro_bias(2)=gyro_bias(2)/-4
' Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
rData(0) = (gyro_bias(0)>>8) AND &HFF ' Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
rData(1) = gyro_bias(0) AND &HFF ' Biases are additive, so change sign on calculated average gyro biases
rData(2) = (gyro_bias(1)>>8) AND &HFF
rData(3) = gyro_bias(1) AND &HFF
rData(4) = (gyro_bias(2)>>8) AND &HFF
rData(5) = gyro_bias(2) AND &HFF
'
' Push gyro biases to hardware registers
writeByte(MPU9250_ADDRESS, XG_OFFSET_H, rData(0))
writeByte(MPU9250_ADDRESS, XG_OFFSET_L, rData(1))
writeByte(MPU9250_ADDRESS, YG_OFFSET_H, rData(2))
writeByte(MPU9250_ADDRESS, YG_OFFSET_L, rData(3))
writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, rData(4))
writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, rData(5))
'
' Output scaled gyro biases for display in the main program
dest1(0) = gyro_bias(0)
dest1(1) = gyro_bias(1)
dest1(2) = gyro_bias(2)
'
' 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.
'
readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, rData()) ' Read factory accelerometer trim values
accel_bias_reg(0) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(0)=&H1
readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, rData())
accel_bias_reg(1) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(1)=&H1
readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, rData())
accel_bias_reg(2) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(2)=&H1
'
' Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg(0) = accel_bias_reg(0) - (accel_bias(0)\16) ' Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg(1) = accel_bias_reg(1) - (accel_bias(1)\16)
accel_bias_reg(2) = accel_bias_reg(2) - (accel_bias(2)\16)
rData(0) = (accel_bias_reg(0) \ 128) AND &HFF
rData(1) = ((accel_bias_reg(0))<<1 ) AND &HFE
rData(1) = rData(1) OR mask_bit(0) ' preserve temperature compensation bit when writing back to accelerometer bias registers
rData(2) = (accel_bias_reg(1) \ 128) AND &HFF
rData(3) = ((accel_bias_reg(1)) <<1) AND &HFE
rData(3) = rData(3) OR mask_bit(1) ' preserve temperature compensation bit when writing back to accelerometer bias registers
rData(4) = (accel_bias_reg(2) \ 128) AND &HFF
rData(5) = ((accel_bias_reg(2))<<1) AND &HFE
rData(5) = rData(5) OR mask_bit(2) ' preserve temperature compensation bit when writing back to accelerometer bias registers
'
' Push accelerometer biases to hardware registers
writeByte(MPU9250_ADDRESS, XA_OFFSET_H, rData(0))
writeByte(MPU9250_ADDRESS, XA_OFFSET_L, rData(1))
writeByte(MPU9250_ADDRESS, YA_OFFSET_H, rData(2))
writeByte(MPU9250_ADDRESS, YA_OFFSET_L, rData(3))
writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, rData(4))
writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, rData(5))
' Output scaled accelerometer biases for display in the main program
dest2(0) = accel_bias(0)
dest2(1) = accel_bias(1)
dest2(2) = accel_bias(2)
print "calibration done"
end sub
'
sub loadcalibrations
LOCAL INTEGER mask_bit(2) = (0, 0, 0) ' Define array to hold mask bit for each accelerometer bias axis
LOCAL INTEGER accel_bias_reg(2) = (0, 0, 0)' A place to hold the factory accelerometer trim biases
LOCAL INTEGER rData(11) ' data array to hold accelerometer and gyro x, y, z, data
' Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
rData(0) = (gyro_bias(0)>>8) AND &HFF ' Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
rData(1) = gyro_bias(0) AND &HFF ' Biases are additive, so change sign on calculated average gyro biases
rData(2) = (gyro_bias(1)>>8) AND &HFF
rData(3) = gyro_bias(1) AND &HFF
rData(4) = (gyro_bias(2)>>8) AND &HFF
rData(5) = gyro_bias(2) AND &HFF
'
' Push gyro biases to hardware registers
writeByte(MPU9250_ADDRESS, XG_OFFSET_H, rData(0))
writeByte(MPU9250_ADDRESS, XG_OFFSET_L, rData(1))
writeByte(MPU9250_ADDRESS, YG_OFFSET_H, rData(2))
writeByte(MPU9250_ADDRESS, YG_OFFSET_L, rData(3))
writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, rData(4))
writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, rData(5))
'
'
readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, rData()) ' Read factory accelerometer trim values
accel_bias_reg(0) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(0)=&H1
readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, rData())
accel_bias_reg(1) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(1)=&H1
readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, rData())
accel_bias_reg(2) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(2)=&H1
'
' Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg(0) = accel_bias_reg(0) - (accel_bias(0)\16) ' Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg(1) = accel_bias_reg(1) - (accel_bias(1)\16)
accel_bias_reg(2) = accel_bias_reg(2) - (accel_bias(2)\16)
rData(0) = (accel_bias_reg(0) \ 128) AND &HFF
rData(1) = ((accel_bias_reg(0))<<1 ) AND &HFE
rData(1) = rData(1) OR mask_bit(0) ' preserve temperature compensation bit when writing back to accelerometer bias registers
rData(2) = (accel_bias_reg(1) \ 128) AND &HFF
rData(3) = ((accel_bias_reg(1)) <<1) AND &HFE
rData(3) = rData(3) OR mask_bit(1) ' preserve temperature compensation bit when writing back to accelerometer bias registers
rData(4) = (accel_bias_reg(2) \ 128) AND &HFF
rData(5) = ((accel_bias_reg(2))<<1) AND &HFE
rData(5) = rData(5) OR mask_bit(2) ' preserve temperature compensation bit when writing back to accelerometer bias registers
'
' Push accelerometer biases to hardware registers
writeByte(MPU9250_ADDRESS, XA_OFFSET_H, rData(0))
writeByte(MPU9250_ADDRESS, XA_OFFSET_L, rData(1))
writeByte(MPU9250_ADDRESS, YA_OFFSET_H, rData(2))
writeByte(MPU9250_ADDRESS, YA_OFFSET_L, rData(3))
writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, rData(4))
writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, rData(5))
end sub

sub MadgwickQuaternionUpdate(ax as float, ay as float, az as float, gx as float, gy as float, gz 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

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

option explicit
option default none
'
' Micromite AHRS using MPU9250
' Version 2: Sensor fusion converted to CFunction
' Original source code from kriswiner (https://github.com/kriswiner/MPU-9250)
' and Sebastion Madgwick's Open source IMU and AHRS algorithms
'
' Use TFT display in landscape mode to display position
'
' MPU9250 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 screenIO=0 'set to 1 to do fast output on TFT screen, otherwise 0

'const MPU9250_ADDRESS =&H69 ' Device address when ADO = 1
const MPU9250_ADDRESS =&H68 ' Device address when ADO = 0
'
'Magnetometer Registers
const AK8963_ADDRESS =&H0C
const WHO_AM_I_AK8963 =&H00 ' should return =&H48
const AK8963_ST1 =&H02 ' data ready status bit 0
const AK8963_XOUT_L =&H03 ' data
const AK8963_CNTL =&H0A ' Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
const AK8963_ASAX =&H10 ' Fuse ROM x-axis sensitivity adjustment value
' Gyro/accelerometer registers
const SELF_TEST_X_GYRO =&H00
const SELF_TEST_Y_GYRO =&H01
const SELF_TEST_Z_GYRO =&H02
const SELF_TEST_X_ACCEL =&H0D
const SELF_TEST_Y_ACCEL =&H0E
const SELF_TEST_Z_ACCEL =&H0F
const XG_OFFSET_H =&H13 ' User-defined trim values for gyroscope
const XG_OFFSET_L =&H14
const YG_OFFSET_H =&H15
const YG_OFFSET_L =&H16
const ZG_OFFSET_H =&H17
const ZG_OFFSET_L =&H18
const SMPLRT_DIV =&H19
const CONFIG =&H1A
const GYRO_CONFIG =&H1B
const ACCEL_CONFIG =&H1C
const ACCEL_CONFIG2 =&H1D
const FIFO_EN =&H23
const I2C_MST_CTRL =&H24
const INT_PIN_CFG =&H37
const INT_ENABLE =&H38
const ACCEL_XOUT_H =&H3B
const TEMP_OUT_H =&H41
const GYRO_XOUT_H =&H43
const USER_CTRL =&H6A ' Bit 7 enable DMP, bit 3 reset DMP
const PWR_MGMT_1 =&H6B ' Device defaults to the SLEEP mode
const PWR_MGMT_2 =&H6C
const FIFO_COUNTH =&H72
const FIFO_R_W =&H74
const WHO_AM_I_MPU9250 =&H75 ' Should return =&H71
const XA_OFFSET_H =&H77
const XA_OFFSET_L =&H78
const YA_OFFSET_H =&H7A
const YA_OFFSET_L =&H7B
const ZA_OFFSET_H =&H7D
const ZA_OFFSET_L =&H7E
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 MFS_14BITS = 0' 0.6 uT per LSB
const MFS_16BITS =1' 0.15 uT0.6 per LSB
const outputFreq=50.0 ' output frequency in iterations
const declination=-0.63 ' local magnetic declination in degrees
'
' Global Variable definitions
'
DIM INTEGER Mmode = &H06 ' 1 for single shot, 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
DIM INTEGER Gscale = GFS_250DPS
DIM INTEGER Ascale = AFS_4G
DIM INTEGER Mscale = MFS_16BITS ' Choose either 14-bit or 16-bit magnetometer resolution
DIM FLOAT MagCalibration(2), gRes, mRes, aRes
dim float ax, ay, az, gx, gy, gz, mx, my, mz, SelfTest(5), mmult(2)
dim float pitch,yaw,roll,heading
dim float beta = 0.5 'derived from testing
dim integer gdata(13),mData(6),temp%
DIM FLOAT deltat = 1/outputFreq
dim float q(3) = (0,0,0,0)' quaternion of sensor frame relative to auxiliary frame
q(0)=sqr(2)/2 'set intial quaternion to north
q(1)=sqr(2)/2
DIM INTEGER lastrun
'
' Calibration parameters for gyro and accelerometer
dim float gyro_Bias(2)=(40,15,100), accel_Bias(2)=(340,55,-390)
'
' calibration parameters for magnetometer derived from running AK8963calibrate
DIM float magbias(2)=(17.2,-23,28), magscale(2)=(0.905,0.92,0.90)
'
' Setup
'
DIM FLOAT accs(2),mags(2),gees(2)
i2c open 400,1000
setpin intPin,DIN
pause 100
writeByte(MPU9250_ADDRESS,PWR_MGMT_1,&H80)
pause 500
initMPU9250
temp%=readByte(MPU9250_ADDRESS,WHO_AM_I_MPU9250)
if temp%<>&H71 then
Print "MPU9250 not found"
end
endif
temp%=readByte(AK8963_ADDRESS,WHO_AM_I_AK8963)
'if temp%<>&H48 then
' Print "AK8963 not found"
' end
'endif
print "Internal temperature is ",readTempData()
if screenIO then cls
initAK8963(MagCalibration())
getAres
getGres
getMres
if screenIO then readMPU9250(0)
' Uncomment this section to run the self test algorithm of the accelerometer and gyro
' Ensure the module is on a level surface and is completely still
' MPU9250SelfTest(selftest())
' print "Errors in %"
' print " Gyro Accel"
' Print "X ",str$(selftest(3),3,3," "),STR$(selftest(0),4,3," ")
' Print "Y ",str$(selftest(4),3,3," "),STR$(selftest(1),4,3," ")
' Print "Z ",str$(selftest(5),3,3," "),STR$(selftest(2),4,3," ")
' end
'
' Uncomment this section to derive calibration parameters for gyro and accelerometer
' Ensure the module is on a level surface and is completely still
' note the parameters and set them to load into the arrays gyroBias and accelBias above
' initMPU9250
' calibrateMPU9250(gyroBias(), accelBias())
' print " Gyro Accel"
' Print "X ",str$(gyroBias(0),3,3," "),STR$(accelbias(0),4,3," ")
' Print "Y ",str$(gyroBias(1),3,3," "),STR$(accelbias(1),4,3," ")
' Print "Z ",str$(gyroBias(2),3,3," "),STR$(accelbias(2),4,3," ")
' end
'

initMPU9250
loadcalibrations
'Print "AK8963 mag calibration X=",MagCalibration(0)," Y=",MagCalibration(1)," Z=",MagCalibration(2)
mmult(0)=mRes*magCalibration(0)/magscale(0)
mmult(1)=mRes*magCalibration(1)/magscale(1)
mmult(2)=mRes*magCalibration(2)/magscale(2)
temp%=0
'
' Main program
'
do
readMPU9250(60)
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)
heading = ((720.0-yaw)- declination)
if heading>360.0 then heading = heading-360.0
if (temp% mod outputFreq) = 0 then
Print "Heading = "+str$(heading,3,1," ")
endif
temp%=temp%+1
if screenIO then text 0,99,"pitch="+str$(pitch,3,2," ")+" roll="+str$(roll,3,2," ")+" yaw="+str$(yaw,3,2," ")
loop
end
'
sub readMPU9250(offset%)
do while not(pin(intPin)): loop
readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, gData()) 'read in the gyro and accelerometer data
readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, mData())' Read the six raw data and ST2 registers sequentially into data array
if((mData(6) AND &H08)=0) THEN ' Check if magnetic sensor overflow set, if not then report data
mx = sint16((mData(1) << 8) OR mData(0))*mmult(0) + magbias(0)' Turn the MSB and LSB into a signed 16-bit value
my = sint16((mData(3) << 8) OR mData(2))*mmult(1) + magbias(1) ' Data stored as little Endian
mz = sint16((mData(5) << 8) OR mData(4))*mmult(2) + magbias(2)
endif
' 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 ' Turn the MSB and LSB into a signed 16-bit value
ay = sint16((gData(2) << 8) OR gData(3))*aRes
az = sint16((gData(4) << 8) OR gData(5))*aRes
' Calculate the gyro value into actual radians per second
gx = RAD(sint16((gData(8) << 8) OR gData(9))* gRes) 'Turn the MSB and LSB into a signed 16-bit value
gy = RAD(sint16((gData(10) << 8) OR gData(11))* gRes)
gz = RAD(sint16((gData(12) << 8) OR gData(13))* gRes)
if screenIO then text 0,0+offset%,"Acc X="+str$(ax,3,2," ")+" Y="+str$(ay,3,2," ")+" Z="+str$(az,3,2," ")
if screenIO then text 0,13+offset%,"Rot X="+str$(gx,3,2," ")+" Y="+str$(gy,3,2," ")+" Z="+str$(gz,3,2," ")
if screenIO then text 0,26+offset%,"Mag X="+str$(mx,3,2," ")+" Y="+str$(my,3,2," ")+" Z="+str$(mz,3,2," ")
end sub
'
sub initMPU9250
LOCAL INTEGER c
' wake up device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, &H80) ' Reset
pause 100 ' Wait for all registers to reset
writeByte(MPU9250_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(MPU9250_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 = 011 this limits the sample rate to 1000 Hz for both
' With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
writeByte(MPU9250_ADDRESS, CONFIG, &H03)
' Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
writeByte(MPU9250_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(MPU9250_ADDRESS, GYRO_CONFIG)
c=c AND &B11100100
c = c OR (Gscale << 3) ' Set full scale range for the gyro
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ) ' Write new GYRO_CONFIG value to register
' Set accelerometer full-scale range configuration
c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG)
c=c AND &B11100111
c = c OR (Ascale << 3) ' Set full scale range for the accelerometer
writeByte(MPU9250_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
c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2)
c = c AND &B11110000
c = c OR &H03 ' Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c) ' Write new ACCEL_CONFIG2 register value
' 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(MPU9250_ADDRESS, INT_PIN_CFG, &H32)
writeByte(MPU9250_ADDRESS, INT_ENABLE, &H01) ' Enable data ready (bit 0) interrupt
writeByte(MPU9250_ADDRESS, USER_CTRL, &H05)
pause 100
end sub

sub initAK8963(destination() as float)
' First extract the factory calibration for each magnetometer axis
local integer rawData(2); ' x/y/z gyro calibration data stored here
writeByte(AK8963_ADDRESS, AK8963_CNTL, &H00); ' Power down magnetometer
Pause 10
writeByte(AK8963_ADDRESS, AK8963_CNTL, &H0F); ' Enter Fuse ROM access mode
readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, rawData()); ' Read the x-, y-, and z-axis calibration values
destination(0) = (rawData(0) - 128)/256 + 1 ' Return x-axis sensitivity adjustment values, etc.
destination(1) = (rawData(1) - 128)/256 + 1
destination(2) = (rawData(2) - 128)/256 + 1
writeByte(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
writeByte(AK8963_ADDRESS, AK8963_CNTL, (Mscale << 4) OR Mmode) 'Set magnetometer data resolution and sample ODR
Pause 10
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%()) as integer
i2c write address%,1,1,reg%
i2c read address%,0,n%,a%()
end sub
'
sub getMres
select case Mscale
' Possible magnetometer scales (and their register bit settings) are:
' 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS
mRes = 0.6 ' Proper scale to return milliGauss
case MFS_16BITS
mRes = 0.15 ' Proper scale to return milliGauss
end select
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
'
function readTempData() as float
local integer rawData(1) ' x/y/z gyro register data stored here
readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, rawData()) ' Read the two raw data registers sequentially into data array
readTempData=sint16((rawData(0) << 8) OR rawData(1)) / 333.87 + 21.0 '
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 iint16(x as integer) as integer ' convert to signed 16 bit number
local a%=x
IF a% AND &H8000 then a%=a% OR &HFFFFFFFFFFFF0000
iint16 = a%
END FUNCTION
'
FUNCTION iint15(x as integer) as integer ' convert to signed 16 bit number
local a%=x
IF a% AND &H4000 then a%=a% OR &HFFFFFFFFFFFF8000
iint15 = a%
END FUNCTION
'
sub MPU9250SelfTest(destination() as float) ' Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
local integer rawData(5) = (0, 0, 0, 0, 0, 0)
local integer selfTest(5)
local integer gAvg(2)=(0,0,0), aAvg(2)=(0,0,0), aSTAvg(2)=(0,0,0), gSTAvg(2)=(0,0,0)
local float factoryTrim(5)
local integer FS = 0,ii
writeByte(MPU9250_ADDRESS, SMPLRT_DIV, &H00) ' Set gyro sample rate to 1 kHz
writeByte(MPU9250_ADDRESS, CONFIG, &H02) ' Set gyro sample rate to 1 kHz and DLPF to 92 Hz
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, (1<<FS)) ' Set full scale range for the gyro to 250 dps
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, &H02) ' Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, (1<<FS)) ' Set full scale range for the accelerometer to 2 g
'
for ii = 0 to 199 ' get average current values of gyro and acclerometer
readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, rawData()) ' Read the six raw data registers into data array
aAvg(0) = aAvg(0) + iint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
aAvg(1) = aAvg(1) + iint16((rawData(2) << 8) OR rawData(3))
aAvg(2) = aAvg(2) + iint16((rawData(4) << 8) OR rawData(5))
readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, rawData()) ' Read the six raw data registers sequentially into data array
gAvg(0) = gAvg(0) + iint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
gAvg(1) = gAvg(1) + iint16((rawData(2) << 8) OR rawData(3))
gAvg(2) = gAvg(2) + iint16((rawData(4) << 8) OR rawData(5))
next ii
'
for ii =0 to 2 ' Get average of 200 values and store as average current readings
aAvg(ii) = aAvg(ii)/ 200
gAvg(ii) = gAvg(ii)/ 200
next ii
' Configure the accelerometer for self-test
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, &HE0) ' Enable self test on all three axes and set accelerometer range to +/- 2 g
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, &HE0) ' Enable self test on all three axes and set gyro range to +/- 250 degrees/s
pause 25 ' Delay a while to let the device stabilize
for ii = 0 to 199 ' get average self-test values of gyro and acclerometer
readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, rawData()) ' Read the six raw data registers into data array
aSTAvg(0) = aSTAvg(0) + iint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
aSTAvg(1) = aSTAvg(1) + iint16((rawData(2) << 8) OR rawData(3))
aSTAvg(2) = aSTAvg(2) + iint16((rawData(4) << 8) OR rawData(5))
readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, rawData()) ' Read the six raw data registers sequentially into data array
gSTAvg(0) = gSTAvg(0) + iint16((rawData(0) << 8) OR rawData(1)) ' Turn the MSB and LSB into a signed 16-bit value
gSTAvg(1) = gSTAvg(1) + iint16((rawData(2) << 8) OR rawData(3))
gSTAvg(2) = gSTAvg(2) + iint16((rawData(4) << 8) OR rawData(5))
next ii
'
for ii =0 to 2 ' Get average of 200 values and store as average current readings
aSTAvg(ii) =aSTAvg(ii)/200
gSTAvg(ii) =gSTAvg(ii)/200
next ii
'
' Configure the gyro and accelerometer for normal operation
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, &H00)
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, &H00)
pause 25 ' Delay a while to let the device stabilize
'
' Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
selfTest(0) = (readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL)) ' X-axis accel self-test results
selfTest(1) = (readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL)) ' Y-axis accel self-test results
selfTest(2) = (readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL)) ' Z-axis accel self-test results
selfTest(3) = (readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO) ) ' X-axis gyro self-test results
selfTest(4) = (readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO) ) ' Y-axis gyro self-test results
selfTest(5) = (readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO) ) ' Z-axis gyro self-test results
'
' Retrieve factory self-test value from self-test code reads
factoryTrim(0) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(0) - 1.0) )) ' FT(Xa) factory trim calculation
factoryTrim(1) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(1) - 1.0) )) ' FT(Ya) factory trim calculation
factoryTrim(2) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(2) - 1.0) )) ' FT(Za) factory trim calculation
factoryTrim(3) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(3) - 1.0) )) ' FT(Xg) factory trim calculation
factoryTrim(4) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(4) - 1.0) )) ' FT(Yg) factory trim calculation
factoryTrim(5) = (2620/(1<<FS))*(( 1.01 ^ (selfTest(5) - 1.0) )) ' FT(Zg) factory trim calculation
'
' Report results as a ratio of (STR - FT)/FT the change from Factory Trim of the Self-Test Response
' To get percent, must multiply by 100
for ii=0 to 2
destination(ii) = (aAvg(ii)-aSTAvg(ii)+factoryTrim(ii)) / factoryTrim(ii)*100 ' Report percent differences
destination(ii+3) = (gAvg(ii)-gSTAvg(ii)+factoryTrim(ii+3)) / factoryTrim(ii+3)*100 ' Report percent differences
next ii
end sub
'
sub calibrateMPU9250(dest1() as float, dest2() as float)
LOCAL INTEGER rData(11) ' data array to hold accelerometer and gyro x, y, z, data
LOCAL INTEGER ii, jj, packet_count, fifo_count
LOCAL INTEGER gyro_bias(2) = (0, 0, 0), accel_bias(2) = (0, 0, 0),accel_temp(2),gyro_temp(2)
LOCAL INTEGER accel_bias_reg(2) = (0, 0, 0)' A place to hold the factory accelerometer trim biases
LOCAL INTEGER mask_bit(2) = (0, 0, 0) ' Define array to hold mask bit for each accelerometer bias axis
LOCAL INTEGER accelsensitivity = 16384 ' = 16384 LSB/g
'
' reset device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, &H80) ' Write a one to bit 7 reset bit toggle reset device
pause 100
'
' get stable time source Auto select clock source to be PLL gyroscope reference if ready
' else use the internal oscillator, bits 2:0 = 001
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, &H01)
writeByte(MPU9250_ADDRESS, PWR_MGMT_2, &H00)
pause 200
'
' Configure device for bias calculation
writeByte(MPU9250_ADDRESS, INT_ENABLE, &H00) ' Disable all interrupts
writeByte(MPU9250_ADDRESS, FIFO_EN, &H00) ' Disable FIFO
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, &H00) ' Turn on internal clock source
writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, &H00) ' Disable I2C master
writeByte(MPU9250_ADDRESS, USER_CTRL, &H00) ' Disable FIFO and I2C master modes
writeByte(MPU9250_ADDRESS, USER_CTRL, &H0C) ' Reset FIFO and DMP
pause 15
'
' Configure MPU6050 gyro and accelerometer for bias calculation
writeByte(MPU9250_ADDRESS, CONFIG, &H01) ' Set low-pass filter to 188 Hz
writeByte(MPU9250_ADDRESS, SMPLRT_DIV, &H00) ' Set sample rate to 1 kHz
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, &H00) ' Set gyro full-scale to 250 degrees per second, maximum sensitivity
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, &H00) ' Set accelerometer full-scale to 2 g, maximum sensitivity

' Configure FIFO to capture accelerometer and gyro data for bias calculation
writeByte(MPU9250_ADDRESS, USER_CTRL, &H40) ' Enable FIFO
writeByte(MPU9250_ADDRESS, FIFO_EN, &H78) ' Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
pause 40 ' accumulate 40 samples in 40 milliseconds = 480 bytes
'
' At end of sample accumulation, turn off FIFO sensor read
writeByte(MPU9250_ADDRESS, FIFO_EN, &H00) ' Disable gyro and accelerometer sensors for FIFO
readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, rData()) ' read FIFO sample count
fifo_count = ((rData(0) << 8) OR rData(1))
packet_count = fifo_count/12' How many sets of full gyro and accelerometer data for averaging
for ii=0 to packet_count-1
for jj=0 to 2
accel_temp(jj)=0
gyro_temp(jj)=0
next jj
readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, rData()) ' read data for averaging
accel_temp(0) = iint16((rData(0) << 8) OR rData(1) ) ' Form signed 16-bit integer for each sample in FIFO
accel_temp(1) = iint16((rData(2) << 8) OR rData(3) )
accel_temp(2) = iint16((rData(4) << 8) OR rData(5) )
gyro_temp(0) = iint16((rData(6) << 8) OR rData(7) )
gyro_temp(1) = iint16((rData(8) << 8) OR rData(9) )
gyro_temp(2) = iint16((rData(10) << 8) OR rData(11))

accel_bias(0) = accel_bias(0)+ accel_temp(0) ' Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias(1) = accel_bias(1)+ accel_temp(1)
accel_bias(2) = accel_bias(2)+ accel_temp(2)
gyro_bias(0) = gyro_bias(0)+ gyro_temp(0)
gyro_bias(1) = gyro_bias(1)+ gyro_temp(1)
gyro_bias(2) = gyro_bias(2)+ gyro_temp(2)
next ii
accel_bias(0) = accel_bias(0)/ packet_count ' Normalize sums to get average count biases
accel_bias(1) = accel_bias(1)/ packet_count
accel_bias(2) = accel_bias(2)/ packet_count
gyro_bias(0) = gyro_bias(0)/ packet_count
gyro_bias(1) = gyro_bias(1)/ packet_count
gyro_bias(2) = gyro_bias(2)/ packet_count
if(accel_bias(2) > 0) THEN
accel_bias(2) = accel_bias(2) - accelsensitivity ' Remove gravity from the z-axis accelerometer bias calculation
else
accel_bias(2) = accel_bias(2) + accelsensitivity
endif
gyro_bias(0)=gyro_bias(0)/-4
gyro_bias(1)=gyro_bias(1)/-4
gyro_bias(2)=gyro_bias(2)/-4
' Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
rData(0) = (gyro_bias(0)>>8) AND &HFF ' Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
rData(1) = gyro_bias(0) AND &HFF ' Biases are additive, so change sign on calculated average gyro biases
rData(2) = (gyro_bias(1)>>8) AND &HFF
rData(3) = gyro_bias(1) AND &HFF
rData(4) = (gyro_bias(2)>>8) AND &HFF
rData(5) = gyro_bias(2) AND &HFF
'
' Push gyro biases to hardware registers
writeByte(MPU9250_ADDRESS, XG_OFFSET_H, rData(0))
writeByte(MPU9250_ADDRESS, XG_OFFSET_L, rData(1))
writeByte(MPU9250_ADDRESS, YG_OFFSET_H, rData(2))
writeByte(MPU9250_ADDRESS, YG_OFFSET_L, rData(3))
writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, rData(4))
writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, rData(5))
'
' Output scaled gyro biases for display in the main program
dest1(0) = gyro_bias(0)
dest1(1) = gyro_bias(1)
dest1(2) = gyro_bias(2)
'
' 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.
'
readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, rData()) ' Read factory accelerometer trim values
accel_bias_reg(0) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(0)=&H1
readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, rData())
accel_bias_reg(1) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(1)=&H1
readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, rData())
accel_bias_reg(2) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(2)=&H1
'
' Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg(0) = accel_bias_reg(0) - (accel_bias(0)\16) ' Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg(1) = accel_bias_reg(1) - (accel_bias(1)\16)
accel_bias_reg(2) = accel_bias_reg(2) - (accel_bias(2)\16)
rData(0) = (accel_bias_reg(0) \ 128) AND &HFF
rData(1) = ((accel_bias_reg(0))<<1 ) AND &HFE
rData(1) = rData(1) OR mask_bit(0) ' preserve temperature compensation bit when writing back to accelerometer bias registers
rData(2) = (accel_bias_reg(1) \ 128) AND &HFF
rData(3) = ((accel_bias_reg(1)) <<1) AND &HFE
rData(3) = rData(3) OR mask_bit(1) ' preserve temperature compensation bit when writing back to accelerometer bias registers
rData(4) = (accel_bias_reg(2) \ 128) AND &HFF
rData(5) = ((accel_bias_reg(2))<<1) AND &HFE
rData(5) = rData(5) OR mask_bit(2) ' preserve temperature compensation bit when writing back to accelerometer bias registers
'
' Push accelerometer biases to hardware registers
writeByte(MPU9250_ADDRESS, XA_OFFSET_H, rData(0))
writeByte(MPU9250_ADDRESS, XA_OFFSET_L, rData(1))
writeByte(MPU9250_ADDRESS, YA_OFFSET_H, rData(2))
writeByte(MPU9250_ADDRESS, YA_OFFSET_L, rData(3))
writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, rData(4))
writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, rData(5))
' Output scaled accelerometer biases for display in the main program
dest2(0) = accel_bias(0)
dest2(1) = accel_bias(1)
dest2(2) = accel_bias(2)
print "calibration done"
end sub
'
sub loadcalibrations
LOCAL INTEGER mask_bit(2) = (0, 0, 0) ' Define array to hold mask bit for each accelerometer bias axis
LOCAL INTEGER accel_bias_reg(2) = (0, 0, 0)' A place to hold the factory accelerometer trim biases
LOCAL INTEGER rData(11) ' data array to hold accelerometer and gyro x, y, z, data
' Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
rData(0) = (gyro_bias(0)>>8) AND &HFF ' Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
rData(1) = gyro_bias(0) AND &HFF ' Biases are additive, so change sign on calculated average gyro biases
rData(2) = (gyro_bias(1)>>8) AND &HFF
rData(3) = gyro_bias(1) AND &HFF
rData(4) = (gyro_bias(2)>>8) AND &HFF
rData(5) = gyro_bias(2) AND &HFF
'
' Push gyro biases to hardware registers
writeByte(MPU9250_ADDRESS, XG_OFFSET_H, rData(0))
writeByte(MPU9250_ADDRESS, XG_OFFSET_L, rData(1))
writeByte(MPU9250_ADDRESS, YG_OFFSET_H, rData(2))
writeByte(MPU9250_ADDRESS, YG_OFFSET_L, rData(3))
writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, rData(4))
writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, rData(5))
'
'
readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, rData()) ' Read factory accelerometer trim values
accel_bias_reg(0) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(0)=&H1
readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, rData())
accel_bias_reg(1) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(1)=&H1
readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, rData())
accel_bias_reg(2) = iint15((rData(0) << 7) OR ((rData(1) and &HFE)>>1))
if rData(1) and &H1 then mask_bit(2)=&H1
'
' Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg(0) = accel_bias_reg(0) - (accel_bias(0)\16) ' Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg(1) = accel_bias_reg(1) - (accel_bias(1)\16)
accel_bias_reg(2) = accel_bias_reg(2) - (accel_bias(2)\16)
rData(0) = (accel_bias_reg(0) \ 128) AND &HFF
rData(1) = ((accel_bias_reg(0))<<1 ) AND &HFE
rData(1) = rData(1) OR mask_bit(0) ' preserve temperature compensation bit when writing back to accelerometer bias registers
rData(2) = (accel_bias_reg(1) \ 128) AND &HFF
rData(3) = ((accel_bias_reg(1)) <<1) AND &HFE
rData(3) = rData(3) OR mask_bit(1) ' preserve temperature compensation bit when writing back to accelerometer bias registers
rData(4) = (accel_bias_reg(2) \ 128) AND &HFF
rData(5) = ((accel_bias_reg(2))<<1) AND &HFE
rData(5) = rData(5) OR mask_bit(2) ' preserve temperature compensation bit when writing back to accelerometer bias registers
'
' Push accelerometer biases to hardware registers
writeByte(MPU9250_ADDRESS, XA_OFFSET_H, rData(0))
writeByte(MPU9250_ADDRESS, XA_OFFSET_L, rData(1))
writeByte(MPU9250_ADDRESS, YA_OFFSET_H, rData(2))
writeByte(MPU9250_ADDRESS, YA_OFFSET_L, rData(3))
writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, rData(4))
writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, rData(5))
end sub

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

#define Version 100 //Version 1.00
#define _SUPPRESS_PLIB_WARNING // required for XC1.33 Later compiler versions will need PLIB to be installed
#include <plib.h> // the pre Harmony peripheral libraries
#include "../cfunctions.h"
float atan2(float y,float x){
float PI = LoadFloat(0x40490FDA);
float PIby2 = LoadFloat(0x3FC90FDA);
float PIbyneg2 = LoadFloat(0xBFC90FDA);
float twoPI = LoadFloat(0x40C90FDA);
float R_0=LoadFloat(0x00000000);
float atan2ret=LoadFloat(0x00000000);
if (FCmp(x , 0)==1 ){
atan2ret = atanf(FDiv(y,x));

}
else if( (FCmp(y , R_0)!=-1) && (FCmp(x , R_0)==-1)){
atan2ret = FAdd(PI , atanf(FDiv(y,x))) ;
}
else if((FCmp(y , R_0)==-1) && (FCmp(x , R_0)==-1)){
atan2ret = FSub(atanf(FDiv(y,x)) , PI) ;
}
else if ((FCmp(y , R_0)==1) && (FCmp(x , R_0)==0) ){
atan2ret = PIby2 ;
}
else if ( (FCmp(y , R_0)==-1) && (FCmp(x , R_0)==0)){
atan2ret = PIbyneg2 ;
}
if ( FCmp(atan2ret , R_0)==-1) {
atan2ret = FAdd(atan2ret , twoPI );
}
return atan2ret;
}

float asin(float x){
float R_05=LoadFloat(0x3F000000);
float R_1=LoadFloat(0x3F800000);
float asincalc;
asincalc = atanf( FDiv(x , FPow( FSub(R_1 , FMul(x , x)) ,R_05) ) ) ;
return asincalc;
}

void main(float q[], float ac[], float g[], float mg[], float *betap, unsigned long long *lastrun, float *yaw, float *pitch, float *roll ){
unsigned int current_ticks;
asm volatile("mfc0 %0, $9" : "=r"(current_ticks));//get the time in ticks
unsigned long long deltalastrun,tick_calc=current_ticks;
if(tick_calc<*lastrun) tick_calc |= 0x100000000;
deltalastrun=tick_calc-*lastrun;
*lastrun=tick_calc & 0xFFFFFFFF;

float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; //short name variable for readability
float ax = ac[0], ay = ac[1], az = ac[2]; //short name variable for readability
float gx = g[0], gy = g[1], gz = g[2]; //short name variable for readability
float mx = mg[0], my = mg[1], mz = mg[2]; //short name variable for readability
float R_2=LoadFloat(0x40000000);
float R_05=LoadFloat(0x3F000000);
float R_1=LoadFloat(0x3F800000);
float R_0=LoadFloat(0x00000000);
float R_4=LoadFloat(0x40800000);
float DEG=LoadFloat(0x42652EE2);
float norm, beta=*betap, deltat;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
float _2q1mx , _2q1my, _2q1mz, _2q2mx, _4bx, _4bz , t2233, t1234, t2413, g3344, g2314, g1324;
float mx1, my1, mz1, mx12, ay1, az1;
deltat=FMul(FDiv(IntToFloat((int)deltalastrun),IntToFloat(CurrentCpuSpeed)),R_2);
// Auxiliary variables to avoid repeated arithmetic
float _2q1 = FMul(R_2 , q1);
float _2q2 = FMul(R_2 , q2);
float _2q3 = FMul(R_2 , q3);
float _2q4 = FMul(R_2 , q4);
float _2q1q3 = FMul(FMul(R_2 , q1), q3);
float _2q3q4 = FMul(FMul(R_2 , q3), q4);
float q1q1 = FMul(q1 , q1);
float q1q2 = FMul(q1 , q2);
float q1q3 = FMul(q1 , q3);
float q1q4 = FMul(q1 , q4);
float q2q2 = FMul(q2 , q2);
float q2q3 = FMul(q2 , q3);
float q2q4 = FMul(q2 , q4);
float q3q3 = FMul(q3 , q3);
float q3q4 = FMul(q3 , q4);
float q4q4 = FMul(q4 , q4);

// Normalise accelerometer measurement
norm = FPow(FAdd(FAdd(FMul(ax,ax) , FMul(ay , ay)) , FMul(az , az)),R_05);
if (FCmp(norm , R_0)==0) return;// handle NaN
norm = FDiv(R_1,norm);
ax = FMul(ax , norm);
ay = FMul(ay , norm);
az = FMul(az , norm);

// Normalise magnetometer measurement
norm = FPow(FAdd(FAdd(FMul(mx,mx) , FMul(my , my)) , FMul(mz , mz)),R_05);
if (FCmp(norm , R_0)==0) return;// handle NaN
norm = FDiv(R_1,norm);
mx = FMul(mx , norm);
my = FMul(my , norm);
mz = FMul(mz , norm);
// Reference direction of Earth//s magnetic field
_2q1mx = FMul(FMul(R_2 , q1), mx);
_2q1my = FMul(FMul(R_2 , q1), my);
_2q1mz = FMul(FMul(R_2 , q1), mz);
_2q2mx = FMul(FMul(R_2 , q2), mx);

hx = FSub(FSub(FAdd(FAdd(FAdd(FAdd(FSub(FMul(mx , q1q1) , FMul(_2q1my , q4)) , FMul(_2q1mz , q3)) , FMul(mx , q2q2)) , FMul(FMul(_2q2 , my) , q3)) ,+ FMul(FMul(_2q2 , mz) , q4)) , FMul(mx , q3q3)) , FMul(mx , q4q4));

hy = FMul(_2q1mx , q4);
hy = FAdd(hy, FMul(my , q1q1));
hy = FSub(hy, FMul(_2q1mz , q2));
hy = FAdd(hy, FMul(_2q2mx , q3));
hy = FSub(hy, FMul(my , q2q2));
hy = FAdd(hy, FMul(my , q3q3));
hy = FAdd(hy, FMul(FMul(_2q3 , mz) , q4));
hy = FSub(hy, FMul(my , q4q4));
_2bx = FPow(FAdd(FMul(hx , hx) , FMul(hy , hy)),R_05);
_2bz = FMul(_2q1my , q2);
_2bz = FSub(_2bz , FMul(_2q1mx , q3));
_2bz = FAdd(_2bz , FMul(mz , q1q1));
_2bz = FAdd(_2bz , FMul(_2q2mx , q4));
_2bz = FSub(_2bz , FMul(mz , q2q2));
_2bz = FAdd(_2bz , FMul(FMul(_2q3 , my) , q4));
_2bz = FSub(_2bz , FMul(mz , q3q3));
_2bz = FAdd(_2bz , FMul(mz , q4q4));
_4bx = FMul(R_2 , _2bx);
_4bz = FMul(R_2 , _2bz);

// Gradient decent algorithm corrective step
t2233 = FMul(_2bz , (FSub(FSub(R_05 , q2q2) , q3q3)));
t1234 = FMul(_2bz , FAdd(q1q2 , q3q4));
t2413 = FMul(_2bz , FSub(q2q4 , q1q3));
g3344 = FMul(_2bx , FSub(FSub(R_05 , q3q3) , q4q4));
g2314 = FMul(_2bx , FSub(q2q3 , q1q4));
g1324 = FMul(_2bx , FAdd(q1q3 , q2q4));
mx1 = FSub(FSub(FMul(R_2 , q2q4) , _2q1q3) , ax);
my1 = FSub(FAdd(g2314 , t1234) , my);
mz1 = FSub(FAdd(g1324 , t2233) , mz);
mx12 = FSub(FAdd(g3344 , t2413) , mx);
ay1 = FSub(FAdd(FMul(R_2 , q1q2) , _2q3q4) , ay);
az1 = FSub(FSub(FSub(R_1 , FMul(R_2 , q2q2)) , FMul(R_2 , q3q3)) , az);

// Gradient decent algorithm corrective step
s1 = FAdd(FAdd(FSub(FSub(FMul(_2q2 , ay1) , FMul(_2q3 , mx1)) , FMul(FMul(_2bz , q3) , mx12)) , FMul(my1 , FSub(FMul(_2bz , q2) , FMul(_2bx , q4)))) , FMul(FMul(_2bx , q3) , mz1));
s2 = FAdd(FAdd(FAdd(FSub(FAdd(FMul(_2q4 , mx1) , FMul(_2q1 , ay1)) , FMul(FMul(R_4 , q2) , az1)) , FMul(FMul(_2bz , q4) , mx12)) , FMul(my1 , FAdd(FMul(_2bx , q3) , FMul(_2bz , q1)))) , FMul(mz1 , FSub(FMul(_2bx , q4) , FMul(_4bz , q2))));
s3 = FAdd(FAdd(FAdd(FSub(FSub(FMul(_2q4 , ay1) , FMul(_2q1 , mx1)) , FMul(FMul(R_4 , q3) , az1)) , FMul(mx12 , FSub(FMul(FSub(R_0,_4bx) , q3) , FMul(_2bz , q1)))) , FMul(my1 , FAdd(FMul(_2bx , q2) , FMul(_2bz , q4)))) , FMul(mz1 , FSub(FMul(_2bx , q1) , FMul(_4bz , q3))));
s4 = FAdd(FAdd(FAdd(FAdd(FMul(_2q2 , mx1) , FMul(_2q3 , ay1)) , FMul(mx12 , FSub( FMul(_2bz , q2) , FMul(_4bx , q4)))) , FMul(my1 , FSub(FMul(_2bz , q3) , FMul(_2bx , q1)))) , FMul(FMul(_2bx , q2) , mz1));


norm = FPow(FAdd(FAdd(FAdd(FMul(s1,s1) , FMul(s2 , s2)) , FMul(s3 , s3)), FMul(s4 , s4)),R_05); // normalise step magnitude
norm = FDiv(R_1,norm);
s1 = FMul(s1 , norm);
s2 = FMul(s2 , norm);
s3 = FMul(s3 , norm);
s4 = FMul(s4 , norm);


// Compute rate of change of quaternion
qDot1 = FSub(FMul(R_05 , (FSub(FSub(FMul(FSub(R_0,q2) , gx) , FMul(q3 , gy)) , FMul(q4 , gz)))) , FMul(beta , s1));
qDot2 = FSub(FMul(R_05 , (FSub(FAdd(FMul(q1 , gx) , FMul(q3 , gz)) , FMul(q4 , gy)))) , FMul(beta , s2));
qDot3 = FSub(FMul(R_05 , (FAdd(FSub(FMul(q1 , gy) , FMul(q2 , gz)) , FMul(q4 , gx)))) , FMul(beta , s3));
qDot4 = FSub(FMul(R_05 , (FSub(FAdd(FMul(q1 , gz) , FMul(q2 , gy)) , FMul(q3 , gx)))), FMul(beta , s4));

// Integrate to yield quaternion
q1 = FAdd(q1 , FMul(qDot1 , deltat));
q2 = FAdd(q2 , FMul(qDot2 , deltat));
q3 = FAdd(q3 , FMul(qDot3 , deltat));
q4 = FAdd(q4 , FMul(qDot4 , deltat));
norm = FPow(FAdd(FAdd(FAdd(FMul(q1,q1) , FMul(q2 , q2)) , FMul(q3 , q3)), FMul(q4 , q4)),R_05); // normalise quaternion
norm = FDiv(R_1,norm);
q[0] = FMul(q1 , norm);
q[1] = FMul(q2 , norm);
q[2] = FMul(q3 , norm);
q[3] = FMul(q4 , norm);
*yaw = FMul(DEG,atan2(FMul(R_2 , FAdd(FMul(q[1] , q[2]) , FMul(q[0] , q[3]))), FSub(FSub(FAdd(FMul(q[0] , q[0]) , FMul(q[1] , q[1])) , FMul(q[2] , q[2])) , FMul(q[3] , q[3])))) ;
*pitch = FMul(DEG,FSub(R_0, asin(FMul(R_2 , FSub(FMul(q[1] , q[3]) , FMul(q[0] , q[2]))))));
*roll = FMul(DEG,atan2(FMul(R_2 , FAdd(FMul(q[0] , q[1]) , FMul(q[2] , q[3]))) , FAdd(FSub(FSub(FMul(q[0] , q[0]) , FMul(q[1] , q[1])) , FMul(q[2] , q[2])) , FMul(q[3] , q[3])))) ;
}