option explicit
option default FLOAT
'
' Micromite AHRS using MPU6050
' Original source ode from kriswiner (https://github.com/kriswiner/MPU-6050)
' and Sebastion Madgwick's Open source IMU and AHRS algorithms
'
' Use TFT display in landscape mode to display position
'
' MPU6050 connections
' GND
' VCC - 3.3V
' SCL to I2C clock
' SDA to I2C data
' NCS to 3.3V (needed to force I2C)
' INT to intPin
' AD0 to GND or 3.3V (see address below)
const intPin = 16
'const MPU6050_ADDRESS =&H69 ' Device address when ADO = 1
const MPU6050_ADDRESS =&H68 ' Device address when ADO = 0
backlight 10
'
const screenIO=0 'set to 1 to do fast output on TFT screen, otherwise 0
'
'Magnetometer Registers and constants
const HMC5883L_I2C = &h1e
const HMC5883_MAGGAIN_4_7 = &hA0' +/- 4.7
const hmc5883_Gauss_LSB_X = 381 'least significant bits per uT
const hmc5883_Gauss_LSB_Y = 403
const hmc5883_Gauss_LSB_Z = 349
const magbias_x = 12.2 ' User environmental correction (hard-iron) in MICROTESLA from calibration routine
const magbias_y = 20.6
const magbias_z = 2.7
const GAUSS_TO_MICROTESLA = 100
Const HMC5883_CONFIG_A = 0
Const HMC5883_CONFIG_B = 1
Const HMC5883_MODE = 2
Const HMC5883_MSB_X = 3
Const HMC5883_ID_A = 10

' Gyro/accelerometer registers
const SMPLRT_DIV =&H19
const CONFIG =&H1A
const GYRO_CONFIG =&H1B
const ACCEL_CONFIG =&H1C
const INT_PIN_CFG =&H37
const INT_ENABLE =&H38
const ACCEL_XOUT_H =&H3B
const TEMP_OUT_H =&H41
const USER_CTRL =&H6A ' Bit 7 enable DMP, bit 3 reset DMP
const PWR_MGMT_1 =&H6B ' Device defaults to the SLEEP mode
const WHO_AM_I_MPU6050 =&H75 ' Should return =&H71
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 mG per LSB
const MFS_16BITS =1' 0.15 mG per LSB
const C_INSIDE = 0' 0000
const C_LEFT = 1 ' 0001
const C_RIGHT = 2 ' 0010
const C_BOTTOM = 4' 0100
const C_TOP = 8 ' 1000
dim integer w = 266
dim integer h=w
dim integer wby2 = w\2
dim integer xmin = 2
dim integer xmax = xmin+w
dim integer ymin = 2
dim integer ymax=w+ymin
dim integer xm = xmin+wby2
dim integer ym = ymin+wby2
dim integer xmn3=xm-3
dim integer xmn7=xm-7
dim integer xmn10=xm-10
dim integer xmn15=xm-15
dim integer xmn20=xm-20
dim integer xmn25=xm-25
dim integer xmn26=xm-26
dim integer xmn30=xm-30
dim integer xmp7=xm+7
dim integer xmp10=xm+10
dim integer xmp15=xm+15
dim integer xmp20=xm+20
dim integer xmp21=xm+21
dim integer xmp25=xm+25
dim integer xmp26=xm+26

dim integer ymn2=ym-2
dim integer ymn16=ym-16
dim integer ymn32=ym-32
dim integer ymn48=ym-48
dim integer ymn64=ym-64
dim integer ymp4=ym+4
dim integer ymp13=ym+13
dim integer ymp16=ym+16
dim integer ymp32=ym+32
dim integer ymp48=ym+48
dim integer ymp64=ym+64
dim integer ymp80=ym+80
dim integer ymp96=ym+96
dim integer ymp112=ym+112

dim integer radius = 101
dim integer ymmr16 = ym-radius+16
dim integer ymmrm16 = ym-radius-16
dim integer ymmr1 = ym-radius+1
dim integer ymmrm1 = ym-radius-1
dim integer rp20 = radius+20
dim integer rp10 = radius+10
dim integer rp15 = radius+15
dim integer rm20 = radius-20
dim integer xmnr = xm-radius
dim integer rgbw = rgb(white)
dim integer rgbb = rgb(black)
dim integer rgby = rgb(yellow)
dim integer rgbbr = rgb(brown)
dim integer rgbbl = rgb(blue)
dim integer x0,y0,x1,y1,x2,y2
dim integer rx1(9),ry1(9),rx2(9),ry2(9),rcol(9)=(rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw)
dim float pitchperdegree=-3.2
dim integer lx1(24)=(xmn26,xmn20,xmn26,xmp20,xmp26,xmp20,xmn15,xmn15,xmn15,xmn15,xmn15,xmn15,xmn25,xmn25,xmn25,xmn25,xmn25,xmp10,xmp 10,xmp10,xmp10,xmp10,xmp7,xm,xm)
dim integer ly1(24)=(ymp4,ymp4,ymp13,ymp4,ymp4,ymp13,ymp16,ymp48,ymp80,ymp112,ymn16,ymn48,ymn32,ymn64,ymp64,ymp32,ymp96,ymn32,ymn64, ymp64,ymp32,ymp96,ymmr16,ymmr1,ymmr1)
dim integer lx2(24)=(xmn26,xmn20,xmn20,xmp20,xmp26,xmp26,xmp15,xmp15,xmp15,xmp15,xmp15,xmp15,xmn10,xmn10,xmn10,xmn10,xmn10,xmp25,xmp 25,xmp25,xmp25,xmp25,xmn7,xmp7,xmn7)
dim integer ly2(24)=(ymp13,ymp13,ymp13,ymp13,ymp13,ymp13,ymp16,ymp48,ymp80,ymp112,ymn16,ymn48,ymn32,ymn64,ymp64,ymp32,ymp96,ymn32,ym n64,ymp64,ymp32,ymp96,ymmr16,ymmr16,ymmr16)
dim integer lcol(24)=(rgbb,rgbb,rgbb,rgbb,rgbb,rgbb,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw,rgbw, rgbb,rgbb,rgbb)

