Problems heading north - GameCreators Forum

Author

Message

wuergezwerg

5

Years of Service

User Offline

Joined: 30th Jul 2018

Location: Germany

Posted: 20th Feb 2020 20:12

Link

Hello,

I am trying to code a compass and have a problem when the needle hits the 360°/0° border.

I have already found the only other AppGameKit compass (https://forum.thegamecreators.com/thread/212325) but I wanted to have the tilt compensation

After searching for hours I found some formulas (https://ozzmaker.com/compass1/) but now I am stuck. When hitting 360° the compass turns around until it reaches 0° and then jumps around when pointing north.

Any ideas how to get it to work ?

Thanks in advance!

+ Code Snippet

arrowPNG = LoadImage("arrow.png")
arrow = CreateSprite(arrowPNG)
SetSpriteSize(arrow,30,-1)
SetSpritePosition(arrow,50-(GetSpriteWidth(arrow)/2),50-(GetSpriteHeight(arrow)/2))

do
	//GetPhoneAngle()
	heading = CompassHeading(1,1,0,0.05)
	Print(str(heading)+"°")
	SetSpriteAngle(arrow,-heading)
	if ( GetPointerPressed ( ) = 1 )
        sprite = GetSpriteHit ( GetPointerX ( ), GetPointerY ( ) )
        if sprite=arrow
        		CalibrateCompass()
        endif
    endif
Sync()
loop

Compass code:

type XYZ
	X as float
	Y as float
	Z as float
endtype

type XY
	X as float
	Y as float
endtype

type CompassData
	Accelerometer as integer //Accelerometer exists
	MagneticSensor as integer //Magnetic sensor exists
	//Magnetometer RAW
	Mag as XYZ
	//Tilt Compensation
	Acc as XYZ
	AccNorm as XY
	PhonePitch as float
	PhoneRoll as float
	//MinMax for Calibration
	MagMin as XYZ
	MagMax as XYZ
	//Calculated Values
	Result as XYZ
	//Previous value
	lastvalue as float
endtype

global Data as CompassData

Data.Accelerometer = GetAccelerometerExists()
Data.MagneticSensor = GetMagneticSensorExists()
GetCalibrationDataExists()

//Check if saved MinMax data exists and set values
function GetCalibrationDataExists()
	if GetFileExists("CompassCal.data") = 1
		read = OpenToRead("CompassCal.data")
		Data.MagMin.X = ReadFloat(read)
		Data.MagMin.Y = ReadFloat(read)
		Data.MagMin.Z = ReadFloat(read)
		Data.MagMax.X = ReadFloat(read)
		Data.MagMax.Y = ReadFloat(read)
		Data.MagMax.Z = ReadFloat(read)
		CloseFile(read)
	else
		CalibrateCompass()
	endif
endfunction

function CalibrateCompass()
		write = OpenToWrite("CompassCal.data")
		Data.MagMin.X = 0.0
		Data.MagMin.Y = 0.0
		Data.MagMin.Z = 0.0
		Data.MagMax.X = 0.0
		Data.MagMax.Y = 0.0
		Data.MagMax.Z = 0.0
		timer = GetSeconds()
		while seconds <= 10
			Print("Calibrate now")
			seconds = GetSeconds() - timer
			Print(str(seconds))
			GetSensorData("mag")
			GetMinMaxData()
			sync()
		endwhile
		WriteFloat(write,Data.MagMin.X)
		WriteFloat(write,Data.MagMin.Y)
		WriteFloat(write,Data.MagMin.Z)
		WriteFloat(write,Data.MagMax.X)
		WriteFloat(write,Data.MagMax.Y)
		WriteFloat(write,Data.MagMax.Z)
		CloseFile(write)
endfunction

function GetSensorData(sensor as string)
	select sensor
		case "mag":
			if Data.MagneticSensor = 1
				Data.Mag.X = GetRawMagneticX()
				Data.Mag.Y = -GetRawMagneticY()
				Data.Mag.Z = GetRawMagneticZ()
				//Set initial values (needed for raw calculation)
				Data.Result.X = Data.Mag.X
				Data.Result.Y = Data.Mag.Y
				Data.Result.Z = Data.Mag.Z
			else
				Data.Result.X = 0
				Data.Result.Y = 0
				Data.Result.Z = 0				
			endif
		endcase
		case "acc":
			if Data.Accelerometer = 1
				Data.Acc.X = GetRawAccelX()
				Data.Acc.Y = GetRawAccelY()
				Data.Acc.Z = GetRawAccelZ()
			endif
		endcase
	endselect
endfunction

function GetMinMaxData()
	if Data.Mag.X > Data.MagMax.X then Data.MagMax.X = Data.Mag.X
	if Data.Mag.Y > Data.MagMax.Y then Data.MagMax.Y = Data.Mag.Y
	if Data.Mag.Z > Data.MagMax.Z then Data.MagMax.Z = Data.Mag.Z
	if Data.Mag.X < Data.MagMin.X then Data.MagMin.X = Data.Mag.X
	if Data.Mag.Y < Data.MagMin.Y then Data.MagMin.Y = Data.Mag.Y
	if Data.Mag.Z < Data.MagMin.Z then Data.MagMin.Z = Data.Mag.Z
endfunction

function CalcHeading(smoothness as float)
	offset = 90
	heading = ATanFull(Data.Result.Y,Data.Result.X)+offset
	if(heading > 360)
		heading = heading-360
	endif
	heading = SmoothValues(heading,smoothness)
endfunction heading

function SmoothValues(newvalue as float,smoothness as float)
	diff# = (newvalue - Data.lastvalue) * smoothness
	value = Data.lastvalue + diff#
	Data.lastvalue = value
endfunction value

function CompassHeading(compensate,hardiron,softiron,smoothness as float)
	GetSensorData("mag")
	//Compensate Tilt
	if compensate = 1 and Data.Accelerometer = 1
		GetSensorData("acc")
		
		Data.AccNorm.X = Data.Acc.X/sqrt(Data.Acc.X * Data.Acc.X + Data.Acc.Y * Data.Acc.Y + Data.Acc.Z * Data.Acc.Z)
		Data.AccNorm.Y = Data.Acc.Y/sqrt(Data.Acc.X * Data.Acc.X + Data.Acc.Y * Data.Acc.Y + Data.Acc.Z * Data.Acc.Z)
		
		Data.PhonePitch = asin(Data.AccNorm.X)
		Data.PhoneRoll = asin(Data.AccNorm.Y/cos(Data.PhonePitch))
	
		Data.Result.X = (Data.Mag.X*cos(Data.PhonePitch))+(Data.Mag.Z*sin(Data.PhonePitch))
		Data.Result.Y = (Data.Mag.X*sin(Data.PhoneRoll)*sin(Data.PhonePitch)) + (Data.Mag.Y*cos(Data.PhoneRoll)+Data.Mag.Z*sin(Data.PhoneRoll)*cos(Data.PhonePitch))
	endif
	//Setting Hard Iron Offset
	if hardiron = 1
		Data.Result.X = Data.Result.X - (Data.MagMin.X + Data.MagMax.X)/2
		Data.Result.Y = Data.Result.Y - (Data.MagMin.Y + Data.MagMax.Y)/2
		Data.Result.Z = Data.Result.Z - (Data.MagMin.Z + Data.MagMax.Z)/2
	endif
	//Setting Soft Iron Offset
	if softiron = 1
		Data.Result.X = ((Data.Result.X - Data.MagMin.X) / (Data.MagMax.X - Data.MagMin.X) * 2) -1
		Data.Result.Y = ((Data.Result.Y - Data.MagMin.Y) / (Data.MagMax.Y - Data.MagMin.Y) * 2) -1
		Data.Result.Z = ((Data.Result.Z - Data.MagMin.Z) / (Data.MagMax.Z - Data.MagMin.Z) * 2) -1
	endif
	//Calculate heading
	heading = CalcHeading(smoothness)
endfunction heading

+ Code Snippet

type XYZ
	X as float
	Y as float
	Z as float
endtype

type XY
	X as float
	Y as float
endtype

type CompassData
	Accelerometer as integer //Accelerometer exists
	MagneticSensor as integer //Magnetic sensor exists
	//Magnetometer RAW
	Mag as XYZ
	//Tilt Compensation
	Acc as XYZ
	AccNorm as XY
	PhonePitch as float
	PhoneRoll as float
	//MinMax for Calibration
	MagMin as XYZ
	MagMax as XYZ
	//Calculated Values
	Result as XYZ
	//Previous value
	lastvalue as float
endtype

global Data as CompassData

Data.Accelerometer = GetAccelerometerExists()
Data.MagneticSensor = GetMagneticSensorExists()
GetCalibrationDataExists()

//Check if saved MinMax data exists and set values
function GetCalibrationDataExists()
	if GetFileExists("CompassCal.data") = 1
		read = OpenToRead("CompassCal.data")
		Data.MagMin.X = ReadFloat(read)
		Data.MagMin.Y = ReadFloat(read)
		Data.MagMin.Z = ReadFloat(read)
		Data.MagMax.X = ReadFloat(read)
		Data.MagMax.Y = ReadFloat(read)
		Data.MagMax.Z = ReadFloat(read)
		CloseFile(read)
	else
		CalibrateCompass()
	endif
endfunction

function CalibrateCompass()
		write = OpenToWrite("CompassCal.data")
		Data.MagMin.X = 0.0
		Data.MagMin.Y = 0.0
		Data.MagMin.Z = 0.0
		Data.MagMax.X = 0.0
		Data.MagMax.Y = 0.0
		Data.MagMax.Z = 0.0
		timer = GetSeconds()
		while seconds <= 10
			Print("Calibrate now")
			seconds = GetSeconds() - timer
			Print(str(seconds))
			GetSensorData("mag")
			GetMinMaxData()
			sync()
		endwhile
		WriteFloat(write,Data.MagMin.X)
		WriteFloat(write,Data.MagMin.Y)
		WriteFloat(write,Data.MagMin.Z)
		WriteFloat(write,Data.MagMax.X)
		WriteFloat(write,Data.MagMax.Y)
		WriteFloat(write,Data.MagMax.Z)
		CloseFile(write)
endfunction

function GetSensorData(sensor as string)
	select sensor
		case "mag":
			if Data.MagneticSensor = 1
				Data.Mag.X = GetRawMagneticX()
				Data.Mag.Y = -GetRawMagneticY()
				Data.Mag.Z = GetRawMagneticZ()
				//Set initial values (needed for raw calculation)
				Data.Result.X = Data.Mag.X
				Data.Result.Y = Data.Mag.Y
				Data.Result.Z = Data.Mag.Z
			else
				Data.Result.X = 0
				Data.Result.Y = 0
				Data.Result.Z = 0				
			endif
		endcase
		case "acc":
			if Data.Accelerometer = 1
				Data.Acc.X = GetRawAccelX()
				Data.Acc.Y = GetRawAccelY()
				Data.Acc.Z = GetRawAccelZ()
			endif
		endcase
	endselect
endfunction

function GetMinMaxData()
	if Data.Mag.X > Data.MagMax.X then Data.MagMax.X = Data.Mag.X
	if Data.Mag.Y > Data.MagMax.Y then Data.MagMax.Y = Data.Mag.Y
	if Data.Mag.Z > Data.MagMax.Z then Data.MagMax.Z = Data.Mag.Z
	if Data.Mag.X < Data.MagMin.X then Data.MagMin.X = Data.Mag.X
	if Data.Mag.Y < Data.MagMin.Y then Data.MagMin.Y = Data.Mag.Y
	if Data.Mag.Z < Data.MagMin.Z then Data.MagMin.Z = Data.Mag.Z
endfunction

function CalcHeading(smoothness as float)
	offset = 90
	heading = ATanFull(Data.Result.Y,Data.Result.X)+offset
	if(heading > 360)
		heading = heading-360
	endif
	heading = SmoothValues(heading,smoothness)
endfunction heading

function SmoothValues(newvalue as float,smoothness as float)
	diff# = (newvalue - Data.lastvalue) * smoothness
	value = Data.lastvalue + diff#
	Data.lastvalue = value
endfunction value

function CompassHeading(compensate,hardiron,softiron,smoothness as float)
	GetSensorData("mag")
	//Compensate Tilt
	if compensate = 1 and Data.Accelerometer = 1
		GetSensorData("acc")
		
		Data.AccNorm.X = Data.Acc.X/sqrt(Data.Acc.X * Data.Acc.X + Data.Acc.Y * Data.Acc.Y + Data.Acc.Z * Data.Acc.Z)
		Data.AccNorm.Y = Data.Acc.Y/sqrt(Data.Acc.X * Data.Acc.X + Data.Acc.Y * Data.Acc.Y + Data.Acc.Z * Data.Acc.Z)
		
		Data.PhonePitch = asin(Data.AccNorm.X)
		Data.PhoneRoll = asin(Data.AccNorm.Y/cos(Data.PhonePitch))
	
		Data.Result.X = (Data.Mag.X*cos(Data.PhonePitch))+(Data.Mag.Z*sin(Data.PhonePitch))
		Data.Result.Y = (Data.Mag.X*sin(Data.PhoneRoll)*sin(Data.PhonePitch)) + (Data.Mag.Y*cos(Data.PhoneRoll)+Data.Mag.Z*sin(Data.PhoneRoll)*cos(Data.PhonePitch))
	endif
	//Setting Hard Iron Offset
	if hardiron = 1
		Data.Result.X = Data.Result.X - (Data.MagMin.X + Data.MagMax.X)/2
		Data.Result.Y = Data.Result.Y - (Data.MagMin.Y + Data.MagMax.Y)/2
		Data.Result.Z = Data.Result.Z - (Data.MagMin.Z + Data.MagMax.Z)/2
	endif
	//Setting Soft Iron Offset
	if softiron = 1
		Data.Result.X = ((Data.Result.X - Data.MagMin.X) / (Data.MagMax.X - Data.MagMin.X) * 2) -1
		Data.Result.Y = ((Data.Result.Y - Data.MagMin.Y) / (Data.MagMax.Y - Data.MagMin.Y) * 2) -1
		Data.Result.Z = ((Data.Result.Z - Data.MagMin.Z) / (Data.MagMax.Z - Data.MagMin.Z) * 2) -1
	endif
	//Calculate heading
	heading = CalcHeading(smoothness)
endfunction heading

Back to top

Profile PM Website

Raven

19

Years of Service

User Offline

Joined: 23rd Mar 2005

Location: Hertfordshire, England

Posted: 21st Feb 2020 02:53

Link

function CalcHeading(smoothness as float)
offset = 90
heading = ATanFull(Data.Result.Y,Data.Result.X)+offset
if(heading > 360)
heading = heading-360
endif
heading = SmoothValues(heading,smoothness)
endfunction heading

...
This section is the problem.
Remember that the Value Range is 0 to 359 Degrees... as 0 Degrees and 360 Degrees are the SAME Value.

Dark BASIC Professional used to actually have a Helper Function Specifically for Angles (called WrapValue) but basically it's just this function:

+ Code Snippet

Function WrapAngle( Angle As Integer )
	Local Output As Integer
	Output = Abs(Round(WrapValue( Angle, 0.0, 359.99 )))
EndFunction Output

Function WrapValue( Input As Float, Min As Float, Max As Float )
	Local Output As Float
	
	If Input < Min 
		Output = Max - FMod(Min - Input, Max - Min)
	Else
		Output = Min + FMod(Input - Min, Max - Min)
	EndIf
EndFunction Output

That should do the trick. ^_^

Back to top

Profile PM Email Website

wuergezwerg

5

Years of Service

User Offline

Joined: 30th Jul 2018

Location: Germany

Posted: 21st Feb 2020 19:30

Link

Thank you for your answer.

The result of the WrapAngle function is the same as my original code. As ATanFull returns an angle between 0 and 360 I only need to subtract if my offset pushes the value above 360 afterwards.

The problem comes from my attempt to smoothen the movement of the needle. As the magnetic sensor is a little "nervous" I need to average the value in some way.
Both versions set the angle correctly to 0° when reaching 360°, but to reach that angle the sprite turns 360° counter clockwise. As calculated values are somewhere between +5/-5 degrees the sprite turns full circles in both directions while pointing north.

Back to top

Profile PM Website

Scraggle

Moderator

20

Years of Service

User Offline

Joined: 10th Jul 2003

Location: Yorkshire

Posted: 21st Feb 2020 22:42

Link

Why bother resetting the angle at all?
It would be a lot easier to just let it rotate freely. So when it goes beyond 360, you let it happen. The sprite will still point in the same direction. 360 + 90 = 450 but rotating a sprite by 450 degrees will still point it in the same direction as rotating it 90 degrees.
When you want to read the value, use mod()
angle = mod(compassAngle, 360)
So in the example above: angle = mod(450, 360). angle = 90

Animation Station - WIP
Code Breaker
Super Brick Smash
Happy Flapper
Additional Image Functions (9 sprite slicing etc.)
Additional String Functions
XML read, write, parse
Black theme for Geeny
RGB->HSV->RGB
SpriteSheet XML Control

Back to top

Profile PM Email

Raven

19

Years of Service

User Offline

Joined: 23rd Mar 2005

Location: Hertfordshire, England

Posted: 21st Feb 2020 23:37

Link

+ Code Snippet

angleMin = min(Current, Desired)
angleMax = max(Current, Desired)
if (angleMax - angleMin) > 180
   dec Current, SmoothAmount
else
   inc Current, SmoothAmount
endif

I mean I'm shattered, as just got off work... and my brain might be a little asleep but that should do the trick, right?

Back to top

Profile PM Email Website

wuergezwerg

5

Years of Service

User Offline

Joined: 30th Jul 2018

Location: Germany

Posted: 23rd Feb 2020 11:51

Link

This post has been marked by the post author as the answer.

Thank you both! With your ideas I found a working solution.

Fixed Function:

+ Code Snippet

function SmoothValues(newvalue as float,smoothness as float)
	if(smoothness > 0.0)
		if (Max(Data.lastvalue,newvalue) - Min(Data.lastvalue,newvalue) > 180)
			diff# = ((newvalue - Data.lastvalue)-360) * smoothness
		else
			diff# = (newvalue - Data.lastvalue) * smoothness			
		endif
		value = Data.lastvalue + diff#
		Data.lastvalue = value
		if value < 0
			value = value + 360
		endif
	else
		value = newvalue
	endif
endfunction value

If you want to test the results, here is the complete working code. Just add the arrow.png (something pointing upwards) and you can toggle the different corrections on and off.

SetErrorMode(2)

#insert "compass.agc"

// set window properties
SetWindowTitle( "GC" )
SetWindowSize( 1080, 1920, 0 )
SetWindowAllowResize( 1 ) // allow the user to resize the window

// set display properties
//SetVirtualResolution( 1024, 768 ) // doesn't have to match the window
SetDisplayAspect (1080.0/1920.0)
SetOrientationAllowed( 1, 0, 0, 0 ) // allow both portrait and landscape on mobile devices
SetSyncRate( 30, 0 ) // 30fps instead of 60 to save battery
SetScissor( 0,0,0,0 ) // use the maximum available screen space, no black borders
UseNewDefaultFonts( 1 )
SetPrintSize(20)

arrowPNG = LoadImage("arrow.png")
arrow = CreateSprite(arrowPNG)
SetSpriteSize(arrow,40,-1)
//Center Arrow
SetSpritePosition(arrow,50-(GetSpriteWidth(arrow)/2),50-(GetSpriteHeight(arrow)/2))

#constant bRaw = 1
#constant bTilt = 2
#constant bHi = 3
#constant bSi = 4
#constant bSmooth = 5

vTilt as integer = 0
vHi as integer = 0
vSi as integer = 0
vSmooth as float = 0.0

AddVirtualButton(bTilt,30,90,20)
SetVirtualButtonText(bTilt,"Tilt")

AddVirtualButton(bHi,50,90,20)
SetVirtualButtonText(bHi,"Hard Iron")

AddVirtualButton(bSi,70,90,20)
SetVirtualButtonText(bSi,"Soft Iron")

AddVirtualButton(bSmooth,90,90,20)
SetVirtualButtonText(bSmooth,"Smooth")

function toggleBtn(id,value,on,off)
	out = value
	if GetVirtualButtonPressed(id)
		if value = on
			SetVirtualButtonColor(id,255,255,255)
			out = off
		else
			SetVirtualButtonColor(id,0,200,0)
			out = on
		endif
	endif
endfunction out

function toggleBtnF(id,value as float,on as float,off as float)
	out as float
	out = value
	if GetVirtualButtonPressed(id)
		if value = on
			SetVirtualButtonColor(id,255,255,255)
			out = off
		else
			SetVirtualButtonColor(id,0,200,0)
			out = on
		endif
	endif
endfunction out

do
	vTilt = toggleBtn(bTilt,vTilt,1,0)
	vHi = toggleBtn(bHi,vHi,1,0)
	vSi = toggleBtn(bSi,vSi,1,0)
	vSmooth = toggleBtnF(bSmooth,vSmooth,0.1,0.0)
	heading = CompassHeading(vTilt,vHi,vSi,vSmooth)
	Print(str(heading)+"°")
	SetSpriteAngle(arrow,-heading)
	if ( GetPointerPressed ( ) = 1 )
        sprite = GetSpriteHit ( GetPointerX ( ), GetPointerY ( ) )
        if sprite=arrow
        		CalibrateCompass()
        endif
    endif

Sync()
loop

+ Code Snippet

SetErrorMode(2)

#insert "compass.agc"

// set window properties
SetWindowTitle( "GC" )
SetWindowSize( 1080, 1920, 0 )
SetWindowAllowResize( 1 ) // allow the user to resize the window

// set display properties
//SetVirtualResolution( 1024, 768 ) // doesn't have to match the window
SetDisplayAspect (1080.0/1920.0)
SetOrientationAllowed( 1, 0, 0, 0 ) // allow both portrait and landscape on mobile devices
SetSyncRate( 30, 0 ) // 30fps instead of 60 to save battery
SetScissor( 0,0,0,0 ) // use the maximum available screen space, no black borders
UseNewDefaultFonts( 1 )
SetPrintSize(20)



arrowPNG = LoadImage("arrow.png")
arrow = CreateSprite(arrowPNG)
SetSpriteSize(arrow,40,-1)
//Center Arrow
SetSpritePosition(arrow,50-(GetSpriteWidth(arrow)/2),50-(GetSpriteHeight(arrow)/2))

#constant bRaw = 1
#constant bTilt = 2
#constant bHi = 3
#constant bSi = 4
#constant bSmooth = 5

vTilt as integer = 0
vHi as integer = 0
vSi as integer = 0
vSmooth as float = 0.0

AddVirtualButton(bTilt,30,90,20)
SetVirtualButtonText(bTilt,"Tilt")

AddVirtualButton(bHi,50,90,20)
SetVirtualButtonText(bHi,"Hard Iron")

AddVirtualButton(bSi,70,90,20)
SetVirtualButtonText(bSi,"Soft Iron")

AddVirtualButton(bSmooth,90,90,20)
SetVirtualButtonText(bSmooth,"Smooth")

function toggleBtn(id,value,on,off)
	out = value
	if GetVirtualButtonPressed(id)
		if value = on
			SetVirtualButtonColor(id,255,255,255)
			out = off
		else
			SetVirtualButtonColor(id,0,200,0)
			out = on
		endif
	endif
endfunction out

function toggleBtnF(id,value as float,on as float,off as float)
	out as float
	out = value
	if GetVirtualButtonPressed(id)
		if value = on
			SetVirtualButtonColor(id,255,255,255)
			out = off
		else
			SetVirtualButtonColor(id,0,200,0)
			out = on
		endif
	endif
endfunction out

do
	vTilt = toggleBtn(bTilt,vTilt,1,0)
	vHi = toggleBtn(bHi,vHi,1,0)
	vSi = toggleBtn(bSi,vSi,1,0)
	vSmooth = toggleBtnF(bSmooth,vSmooth,0.1,0.0)
	heading = CompassHeading(vTilt,vHi,vSi,vSmooth)
	Print(str(heading)+"°")
	SetSpriteAngle(arrow,-heading)
	if ( GetPointerPressed ( ) = 1 )
        sprite = GetSpriteHit ( GetPointerX ( ), GetPointerY ( ) )
        if sprite=arrow
        		CalibrateCompass()
        endif
    endif


Sync()
loop

// File: compass.agc
// Created: 20-02-16

type XYZ
	X as float
	Y as float
	Z as float
endtype

type XY
	X as float
	Y as float
endtype

type CompassData
	Accelerometer as integer //Accelerometer exists
	MagneticSensor as integer //Magnetic sensor exists
	//Magnetometer RAW
	Mag as XYZ
	//Tilt Compensation
	Acc as XYZ
	AccNorm as XY
	PhonePitch as float
	PhoneRoll as float
	//MinMax for Calibration
	MagMin as XYZ
	MagMax as XYZ
	//Calculated Values
	Result as XYZ
	//Previous value
	lastvalue as float
endtype

global Data as CompassData

Data.Accelerometer = GetAccelerometerExists()
Data.MagneticSensor = GetMagneticSensorExists()
GetCalibrationDataExists()

//Check if saved MinMax data exists and set values
function GetCalibrationDataExists()
	if GetFileExists("CompassCal.data") = 1
		read = OpenToRead("CompassCal.data")
		Data.MagMin.X = ReadFloat(read)
		Data.MagMin.Y = ReadFloat(read)
		Data.MagMin.Z = ReadFloat(read)
		Data.MagMax.X = ReadFloat(read)
		Data.MagMax.Y = ReadFloat(read)
		Data.MagMax.Z = ReadFloat(read)
		CloseFile(read)
	else
		CalibrateCompass()
	endif
endfunction

function CalibrateCompass()
		write = OpenToWrite("CompassCal.data")
		Data.MagMin.X = 0.0
		Data.MagMin.Y = 0.0
		Data.MagMin.Z = 0.0
		Data.MagMax.X = 0.0
		Data.MagMax.Y = 0.0
		Data.MagMax.Z = 0.0
		timer = GetSeconds()
		while seconds <= 10
			Print("Calibrate now")
			seconds = GetSeconds() - timer
			Print(str(seconds))
			GetSensorData("mag")
			GetMinMaxData()
			sync()
		endwhile
		WriteFloat(write,Data.MagMin.X)
		WriteFloat(write,Data.MagMin.Y)
		WriteFloat(write,Data.MagMin.Z)
		WriteFloat(write,Data.MagMax.X)
		WriteFloat(write,Data.MagMax.Y)
		WriteFloat(write,Data.MagMax.Z)
		CloseFile(write)
endfunction

function GetSensorData(sensor as string)
	select sensor
		case "mag":
			if Data.MagneticSensor = 1
				Data.Mag.X = GetRawMagneticX()
				Data.Mag.Y = -GetRawMagneticY()
				Data.Mag.Z = GetRawMagneticZ()
				//Set initial values (needed for raw calculation)
				Data.Result.X = Data.Mag.X
				Data.Result.Y = Data.Mag.Y
				Data.Result.Z = Data.Mag.Z
			else
				Data.Result.X = 0
				Data.Result.Y = 0
				Data.Result.Z = 0				
			endif
		endcase
		case "acc":
			if Data.Accelerometer = 1
				Data.Acc.X = GetRawAccelX()
				Data.Acc.Y = GetRawAccelY()
				Data.Acc.Z = GetRawAccelZ()
			endif
		endcase
	endselect
endfunction

function GetMinMaxData()
	if Data.Mag.X > Data.MagMax.X then Data.MagMax.X = Data.Mag.X
	if Data.Mag.Y > Data.MagMax.Y then Data.MagMax.Y = Data.Mag.Y
	if Data.Mag.Z > Data.MagMax.Z then Data.MagMax.Z = Data.Mag.Z
	if Data.Mag.X < Data.MagMin.X then Data.MagMin.X = Data.Mag.X
	if Data.Mag.Y < Data.MagMin.Y then Data.MagMin.Y = Data.Mag.Y
	if Data.Mag.Z < Data.MagMin.Z then Data.MagMin.Z = Data.Mag.Z
endfunction

function CalcHeading(smoothness as float)
	offset = 90
	heading = ATanFull(Data.Result.Y,Data.Result.X)+offset
	//Correct angle if offset generates value over 360
	heading = mod(heading, 360)
	//heading = WrapAngle(heading)
	heading = SmoothValues(heading,smoothness)
endfunction heading

function SmoothValues(newvalue as float,smoothness as float)
	if(smoothness > 0.0)
		if (Max(Data.lastvalue,newvalue) - Min(Data.lastvalue,newvalue) > 180)
			diff# = ((newvalue - Data.lastvalue)-360) * smoothness
		else
			diff# = (newvalue - Data.lastvalue) * smoothness			
		endif
		value = Data.lastvalue + diff#
		Data.lastvalue = value
		if value < 0
			value = value + 360
		endif
	else
		value = newvalue
	endif
endfunction value

function CompassHeading(compensate,hardiron,softiron,smoothness as float)
	GetSensorData("mag")
	//Compensate Tilt
	if compensate = 1 and Data.Accelerometer = 1
		GetSensorData("acc")
		
		Data.AccNorm.X = Data.Acc.X/sqrt(Data.Acc.X * Data.Acc.X + Data.Acc.Y * Data.Acc.Y + Data.Acc.Z * Data.Acc.Z)
		Data.AccNorm.Y = Data.Acc.Y/sqrt(Data.Acc.X * Data.Acc.X + Data.Acc.Y * Data.Acc.Y + Data.Acc.Z * Data.Acc.Z)
		
		Data.PhonePitch = asin(Data.AccNorm.X)
		Data.PhoneRoll = asin(Data.AccNorm.Y/cos(Data.PhonePitch))
	
		Data.Result.X = (Data.Mag.X*cos(Data.PhonePitch))+(Data.Mag.Z*sin(Data.PhonePitch))
		Data.Result.Y = (Data.Mag.X*sin(Data.PhoneRoll)*sin(Data.PhonePitch)) + (Data.Mag.Y*cos(Data.PhoneRoll)+Data.Mag.Z*sin(Data.PhoneRoll)*cos(Data.PhonePitch))
	endif
	//Setting Hard Iron Offset
	if hardiron = 1
		Data.Result.X = Data.Result.X - (Data.MagMin.X + Data.MagMax.X)/2
		Data.Result.Y = Data.Result.Y - (Data.MagMin.Y + Data.MagMax.Y)/2
		Data.Result.Z = Data.Result.Z - (Data.MagMin.Z + Data.MagMax.Z)/2
	endif
	//Setting Soft Iron Offset
	if softiron = 1
		Data.Result.X = ((Data.Result.X - Data.MagMin.X) / (Data.MagMax.X - Data.MagMin.X) * 2) -1
		Data.Result.Y = ((Data.Result.Y - Data.MagMin.Y) / (Data.MagMax.Y - Data.MagMin.Y) * 2) -1
		Data.Result.Z = ((Data.Result.Z - Data.MagMin.Z) / (Data.MagMax.Z - Data.MagMin.Z) * 2) -1
	endif
	//Calculate heading
	heading = CalcHeading(smoothness)
endfunction heading

Function min(value1,value2)
	if(value1 < value2)
		min = value1
	else
		min = value2
	endif
Endfunction min
Function max(value1,value2)
	if(value1 > value2)
		max = value1
	else
		max = value2
	endif
Endfunction max

+ Code Snippet

// File: compass.agc
// Created: 20-02-16

type XYZ
	X as float
	Y as float
	Z as float
endtype

type XY
	X as float
	Y as float
endtype

type CompassData
	Accelerometer as integer //Accelerometer exists
	MagneticSensor as integer //Magnetic sensor exists
	//Magnetometer RAW
	Mag as XYZ
	//Tilt Compensation
	Acc as XYZ
	AccNorm as XY
	PhonePitch as float
	PhoneRoll as float
	//MinMax for Calibration
	MagMin as XYZ
	MagMax as XYZ
	//Calculated Values
	Result as XYZ
	//Previous value
	lastvalue as float
endtype

global Data as CompassData

Data.Accelerometer = GetAccelerometerExists()
Data.MagneticSensor = GetMagneticSensorExists()
GetCalibrationDataExists()

//Check if saved MinMax data exists and set values
function GetCalibrationDataExists()
	if GetFileExists("CompassCal.data") = 1
		read = OpenToRead("CompassCal.data")
		Data.MagMin.X = ReadFloat(read)
		Data.MagMin.Y = ReadFloat(read)
		Data.MagMin.Z = ReadFloat(read)
		Data.MagMax.X = ReadFloat(read)
		Data.MagMax.Y = ReadFloat(read)
		Data.MagMax.Z = ReadFloat(read)
		CloseFile(read)
	else
		CalibrateCompass()
	endif
endfunction

function CalibrateCompass()
		write = OpenToWrite("CompassCal.data")
		Data.MagMin.X = 0.0
		Data.MagMin.Y = 0.0
		Data.MagMin.Z = 0.0
		Data.MagMax.X = 0.0
		Data.MagMax.Y = 0.0
		Data.MagMax.Z = 0.0
		timer = GetSeconds()
		while seconds <= 10
			Print("Calibrate now")
			seconds = GetSeconds() - timer
			Print(str(seconds))
			GetSensorData("mag")
			GetMinMaxData()
			sync()
		endwhile
		WriteFloat(write,Data.MagMin.X)
		WriteFloat(write,Data.MagMin.Y)
		WriteFloat(write,Data.MagMin.Z)
		WriteFloat(write,Data.MagMax.X)
		WriteFloat(write,Data.MagMax.Y)
		WriteFloat(write,Data.MagMax.Z)
		CloseFile(write)
endfunction

function GetSensorData(sensor as string)
	select sensor
		case "mag":
			if Data.MagneticSensor = 1
				Data.Mag.X = GetRawMagneticX()
				Data.Mag.Y = -GetRawMagneticY()
				Data.Mag.Z = GetRawMagneticZ()
				//Set initial values (needed for raw calculation)
				Data.Result.X = Data.Mag.X
				Data.Result.Y = Data.Mag.Y
				Data.Result.Z = Data.Mag.Z
			else
				Data.Result.X = 0
				Data.Result.Y = 0
				Data.Result.Z = 0				
			endif
		endcase
		case "acc":
			if Data.Accelerometer = 1
				Data.Acc.X = GetRawAccelX()
				Data.Acc.Y = GetRawAccelY()
				Data.Acc.Z = GetRawAccelZ()
			endif
		endcase
	endselect
endfunction

function GetMinMaxData()
	if Data.Mag.X > Data.MagMax.X then Data.MagMax.X = Data.Mag.X
	if Data.Mag.Y > Data.MagMax.Y then Data.MagMax.Y = Data.Mag.Y
	if Data.Mag.Z > Data.MagMax.Z then Data.MagMax.Z = Data.Mag.Z
	if Data.Mag.X < Data.MagMin.X then Data.MagMin.X = Data.Mag.X
	if Data.Mag.Y < Data.MagMin.Y then Data.MagMin.Y = Data.Mag.Y
	if Data.Mag.Z < Data.MagMin.Z then Data.MagMin.Z = Data.Mag.Z
endfunction

function CalcHeading(smoothness as float)
	offset = 90
	heading = ATanFull(Data.Result.Y,Data.Result.X)+offset
	//Correct angle if offset generates value over 360
	heading = mod(heading, 360)
	//heading = WrapAngle(heading)
	heading = SmoothValues(heading,smoothness)
endfunction heading

function SmoothValues(newvalue as float,smoothness as float)
	if(smoothness > 0.0)
		if (Max(Data.lastvalue,newvalue) - Min(Data.lastvalue,newvalue) > 180)
			diff# = ((newvalue - Data.lastvalue)-360) * smoothness
		else
			diff# = (newvalue - Data.lastvalue) * smoothness			
		endif
		value = Data.lastvalue + diff#
		Data.lastvalue = value
		if value < 0
			value = value + 360
		endif
	else
		value = newvalue
	endif
endfunction value

function CompassHeading(compensate,hardiron,softiron,smoothness as float)
	GetSensorData("mag")
	//Compensate Tilt
	if compensate = 1 and Data.Accelerometer = 1
		GetSensorData("acc")
		
		Data.AccNorm.X = Data.Acc.X/sqrt(Data.Acc.X * Data.Acc.X + Data.Acc.Y * Data.Acc.Y + Data.Acc.Z * Data.Acc.Z)
		Data.AccNorm.Y = Data.Acc.Y/sqrt(Data.Acc.X * Data.Acc.X + Data.Acc.Y * Data.Acc.Y + Data.Acc.Z * Data.Acc.Z)
		
		Data.PhonePitch = asin(Data.AccNorm.X)
		Data.PhoneRoll = asin(Data.AccNorm.Y/cos(Data.PhonePitch))
	
		Data.Result.X = (Data.Mag.X*cos(Data.PhonePitch))+(Data.Mag.Z*sin(Data.PhonePitch))
		Data.Result.Y = (Data.Mag.X*sin(Data.PhoneRoll)*sin(Data.PhonePitch)) + (Data.Mag.Y*cos(Data.PhoneRoll)+Data.Mag.Z*sin(Data.PhoneRoll)*cos(Data.PhonePitch))
	endif
	//Setting Hard Iron Offset
	if hardiron = 1
		Data.Result.X = Data.Result.X - (Data.MagMin.X + Data.MagMax.X)/2
		Data.Result.Y = Data.Result.Y - (Data.MagMin.Y + Data.MagMax.Y)/2
		Data.Result.Z = Data.Result.Z - (Data.MagMin.Z + Data.MagMax.Z)/2
	endif
	//Setting Soft Iron Offset
	if softiron = 1
		Data.Result.X = ((Data.Result.X - Data.MagMin.X) / (Data.MagMax.X - Data.MagMin.X) * 2) -1
		Data.Result.Y = ((Data.Result.Y - Data.MagMin.Y) / (Data.MagMax.Y - Data.MagMin.Y) * 2) -1
		Data.Result.Z = ((Data.Result.Z - Data.MagMin.Z) / (Data.MagMax.Z - Data.MagMin.Z) * 2) -1
	endif
	//Calculate heading
	heading = CalcHeading(smoothness)
endfunction heading

Function min(value1,value2)
	if(value1 < value2)
		min = value1
	else
		min = value2
	endif
Endfunction min
Function max(value1,value2)
	if(value1 > value2)
		max = value1
	else
		max = value2
	endif
Endfunction max

Back to top

Profile PM Website

Sorry your browser is not supported!

AppGameKit Studio Chat / [SOLVED] Problems heading north