'
' 3D solid graphics demo using quaternions to rotate the coordinates
'
cls
backlight 50
'
' Adjust the following to fit your display
'
dim float accelbias(2)=(0.009,-0.024,0.0), gyrobias(2)=(0.032,-0.015,0.0)
'
' Global Variable definitions
'
DIM FLOAT BETA = 0.2 ' gain of algorithm derived from testing
dim float q(3) = (0,0,0,0)' quaternion of sensor frame relative to auxiliary frame set to North
q(0)=sqr(2)/2
q(1)=sqr(2)/2
'
' Golbal Variable definitions
'
DIM INTEGER Gscale = GFS_500DPS
DIM INTEGER Ascale = AFS_8G
DIM FLOAT gRes,aRes
dim float ax,ay,az,gx,gy,gz,mx,my,mz
dim float pitch,yaw,roll
dim integer gdata(13),mData(5),temp%,page%
DIM ID$ length 3
DIM INTEGER lastrun
const MA = 10 'number of elements in moving average
DIM FLOAT accs(2),mags(2),gees(2)
dim float stemp,ctemp,pitchSMA(MA-1), rollSMAcos(MA-1), rollSMAsine(MA-1), pitchMA, rollMA, rollMAs, rollMAc
'
' Setup
'
i2c open 400,1000
setpin intPin,DIN
pause 100
writeByte(MPU6050_ADDRESS,PWR_MGMT_1,&H80)
pause 500
initMPU6050
temp%=readByte(MPU6050_ADDRESS,WHO_AM_I_MPU6050)
if temp%<>&H68 then
Print "MPU6050 not found"
end
endif
readByteString(HMC5883L_I2C,HMC5883_ID_A,3,ID$)
if ID$<>"H43" then
Print "HMC5883 not found"
end
endif
print "Internal temperature is ",readTempData()
cls
initHMC5883
getAres
getGres

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

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

'
' I2C primitives
'
sub writeByte(address%,reg%,value%)
i2c write address%,0,2,reg%,value%
end sub
'
function readByte(address%,reg%) as integer
i2c write address%,1,1,reg%
i2c read address%,0,1,readByte
end function
'
sub readBytes(address%,reg%,n%,a%())
i2c write address%,1,1,reg%
i2c read address%,0,n%,a%()
end sub
'
sub readByteString(address%,reg%,n%,s$)
i2c write address%,1,1,reg%
i2c read address%,0,n%,S$
end sub
'
' sensor scaling factors
'
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 'read temperature
local integer rawData(1) ' x/y/z gyro register data stored here
readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, rawData()) ' Read the two raw data registers sequentially into data array
readTempData=sint16((rawData(0) << 8) OR rawData(1)) / 340 + 36.53
end function
'
FUNCTION sint16(x as integer) as float ' convert to signed 16 bit number
local a%=x
IF a% AND &H8000 then a%=a% OR &HFFFFFFFFFFFF0000
sint16 = a%
END FUNCTION
'
sub drawAI(page as integer, roll as float, pitch as float) 'draw the AI display

local float xf1,yf1,xf2,yf2, pitchoffset, rolloffset,yo1,yo2
local integer xi1,yi1,xi2,yi2,rgbtop,rgbbottom;
local integer offscreen,o = (page-1)*272
local float rroll=rad(roll)
if cos(rad(roll))>=0 then
rgbtop=rgbbl
rgbbottom=rgbbr
else
rgbtop=rgbbr
rgbbottom=rgbbl
endif
'
'sections which vary with pitch and roll
'
pitchoffset=pitch*pitchperdegree
rolloffset=wby2 * tan(rroll)
xf1=xmin
xf2=xmax
yf1=ym+pitchoffset+rolloffset:yo1=yf1
yf2=ym+pitchoffset-rolloffset:yo2=yf2
offscreen = CohenSutherlandLineClip(xf1,yf1,xf2,yf2)
xi1=fix(xf1):yi1=fix(yf1):xi2=fix(xf2):yi2=fix(yf2)
print
if NOT offscreen then
box xmin-2,0+o,w+4,h+4,2,rgbw,rgbtop
if yo2<yo1 then
if (xi1=xmin) and (xi2<>xmax) then
box xi1,yi1+o,w,ymax-yi1,0,,rgbbottom
box xi2,yi2+o,xmax-xi2,h,0,,rgbbottom
triangles(1,xi1,yi1+o,xi2,yi2+o,xi2,yi1+o,rgbbottom,0)
elseif (xi1=xmin) and (xi2=xmax) then
box xi1,yi1+o,w,ymax-yi1,0,,rgbbottom
triangles(1,xi1,yi1+o,xi2,yi2+o,xi2,yi1+o,rgbbottom,0)
elseif (xi1<>xmin) and (xi2=xmax) then
triangles(1,xi1,yi1+o,xi2,yi2+o,xi2,yi1+o,rgbbottom,0)
elseif (xi1<>xmin) and (xi2<>xmax) then
box xi2,yi2+o,xmax-xi2,h,0,,rgbbottom
triangles(1,xi1,yi1+o,xi2,yi2+o,xi2,yi1+o,rgbbottom,0)
endif
elseif yo1<yo2 then
if (xi1<>xmin) and (xi2=xmax) then
box xmin,yi2+o,w,ymax-yi2,0,,rgbbottom
box xmin,yi1+o,xi1-xmin,h,0,,rgbbottom
triangles(1,xi1,yi1+o,xi2,yi2+o,xi1,yi2+o,rgbbottom,0)
elseif (xi1=xmin) and (xi2=xmax) then
box xi1,yi2+o,w,ymax-yi2,0,,rgbbottom
triangles(1,xi1,yi1+o,xi2,yi2+o,xi1,yi2+o,rgbbottom,0)
elseif (xi1=xmin) and (xi2<>xmax) then
triangles(1,xi1,yi1+o,xi2,yi2+o,xi1,yi2+o,rgbbottom,0)
elseif (xi1<>xmin) and (xi2<>xmax) then
box xmin,yi1+o,xi1-xmin,h,0,,rgbbottom
triangles(1,xi1,yi1+o,xi2,yi2+o,xi1,yi2+o,rgbbottom,0)
endif
else
box xi1,yi1+o,w,ymax-yi1,0,,rgbbottom
endif
dcirch xm,ym+o,radius,rgbw,xmn30,ymp96+o,xmn30+60,ymp96+o+8
line xi1,yi1+o,xi2,yi2+o,,rgbw
else 'no useful information
if (offscreen and C_TOP) then
box xmin-2,0+o,w+4,h+4,2,rgbw,rgbtop
else
box xmin-2,0+o,w+4,h+4,2,rgbw,rgbbottom
endif
dcirch xm,ym+o,radius,rgbw,xmn30,ymp96+o,xmn30+60,ymp96+o+8
endif
'
' fixed background
'
text xm,ymn64+o,"20",CM,,,rgbw,1
text xm,ymn32+o,"10",CM,,,rgbw,1
text xm,ymp64+o,"20",CM,,,rgbw,1
text xm,ymp32+o,"10",CM,,,rgbw,1
text xm,ymp96+o,"30",CM,,,rgbw,1
triangles(1,xm,ymmr1+o,xmn7,ymmr16+o,xmp7,ymmr16+o,rgby,0)
BOX xmn3,ymn2+o,6,6,1,rgbb,rgby
BOX xmnr ,ymn2+o,rm20,6,1,rgbb,rgby
BOX xmp20 ,ymn2+o,rm20,6,1,rgbb,rgby
box xmn25,ymp4+o,4,8,0,,rgby
box xmp21,ymp4+o,4,8,0,,rgby
'
lines(25,lx1(),ly1(),lx2(),ly2(),lcol(),page)
'
' sections which vary with roll
'
rotxy(xm,ym,roll,xm,ymmrm1,x0,y0)
rotxy(xm,ym,roll,xmn7,ymmrm16,x1,y1)
rotxy(xm,ym,roll,xmp7,ymmrm16,x2,y2)
triangles(1,x0,y0+o,x1,y1+o,x2,y2+o,rgbw,0)
radial xm,ym,rp20,330-roll,radius,0
radial xm,ym,rp20,30-roll,radius,1
radial xm,ym,rp20,300-roll,radius,2
radial xm,ym,rp20,60-roll,radius,3
radial xm,ym,rp10,10-roll,radius,4
radial xm,ym,rp10,20-roll,radius,5
radial xm,ym,rp10,340-roll,radius,6
radial xm,ym,rp10,350-roll,radius,7
radial xm,ym,rp15,45-roll,radius,8
radial xm,ym,rp15,315-roll,radius,9
lines (10,rx1(),ry1(),rx2(),ry2(),rcol(),page)
ssd1963 page
pause 10 'needed to avoid flash on page switch
end sub
'
sub rotxy(xC as float, yC as float, angleD as float, x as float, y as float, xmaxot as integer, yrot as integer)
local float angle = rad(-angled)
local float s=sin(Angle),c=cos(Angle)
xmaxot = FIX(xC + c * (x - xC) - s * (y - yC))
yRot = FIX(yC + s * (x - xC) + c * (y - yC))
end sub
'
' Routine to calculate a radial to a circle
' Parameters are:
' x-coordinate of centre of circle
' y-coordinate of centre of circle
' radius of circle
' radial of segment to be drawn (0-360 degrees)
' inner radius for drawing radial lines
'
Sub radial(x As integer, y As integer, o As integer, sr As integer, i as integer ,lpos as integer)
Local integer x1, x2, y1, y2
local float s=Sin(Rad(sr))
local float c=-Cos(Rad(sr))
rx2(lpos)=s*o + x
ry2(lpos)=c*o + y
rx1(lpos)=s*i + x 'i is 0 if not specified so a complete line from the centre is drawn
ry1(lpos)=c*i + y
End Sub
'
' bound a line in a box defined by xmin,xmax,ymin,ymax
'
function CohenSutherlandLineClip(x0 as float, y0 as float, x1 as float, y1 as float) as integer
' compute outcodes for P0, P1, and whatever point lies outside the clip rectangle
local integer outcode0 = ComputeOutCode(x0, y0)
local integer outcode1 = ComputeOutCode(x1, y1)
local integer outcodeout
local float x,y
do while (1)
if (NOT(outcode0 OR outcode1)) then ' Bitwise OR is 0. Trivially accept and get out of loop
CohenSutherlandLineClip = 0
exit do
elseif (outcode0 AND outcode1) then 'Bitwise AND is not 0. Trivially reject and get out of loop
CohenSutherlandLineClip = (outcode0 AND outcode1)
exit do
else
' failed both tests, so calculate the line segment to clip
' from an outside point to an intersection with clip edge
' At least one endpoint is outside the clip rectangle; pick it.
if outcode0 then
outcodeOut = outcode0
else
outcodeOut = outcode1
endif
' Now find the intersection point;
' use formulas y = y0 + slope * (x - x0), x = x0 + (1 / slope) * (y - y0)
if (outcodeOut and C_TOP) then ' point is above the clip rectangle
x = x0 + (x1 - x0) * (ymax - y0) / (y1 - y0)
y = ymax
elseif (outcodeOut AND C_BOTTOM) then ' point is below the clip rectangle
x = x0 + (x1 - x0) * (ymin - y0) / (y1 - y0)
y = ymin
elseif (outcodeOut AND C_RIGHT) then ' point is to the right of clip rectangle
y = y0 + (y1 - y0) * (xmin - x0) / (x1 - x0)
x = xmax
elseif (outcodeOut AND C_LEFT) then' point is to the left of clip rectangle
y = y0 + (y1 - y0) * (xmax - x0) / (x1 - x0)
x = xmin
endif
' Now we move outside point to intersection point to clip
' and get ready for next pass.
if (outcodeOut = outcode0) then
x0 = x
y0 = y
outcode0 = ComputeOutCode(x0, y0)
else
x1 = x
y1 = y
outcode1 = ComputeOutCode(x1, y1)
endif
endif
loop
end function
'
function ComputeOutCode(x as float, y as float) as integer
ComputeOutCode = C_INSIDE ' initialised as being inside of clip window
if (x < xmin) then ' to the left of clip window
ComputeOutCode =ComputeOutCode OR C_LEFT
elseif (x > xmax) then ' to the right of clip window
ComputeOutCode =ComputeOutCode OR RIGHT
endif
if (y < ymin) then ' below the clip window
ComputeOutCode =ComputeOutCode OR C_BOTTOM
elseif (y > ymax) then ' above the clip window
ComputeOutCode =ComputeOutCode OR C_TOP
endif
end function
'
FUNCTION atan2(y as float,x as float) as float
IF x > 0 THEN
atan2 = ATN(y/x)
ELSEIF y >= 0 AND x < 0 THEN
atan2 = PI + ATN(y/x)
ELSEIF y < 0 AND x < 0 THEN
atan2 = ATN(y/x) - PI
ELSEIF y > 0 AND x = 0 THEN
atan2 = PI / 2
ELSEIF y < 0 AND x = 0 THEN
atan2 = PI / -2
ENDIF
IF atan2 < 0 THEN
atan2 = atan2 + 2 * PI
ENDIF
END FUNCTION
'
CSUB MadgwickQuaternionUpdate
000000DA
'atan2
27BDFFC8 AFBF0034 AFBE0030 AFB7002C AFB60028 AFB50024 AFB40020 AFB3001C
AFB20018 AFB10014 AFB00010 00809021 00A0A021 3C109D00 8E02009C 3C044049
0040F809 24840FDA 0040B021 8E02009C 3C043FC9 0040F809 24840FDA 0040B821
8E02009C 3C04BFC9 0040F809 24840FDA 0040F021 8E02009C 3C0440C9 0040F809
24840FDA 0040A821 8E02009C 0040F809 00002021 00408821 8E02009C 0040F809
00002021 00409821 8E020068 02802021 0040F809 00002821 24030001 1443000A
3C029D00 8C500078 8C420064 02402021 0040F809 02802821 0200F809 00402021
10000054 00409821 8C420068 02402021 0040F809 02202821 2403FFFF 10430015
3C029D00 8C420068 02802021 0040F809 02202821 2403FFFF 1443000E 3C029D00
8C53005C 8C500078 8C420064 02402021 0040F809 02802821 0200F809 00402021
02C02021 0260F809 00402821 10000039 00409821 8C420068 02402021 0040F809
02202821 2403FFFF 14430015 3C029D00 8C420068 02802021 0040F809 02202821
2403FFFF 1443000E 3C029D00 8C530060 8C500078 8C420064 02402021 0040F809
02802821 0200F809 00402021 00402021 0260F809 02C02821 1000001E 00409821
8C420068 02402021 0040F809 02202821 24030001 14430008 3C029D00 8C420068
02802021 0040F809 02202821 50400011 02E09821 3C029D00 8C420068 02402021
0040F809 02202821 2403FFFF 1443000A 3C029D00 8C420068 02802021 0040F809
02202821 50400003 03C09821 10000002 3C029D00 3C029D00 8C420068 02602021
0040F809 02202821 2403FFFF 14430008 02601021 3C029D00 8C42005C 02602021
0040F809 02A02821 00409821 02601021 8FBF0034 8FBE0030 8FB7002C 8FB60028
8FB50024 8FB40020 8FB3001C 8FB20018 8FB10014 8FB00010 03E00008 27BD0038
'asin
27BDFFC8 AFBF0034 AFB70030 AFB6002C AFB50028 AFB40024 AFB30020 AFB2001C
AFB10018 AFB00014 00808821 3C109D00 8E02009C 0040F809 3C043F00 0040A021
8E02009C 0040F809 3C043F80 00409021 8E170078 8E160064 8E150074 8E130060
8E020058 02202021 0040F809 02202821 02402021 0260F809 00402821 00402021
02A0F809 02802821 02202021 02C0F809 00402821 02E0F809 00402021 8FBF0034
8FB70030 8FB6002C 8FB50028 8FB40024 8FB30020 8FB2001C 8FB10018 8FB00014
03E00008 27BD0038
'main
27BDFF30 AFBF00CC AFBE00C8 AFB700C4 AFB600C0 AFB500BC AFB400B8 AFB300B4
AFB200B0 AFB100AC AFB000A8 0080B021 8FA200E4 40034800 8C440000 AFA40018
8C440004 14800007 00608021 54800007 AC500000 8FA80018 0068202B 50800003
AC500000 00608021 AC500000 AC400004 8ED40000 8ED70004 8ED20008 8ED3000C
8CBE0000 8CA20004 AFA20010 8CA50008 AFA50020 8CC30000 AFA30054 8CC40004
AFA40058 8CC60008 AFA6005C 8CE80000 AFA80028 8CE20004 AFA20030 8CE70008
AFA70024 3C119D00 8E22009C 0040F809 3C044000 0040A821 8E22009C 0040F809
3C043F00 AFA20038 8E22009C 0040F809 3C043F80 AFA20044 8E22009C 0040F809
00002021 AFA20040 8E22009C 0040F809 3C044080 AFA20050 8E22009C 3C044265
0040F809 24842EE2 AFA20078 8FA200E0 8C420000 AFA20060 8E230058 AFA3001C
8E240064 AFA40014 8FA80018 02088023 8E220080 02002021 0040F809 00102FC3
00408021 8E230000 8E220080 8C640000 0040F809 00002821 02002021 8FA30014
0060F809 00402821 00402021 8FA8001C 0100F809 02A02821 AFA20064 8E220058
02A02021 0040F809 02802821 AFA2006C 8E220058 02A02021 0040F809 02E02821
AFA20048 8E220058 02A02021 0040F809 02402821 AFA20068 8E220058 02A02021
0040F809 02602821 AFA20084 8E300058 02A02021 0200F809 02802821 00402021
0200F809 02402821 AFA20088 8E300058 02A02021 0200F809 02402821 00402021
0200F809 02602821 AFA2008C 8E220058 02802021 0040F809 02802821 AFA2004C
8E220058 02802021 0040F809 02E02821 AFA20070 8E220058 02802021 0040F809
02402821 AFA20074 8E220058 02802021 0040F809 02602821 AFA2007C 8E220058
02E02021 0040F809 02E02821 AFA2002C 8E220058 02E02021 0040F809 02402821
AFA20090 8E220058 02E02021 0040F809 02602821 AFA2003C 8E220058 02402021
0040F809 02402821 AFA20014 8E220058 02402021 0040F809 02602821 AFA20080
8E220058 02602021 0040F809 02602821 AFA20034 8E220074 AFA20018 8E30005C
8E220058 03C02021 0040F809 03C02821 AFA2001C 8E220058 8FA40010 0040F809
00802821 8FA4001C 0200F809 00402821 AFA2001C 8E220058 8FA40020 0040F809
00802821 8FA4001C 0200F809 00402821 00402021 8FA30018 0060F809 8FA50038
AFA20018 8E220068 8FA40018 0040F809 8FA50040 104004FD 8FBF00CC 3C109D00
8E020064 8FA40044 0040F809 8FA50018 00408821 8E020058 03C02021 0040F809
02202821 AFA2009C 8E020058 8FA40010 0040F809 02202821 AFA200A0 8E020058
8FA40020 0040F809 02202821 AFA200A4 8E040074 AFA40010 8E11005C 8E020058
8FA40028 0040F809 00802821 0040F021 8E020058 8FA40030 0040F809 00802821
03C02021 0220F809 00402821 0040F021 8E020058 8FA40024 0040F809 00802821
03C02021 0220F809 00402821 00402021 8FA80010 0100F809 8FA50038 0040F021
8E020068 03C02021 0040F809 8FA50040 104004C6 8FBF00CC 3C119D00 8E220064
8FA40044 0040F809 03C02821 00408021 8E220058 8FA40028 0040F809 02002821
AFA2001C 8E220058 8FA40030 0040F809 02002821 AFA20020 8E220058 8FA40024
0040F809 02002821 AFA20018 8E300058 02A02021 0200F809 02802821 00402021
0200F809 8FA5001C AFA20028 8E300058 02A02021 0200F809 02802821 00402021
0200F809 8FA50020 AFA20030 8E300058 02A02021 0200F809 02802821 00402021
0200F809 8FA50018 AFA20024 8E300058 02A02021 0200F809 02E02821 00402021
0200F809 8FA5001C AFA20094 8E3E005C 8E220060 AFA20010 8E220058 8FA4001C
0040F809 8FA5004C 00408021 8E220058 8FA40030 0040F809 02602821 02002021
8FA30010 0060F809 00402821 00408021 8E220058 8FA40024 0040F809 02402821
02002021 03C0F809 00402821 00408021 8E220058 8FA4001C 0040F809 8FA5002C
02002021 03C0F809 00402821 AFA20098 8E300058 8FA40048 0200F809 8FA50020
00402021 0200F809 02402821 8FA40098 03C0F809 00402821 AFA20098 8E300058
8FA40048 0200F809 8FA50018 00402021 0200F809 02602821 8FA40098 03C0F809
00402821 00408021 8E220058 8FA4001C 0040F809 8FA50014 02002021 8FA80010
0100F809 00402821 00408021 8E220058 8FA4001C 0040F809 8FA50034 02002021
8FA30010 0060F809 00402821 0040F021 8E220058 8FA40028 0040F809 02602821
00408021 8E24005C AFA40010 8E220058 8FA40020 0040F809 8FA5004C 02002021
8FA80010 0100F809 00402821 00408021 8E220060 AFA20010 8E220058 8FA40024
0040F809 02E02821 02002021 8FA30010 0060F809 00402821 00408021 8E24005C
AFA40010 8E220058 8FA40094 0040F809 02402821 02002021 8FA80010 0100F809
00402821 00408021 8E220060 AFA20010 8E220058 8FA40020 0040F809 8FA5002C
02002021 8FA30010 0060F809 00402821 00408021 8E24005C AFA40010 8E220058
8FA40020 0040F809 8FA50014 02002021 8FA80010 0100F809 00402821 AFA20010
8E22005C AFA20024 8E300058 8FA40068 0200F809 8FA50018 00402021 0200F809
02602821 8FA40010 8FA30024 0060F809 00402821 00408021 8E240060 AFA40010
8E220058 8FA40020 0040F809 8FA50034 02002021 8FA80010 0100F809 00402821
00408021 8E220074 AFA20010 8E23005C AFA30024 8E220058 03C02021 0040F809
03C02821 0040F021 8E220058 02002021 0040F809 02002821 03C02021 8FA80024
0100F809 00402821 00402021 8FA30010 0060F809 8FA50038 AFA20010 8E220058
8FA40030 0040F809 02E02821 00408021 8E3E0060 8E220058 8FA40028 0040F809
02402821 02002021 03C0F809 00402821 00408021 8E3E005C 8E220058 8FA40018
0040F809 8FA5004C 02002021 03C0F809 00402821 00408021 8E3E005C 8E220058
8FA40094 0040F809 02602821 02002021 03C0F809 00402821 00408021 8E3E0060
8E220058 8FA40018 0040F809 8FA5002C 02002021 03C0F809 00402821 0040F021
8E24005C AFA40028 8E300058 8FA40068 0200F809 8FA50020 00402021 0200F809
02602821 03C02021 8FA80028 0100F809 00402821 00408021 8E3E0060 8E220058
8FA40018 0040F809 8FA50014 02002021 03C0F809 00402821 00408021 8E3E005C
8E220058 8FA40018 0040F809 8FA50034 02002021 03C0F809 00402821 0040F021
8E220058 02A02021 0040F809 8FA50010 AFA2004C 8E220058 02A02021 0040F809
03C02821 AFA20094 8E220058 AFA20028 8E300060 8FA40038 0200F809 8FA5002C
00402021 0200F809 8FA50014 03C02021 8FA30028 0060F809 00402821 AFA20030
8E300058 8E22005C 8FA40070 0040F809 8FA50080 03C02021 0200F809 00402821
AFA20024 8E300058 8E220060 8FA4003C 0040F809 8FA50074 03C02021 0200F809
00402821 AFA20080 8E240058 AFA40028 8E300060 8FA40038 0200F809 8FA50014
00402021 0200F809 8FA50034 8FA40010 8FA80028 0100F809 00402821 AFA20034
8E300058 8E220060 8FA40090 0040F809 8FA5007C 8FA40010 0200F809 00402821
AFA2007C 8E300058 8E22005C 8FA40074 0040F809 8FA5003C 8FA40010 0200F809
00402821 AFA20074 8E300060 8E220058 02A02021 0040F809 8FA5003C 00402021
0200F809 8FA50088 00402021 0200F809 8FA5009C AFA20028 8E300060 8E22005C
8FA4007C 0040F809 8FA50024 00402021 0200F809 8FA50020 AFA20020 8E300060
8E22005C 8FA40074 0040F809 8FA50030 00402021 0200F809 8FA50018 AFA20018
8E300060 8E22005C 8FA40034 0040F809 8FA50080 00402021 0200F809 8FA5001C
AFA2001C 8E220060 AFA20030 8E30005C 8E220058 02A02021 0040F809 8FA50070
00402021 0200F809 8FA5008C 00402021 8FA30030 0060F809 8FA500A0 AFA20030
8E300060 8E220058 02A02021 0040F809 8FA5002C 8FA40044 0200F809 00402821
AFA20024 8E220058 02A02021 0040F809 8FA50014 8FA40024 0200F809 00402821
00402021 0200F809 8FA500A4 AFA20034 8E24005C AFA40014 8E280060 AFA80024
8E220058 8FA40048 0040F809 8FA50030 00408021 8E220058 8FA40068 0040F809
8FA50028 02002021 8FA30024 0060F809 00402821 AFA2002C 8E300058 03C02021
0200F809 02402821 00402021 0200F809 8FA5001C 8FA4002C 8FA80024 0100F809
00402821 AFA20024 8E220060 AFA2002C 8E220058 03C02021 0040F809 02E02821
AFA2003C 8E300058 8FA40010 0200F809 02602821 8FA4003C 8FA3002C 0060F809
00402821 8FA40020 0200F809 00402821 8FA40024 8FA80014 0100F809 00402821
AFA20024 8E300058 8FA40010 0200F809 02402821 00402021 0200F809 8FA50018
8FA40024 8FA30014 0060F809 00402821 AFA20024 8E240060 AFA4002C 8E28005C
AFA80014 8E220058 8FA40084 0040F809 8FA50028 00408021 8E220058 8FA4006C
0040F809 8FA50030 02002021 8FA30014 0060F809 00402821 AFA2003C 8E300058
8FA40050 0200F809 02E02821 00402021 0200F809 8FA50034 8FA4003C 8FA8002C
0100F809 00402821 AFA2002C 8E300058 03C02021 0200F809 02602821 00402021
0200F809 8FA5001C 8FA4002C 8FA30014 0060F809 00402821 AFA2002C 8E24005C
AFA40014 8E220058 8FA40010 0040F809 02402821 AFA2003C 8E300058 03C02021
0200F809 02802821 8FA4003C 8FA80014 0100F809 00402821 8FA40020 0200F809
00402821 8FA4002C 8FA30014 0060F809 00402821 AFA2002C 8E240060 AFA4003C
8E220058 8FA40010 0040F809 02602821 AFA20070 8E300058 8FA40094 0200F809
02E02821 8FA40070 8FA8003C 0100F809 00402821 8FA40018 0200F809 00402821
8FA4002C 8FA30014 0060F809 00402821 AFA2002C 8E24005C AFA4003C 8E280060
AFA80014 8E220058 8FA40084 0040F809 8FA50030 00408021 8E220058 8FA4006C
0040F809 8FA50028 02002021 8FA30014 0060F809 00402821 AFA2006C 8E300058
8FA40050 0200F809 02402821 00402021 0200F809 8FA50034 8FA4006C 8FA80014
0100F809 00402821 AFA20034 8E300058 8E220060 AFA20014 8FA40040 0040F809
8FA5004C 00402021 0200F809 02402821 AFA20050 8E300058 03C02021 0200F809
02802821 8FA40050 8FA30014 0060F809 00402821 8FA4001C 0200F809 00402821
8FA40034 8FA8003C 0100F809 00402821 AFA20034 8E22005C AFA20014 8E220058
8FA40010 0040F809 02E02821 AFA2003C 8E300058 03C02021 0200F809 02602821
8FA4003C 8FA30014 0060F809 00402821 8FA40020 0200F809 00402821 8FA40034
8FA80014 0100F809 00402821 AFA20034 8E220060 AFA2003C 8E220058 8FA40010
0040F809 02802821 AFA20050 8E300058 8FA40094 0200F809 02402821 8FA40050
8FA3003C 0060F809 00402821 8FA40018 0200F809 00402821 8FA40034 8FA80014
0100F809 00402821 AFA20034 8E22005C AFA20014 8E220058 8FA40048 0040F809
8FA50028 00408021 8E220058 8FA40068 0040F809 8FA50030 02002021 8FA30014
0060F809 00402821 AFA20028 8E240060 AFA40030 8E220058 03C02021 0040F809
02E02821 AFA20048 8E300058 8FA4004C 0200F809 02602821 8FA40048 8FA80030
0100F809 00402821 8FA4001C 0200F809 00402821 8FA40028 8FA30014 0060F809
00402821 AFA2001C 8E240060 AFA40028 8E220058 03C02021 0040F809 02402821
0040F021 8E300058 8FA40010 0200F809 02802821 03C02021 8FA80028 0100F809
00402821 8FA40020 0200F809 00402821 8FA4001C 8FA30014 0060F809 00402821
0040F021 8E300058 8FA40010 0200F809 02E02821 00402021 0200F809 8FA50018
03C02021 8FA80014 0100F809 00402821 0040F021 8E220074 AFA20010 8E30005C
8E220058 8FA40024 0040F809 00802821 AFA20020 8E220058 8FA4002C 0040F809
00802821 8FA40020 0200F809 00402821 AFA20020 8E220058 8FA40034 0040F809
00802821 8FA40020 0200F809 00402821 AFA20020 8E220058 03C02021 0040F809
03C02821 8FA40020 0200F809 00402821 00402021 8FA30010 0060F809 8FA50038
8E230064 8FA40044 0060F809 00402821 00408021 8E220058 8FA40024 0040F809
02002821 AFA20010 8E220058 8FA4002C 0040F809 02002821 AFA20020 8E220058
8FA40034 0040F809 02002821 AFA20018 8E220058 03C02021 0040F809 02002821
AFA2001C 3C029D00 8C500058 8E3E0060 8FA40040 03C0F809 02E02821 00402021
0200F809 8FA50054 00408021 8E220058 02402021 0040F809 8FA50058 02002021
03C0F809 00402821 AFA20014 8E300058 02602021 0200F809 8FA5005C 8FA40014
03C0F809 00402821 8FA40038 0200F809 00402821 00408021 8E220058 8FA40060
0040F809 8FA50010 02002021 03C0F809 00402821 AFA20010 8E3E0060 8E24005C
AFA40014 8E220058 02802021 0040F809 8FA50054 00408021 8E220058 02402021
0040F809 8FA5005C 02002021 8FA80014 0100F809 00402821 AFA20014 8E300058
02602021 0200F809 8FA50058 8FA40014 03C0F809 00402821 8FA40038 0200F809
00402821 00408021 8E220058 8FA40060 0040F809 8FA50020 02002021 03C0F809
00402821 AFA20020 8E22005C AFA20014 8E3E0060 8E220058 02802021 0040F809
8FA50058 00408021 8E220058 02E02021 0040F809 8FA5005C 02002021 03C0F809
00402821 AFA20028 8E300058 02602021 0200F809 8FA50054 8FA40028 8FA30014
0060F809 00402821 8FA40038 0200F809 00402821 00408021 8E220058 8FA40060
0040F809 8FA50018 02002021 03C0F809 00402821 AFA20018 8E3E0060 8E24005C
AFA40014 8E220058 02802021 0040F809 8FA5005C 00408021 8E220058 02E02021
0040F809 8FA50058 02002021 8FA80014 0100F809 00402821 AFA20014 8E300058
02402021 0200F809 8FA50054 8FA40014 03C0F809 00402821 8FA40038 0200F809
00402821 00408021 8E220058 8FA40060 0040F809 8FA5001C 02002021 03C0F809
00402821 AFA2001C 8E30005C 8E220058 8FA40010 0040F809 8FA50064 02802021
0200F809 00402821 0040F021 8E30005C 8E220058 8FA40020 0040F809 8FA50064
02E02021 0200F809 00402821 0040B821 8E30005C 8E220058 8FA40018 0040F809
8FA50064 02402021 0200F809 00402821 0040A021 8E30005C 8E220058 8FA4001C
0040F809 8FA50064 02602021 0200F809 00402821 00409021 8E220074 AFA20010
8E30005C 8E220058 03C02021 0040F809 03C02821 00409821 8E220058 02E02021
0040F809 02E02821 02602021 0200F809 00402821 00409821 8E220058 02802021
0040F809 02802821 02602021 0200F809 00402821 00409821 8E220058 02402021
0040F809 02402821 02602021 0200F809 00402821 00402021 8FA30010 0060F809
8FA50038 8E230064 8FA40044 0060F809 00402821 00408021 8E220058 03C02021
0040F809 02002821 AEC20000 8E220058 02E02021 0040F809 02002821 AEC20004
8E220058 02802021 0040F809 02002821 AEC20008 8E220058 02402021 0040F809
02002821 AEC2000C 8E33005C 8E220058 8EC40004 0040F809 8EC50008 00409021
8E300058 8EC40000 0200F809 8EC5000C 02402021 0260F809 00402821 02A02021
0200F809 00402821 0040A021 8E300060 8E33005C 8EC50000 8E220058 0040F809
00A02021 00409021 8EC50004 8E220058 0040F809 00A02021 02402021 0260F809
00402821 00409021 8EC50008 8E220058 0040F809 00A02021 02402021 0200F809
00402821 00409821 8E320058 8EC5000C 0240F809 00A02021 02602021 0200F809
00402821 02802021 0411F9A3 00402821 8FA40078 0240F809 00402821 8FA300E8
AC620000 8E320060 8E220058 8EC40004 0040F809 8EC5000C 00409821 8E300058
8EC40000 0200F809 8EC50008 02602021 0240F809 00402821 02A02021 0200F809
00402821 0411FA34 00402021 8FA40040 0240F809 00402821 8FA40078 0200F809
00402821 8FA300EC AC620000 8E33005C 8E220058 8EC40000 0040F809 8EC50004
00409021 8E300058 8EC40008 0200F809 8EC5000C 02402021 0260F809 00402821
02A02021 0200F809 00402821 0040A021 8E33005C 8E300060 8EC50000 8E220058
0040F809 00A02021 00409021 8EC50004 8E220058 0040F809 00A02021 02402021
0200F809 00402821 00409021 8EC50008 8E220058 0040F809 00A02021 02402021
0200F809 00402821 00409021 8E300058 8EC5000C 0200F809 00A02021 02402021
0260F809 00402821 02802021 0411F952 00402821 8FA40078 0200F809 00402821
8FA300F0 AC620000 8FBF00CC 8FBE00C8 8FB700C4 8FB600C0 8FB500BC 8FB400B8
8FB300B4 8FB200B0 8FB100AC 8FB000A8 03E00008 27BD00D0

End CSUB

Csub triangles 'draws multiple triangles with a delay between if required
00000000
8C820004 27BDFFA0 AFBF005C AFBE0058 AFB70054 AFB60050 AFB5004C AFB40048
AFB30044 AFB20040 AFB1003C AFB00038 AFA40060 AFA50064 AFA60068 184000EE
AFA7006C AFA00030 3C159D00 8FA30030 8FA50068 8FA80070 000310C0 00A22021
01021821 8C840000 8C630000 8FA90064 AFA40018 01223821 AFA3001C 8FA40074
8FA3006C 8FA80078 8FA9007C 00623021 00822821 01022021 01221021 8FA80018
8FA9001C 8CC60000 8CA50000 0128182A 8CF30000 8C9E0000 AFA60028 AFA5002C
10600006 8C540000 02601021 AFA90018 00C09821 AFA8001C AFA20028 8FA5001C
03C5102A 10400008 8FA90018 8FA20028 8FA8002C AFBE001C AFA80028 00A0F021
AFA2002C 8FA90018 8FA3001C 0069102A 10400008 8FA50018 8FA4001C 02601021
AFA40018 8FB30028 AFA9001C AFA20028 8FA50018 10BE008C 8FA4001C 109E00A4
8FA50018 2483FFFF 0065102A 1440002E 8FB00018 8FA90028 8FA2002C 8FA8001C
01334823 00531023 03C5B023 AFBE0034 0105B823 0280F021 AFA90020 AFA20024
00A08021 00009021 00008821 0060A021 0237001A 02E001F4 8FA30024 8FA20020
02002821 02003821 02228821 26100001 00002012 0256001A 02C001F4 00932021
02439021 00003012 00D33021 00C4182A 10600003 00801021 00C02021 00403021
8EA20048 AFBE0010 8C420000 0040F809 00000000 0290102A 1040FFE5 00000000
03C0A021 8FBE0034 03D0102A 14400030 8FA8001C 8FA90018 8FA4002C 8FA50028
0093B023 0085B823 02089023 02098823 03C91823 03C81023 72579002 72368802
AFB40018 AFB6001C AFB70020 0260B021 00A0B821 00609821 0040A021 0254001A
028001F4 8FA3001C 8FA20020 02002821 02003821 02429021 26100001 00002012
0233001A 026001F4 00972021 02238821 00003012 00D63021 00C4182A 10600003
00801021 00C02021 00403021 8FA80018 8EA20048 AFA80010 8C420000 0040F809
00000000 03D0102A 1040FFE4 00000000 8FA90080 8D240000 8D220004 00821025
5440001D 8EA20004 8FA20030 8FA40060 24420001 8C830004 AFA20030 000217C3
0043202A 5480FF52 8FA30030 14620006 8FA50060 8FA80030 8CA20000 0102102B
1440FF4B 8FA30030 8FBF005C 8FBE0058 8FB70054 8FB60050 8FB5004C 8FB40048
8FB30044 8FB20040 8FB1003C 8FB00038 03E00008 27BD0060 0040F809 00000000
1000FFE2 8FA20030 8FA80028 0113102A 1440001D 02603821 0268102A 1440001C
8FA70028 02603821 02602021 8FA9002C 0124102A 54400003 8FA4002C 00E9102A
0122380B 8EA20048 8FA30018 AFB40010 00E33821 8C420000 00602821 00803021
0040F809 00E43823 1000FFC2 8FA90080 8FA3001C 0065102A 1040FF5F 8FA90028
1000FF89 8FB00018 1000FFE8 8FA40028 1000FFE6 02602021 1440FFCC 8FBF005C
8C820000 5440FF10 AFA00030 1000FFC8 8FBE0058
End Csub
'
CSub dcirch
00000000
27BDFFA0 AFB1003C AFB00038 AFBF005C AFBE0058 AFB70054 AFB60050 AFB5004C
AFB40048 AFB30044 AFB20040 8C840000 8FB00070 8FB10078 AFA4002C 8E020000
8CA50000 8CD20000 0082102A 8CF40000 14400010 AFA50018 8E220000 0044102A
1440000D 3C029D00 8FA20074 8C430000 02451021 0043182A 54600007 3C029D00
8FA4007C 8C830000 0062102A 10400015 8FA20074 3C029D00 8C420048 8FA30018
8FA4002C AFB40010 8C420000 02433821 00803021 0040F809 00E02821 8E020000
8FA4002C 0082102A 14400012 3C029D00 8E220000 0044102A 1440000E 3C029D00
8FA20074 8FA40018 8C430000 00921023 0043182A 54600007 3C029D00 8FA4007C
8C830000 0062102A 1040000C 8FA4002C 3C029D00 8C420048 8FA30018 AFB40010
8FA4002C 8C420000 00723823 00E02821 0040F809 00803021 8FA4002C 8E020000
02443021 00C2102A 14400011 3C029D00 8E220000 0046102A 1440000D 3C029D00
8FA30074 8FA40018 8C620000 0082102A 14400007 3C029D00 8FA3007C 8C620000
0044102A 1040000A 8FA4002C 3C029D00 8C420048 8FA50018 AFB40010 8C420000
00C02021 0040F809 00A03821 8FA4002C 8E020000 00923023 00C2102A 14400010
3C029D00 8E220000 0046102A 1440000C 3C029D00 8FA30074 8FA40018 8C620000
0082102A 14400006 3C029D00 8FA3007C 8C620000 0044102A 10400008 3C029D00
8C420048 8FA50018 AFB40010 8C420000 00C02021 0040F809 00A03821 1A40010F
24030001 00721823 00121023 00021040 AFA30024 8FA4002C 8FA3002C AFA20030
8FA20018 2484FFFF 24630001 AFA40020 2457FFFF 245E0001 AFA3001C AFA00028
3C159D00 8FA40024 04800006 8FA20030 2652FFFF 24420002 00822021 AFA20030
AFA40024 8E020000 8FA30028 8FB6001C 24630001 02C2102A 14400011 AFA30028
8E220000 0056102A 5440000E 8EA20048 8FA40074 8C830000 8FA40018 02441021
0043182A 54600007 8EA20048 8FA4007C 8C830000 0062102A 1040000B 8FA4002C
8EA20048 8FA30018 AFB40010 8C420000 02433821 02C02021 00E02821 0040F809
02C03021 8FA4002C 8E020000 02449821 0262102A 54400010 8EA20048 8E220000
0053102A 5440000C 8EA20048 8FA30074 8C620000 03C2102A 54400007 8EA20048
8FA4007C 8C820000 005E102A 5040000A 8E020000 8EA20048 AFB40010 02602021
8C420000 03C02821 02603021 0040F809 03C03821 8E020000 02C2102A 54400012
8EA20048 8E220000 0056102A 5440000E 8EA20048 8FA20074 8FA40018 8C430000
00921023 0043182A 54600007 8EA20048 8FA4007C 8C830000 0062102A 5040000B
8E020000 8EA20048 8FA30018 AFB40010 8C420000 00723823 02C02021 00E02821
0040F809 02C03021 8E020000 0262102A 54400010 8EA20048 8E220000 0053102A
5440000C 8EA20048 8FA40074 8C820000 02E2102A 54400007 8EA20048 8FA3007C
8C620000 0057102A 1040000A 8FA4002C 8EA20048 AFB40010 02602021 8C420000
02E02821 02603021 0040F809 02E03821 8FA4002C 8E020000 00929823 0262102A
54400010 8EA20048 8E220000 0053102A 5440000C 8EA20048 8FA30074 8C620000
03C2102A 54400007 8EA20048 8FA4007C 8C820000 005E102A 5040000A 8E020000
8EA20048 AFB40010 02602021 8C420000 03C02821 02603021 0040F809 03C03821
8E020000 8FB60020 02C2102A 54400012 8EA20048 8E220000 0056102A 5440000E
8EA20048 8FA20074 8FA40018 8C430000 02441021 0043182A 54600007 8EA20048
8FA4007C 8C830000 0062102A 5040000B 8E020000 8EA20048 8FA30018 AFB40010
8C420000 02433821 02C02021 00E02821 0040F809 02C03021 8E020000 0262102A
54400010 8EA20048 8E220000 0053102A 5440000C 8EA20048 8FA40074 8C820000
02E2102A 54400007 8EA20048 8FA3007C 8C620000 0057102A 5040000A 8E020000
8EA20048 AFB40010 02602021 8C420000 02E02821 02603021 0040F809 02E03821
8E020000 02C2102A 54400012 8EA20048 8E220000 0056102A 5440000E 8EA20048
8FA40074 8C830000 8FA40018 00921023 0043182A 54600007 8EA20048 8FA4007C
8C830000 0062102A 1040000B 8FA40028 8EA20048 8FA30018 AFB40010 8C420000
00723823 02C02021 00E02821 0040F809 02C03021 8FA40028 8FA30020 26F7FFFF
0092102A 8FA4001C 2463FFFF AFA30020 24840001 27DE0001 10400008 AFA4001C
8FA30028 8FA40024 00031040 24420001 00822021 1000FF03 AFA40024 8FBF005C
8FBE0058 8FB70054 8FB60050 8FB5004C 8FB40048 8FB30044 8FB20040 8FB1003C
8FB00038 03E00008 27BD0060
End CSub
'
Csub lines
00000000
27BDFFC0 8FA30058 AFB70034 AFB60030 AFB5002C AFB20020 AFBF003C AFBE0038
AFB40028 AFB30024 AFB1001C AFB00018 8C730000 8C820004 00809021 2673FFFF
00131900 00139A00 00A0B821 00C0B021 00E0A821 1C400006 00739821 14400028
8FBF003C 8C820000 10400026 8FBE0038 00008021 00008821 3C149D00 241E0001
8FA30054 8FA70050 02F02021 00701021 8C420000 00F01821 02D02821 02B03021
8C670000 8CA50000 8C840000 8CC60000 AFBE0010 AFA20014 8E820050 02652821
0040F809 02673821 8E430004 26310001 001117C3 0043182A 1460FFE9 26100008
8E430004 14620006 8FBF003C 8E420000 0222202B 1480FFE3 8FA30054 8FBF003C
8FBE0038 8FB70034 8FB60030 8FB5002C 8FB40028 8FB30024 8FB20020 8FB1001C
8FB00018 03E00008 27BD0040
End Csub
'

Option explicit
option default NONE
const pivot_x=200 'specify where the centre of the dial is located
const pivot_y=200
const radius=177 'specify the radius of the dial
const side_length=40 'specify the size of the equilateral triangle pointer
' define the points that make up the triangle relative to a pivot at 0,0
const x0=0
const x1=-side_length/2
const x2=side_length/2
const y0=-radius
const y1=-radius+side_length*cos(rad(30))
const y2=y1
'
dim integer angle, X, Y, W, H
dim integer xx0,xx1,xx2,yy0,yy1,yy2 'variables holding the vertices of the triangle relative to its pivot
dim integer pxx0,pxx1,pxx2,pyy0,pyy1,pyy2 'variables holding the absolute vertices of the triangle
cls
load image "tiger800" 'get a background of some sort
circle pivot_x,pivot_y,radius+1 'draw the rim of the dial
' move the pointer in circles forever
do
for angle = 0 to 359
moveto(angle)
pause 100
NEXT ANGLE
loop
end

sub moveto(angle as integer)
local integer xs=x, ys=y, ws=w, hs=h
rotatetriangle(angle,0,radius,x0,y0,x1,y1,x2,y2)
pxx0=xx0+pivot_x
pyy0=yy0+pivot_y-radius
pxx1=xx1+pivot_x
pyy1=yy1+pivot_y-radius
pxx2=xx2+pivot_x
pyy2=yy2+pivot_y-radius
getlimits(x, y, w, h)
' blit write and blit close first time in will error so skip
ON ERROR SKIP 2
BLIT WRITE #1, xs, ys, ws, hs 'restore the save background
blit close #1
blit read #1, x, y, w, h
triangle pxx0, pyy0, pxx1, pyy1, pxx2, pyy2, rgb(red), rgb(blue)
end sub

sub getlimits(x as integer, y as integer, w as integer, h as integer)
Local integer i,max_x=0,min_x=MM.HRES,max_y=0,min_y=MM.VRES
if(pxx0(i)>max_x)then max_x=pxx0(i)
if(pxx1(i)>max_x)then max_x=pxx1(i)
if(pxx2(i)>max_x)then max_x=pxx2(i)
if(pxx0(i)<min_x)then min_x=pxx0(i)
if(pxx1(i)<min_x)then min_x=pxx1(i)
if(pxx2(i)<min_x)then min_x=pxx2(i)
if(pyy0(i)>max_y)then max_y=pyy0(i)
if(pyy1(i)>max_y)then max_y=pyy1(i)
if(pyy2(i)>max_y)then max_y=pyy2(i)
if(pyy0(i)<min_y)then min_y=pyy0(i)
if(pyy1(i)<min_y)then min_y=pyy1(i)
if(pyy2(i)<min_y)then min_y=pyy2(i)
x=min_x
y=min_y
w=max_x-min_x+2
h=max_y-min_y+2
end sub
'
' Simple trig to rotate the vertices of a triangle
' specified as x0,y0,x1,y1,x2,x2 relative to coordinate 0,0
' by the angle specified
' and then translate them about the supplied real centre x,y
' the calculated coordinates are then placed into element n of a set of coordinate arrays
'
sub rotatetriangle(angle as float, x as integer, y as integer, x0 as integer, y0 as integer, x1 as integer, y1 as integer, x2 as integer, y2 as integer)
local float sine=sin(rad(angle)),cosine=cos(rad(angle))
local integer x0a,y0a,x1a,y1a,x2a,y2a
x0a= x0*cosine - y0 * sine + x
y0a= y0*cosine + x0 * sine + y
x1a= x1*cosine - y1 * sine + x
y1a= y1*cosine + x1 * sine + y
x2a= x2*cosine - y2 * sine + x
y2a= y2*cosine + x2 * sine + y
xx0=x0a
yy0=y0a
xx1=x1a
yy1=y1a
xx2=x2a
yy2=y2a
end sub