Calculating heading error from desired bearing.

AndyGadget

Senior Member
This is one of those things which should be simple, but by brain is refusing to co-operate on this one.

I'm using a Honeywell compass sensor with an 18M2 for a small robot, and trying to calculate the difference between the desired (btgt) and actual bearing (babs). Getting data from the sensor is fine but I'm having trouble with the logic of the bearing difference (berr). The simple code below is fine as long as the zero / 71 heading does not fall between the desired and actual bearing - If it does, the > and < tests reverse and an incorrect error reading is returned.
How do I cope with this situation?

Code:
GetBearing:
	' HMC6352 outputs 0 to 3599
	' 72 steps complete turn when divided by 50.
	' 18 steps / quadrant.  
	hi2cout ("A")
	pause 10
	hi2cin (bngh,bngl)
   	babs = babs/50 ' 72 points   ' babs is compass reading.  btgt is target bearing.

	brglft = 0 : brgrgt = 0 ' Flags for L or R of desired bearing.
	berr = 0		      ' Bearing error. Zero if on course.
	
	if btgt > babs then 
		berr = btgt - babs
		brgrgt = 1
	end if
	
	if btgt < babs then
		berr = babs - btgt
		brglft = 1
	end if
return
Edit : Think I've found a clue in Fast Freddy's code here : http://www.picaxeforum.co.uk/showthread.php?17616-Relative-Compass-Heading&highlight=heading+error
 
Last edited:

hippy

Technical Support
Staff member
I'd guess it's because to the left of 0 is 359 not -1, on both sensor reading and heading ranges. You need to jump through two sets of hoops, change 359, 0, 1 to -1, 0, +1 and then handle the negatives when they occur.
 

AndyGadget

Senior Member
I'd guess it's because to the left of 0 is 359 not -1, on both sensor reading and heading ranges. You need to jump through two sets of hoops, change 359, 0, 1 to -1, 0, +1 and then handle the negatives when they occur.
It's not that. The division by 50 scales it to 0 to 71 which I have proven with debug.
(Babs is a word variable consisting of bngh and bngl; returned from the compass module.)
 

alband

Senior Member
Not sure what the issue you're describing is, but it think it's to do with measurements that go through the zero value and overflow.
I think this would work, not tested:

Code:
if (btgt > 36) then
	blow = btgt - 36
	bhigh = btgt

	if (babs < bhigh && babs > blow) then
		berr = bhigh - babs
		left = 1
	else
		if (babs > 36) then
			berr = babs - bhigh
		else
			berr = 36 + babs - blow
		end if
		right = 1
	end if

else
	blow = btgt
	bhigh = btgt + 36

	if (babs < bhigh && babs > blow) then
		berr = babs - blow
		right = 1
	else
		if (babs > 36) then
			berr = 36 + bhigh - babs
		else
			berr = btgt - babs
		end if
		left = 1
	end if
end if
N.B. It's not "PICAXE BASIC". I just wrote it out logically, so there will be syntax errors.

To avoid the overflow issues, it works out a "safe" 180deg segment that doesn't contain 72/0 and is either starts or ends with btgt. It then checks if babs is in the segment; If it is, all is fine and dandy - berr is the (positive) difference between babs and btgt. If it isn't, things are more tricky, because babs could be on either side of 0. It checks for this the works out the error accordingly.


Hope that makes sense. Sorry if there are mistakes, but the basic princible was to create a "safe" half of the circle and use enough if statements to avoid going though 72/0.
 
Last edited:

AndyGadget

Senior Member
Thanks both - I've cracked it.
All it needs is this on the end :

Code:
	if berr > 36 then 
		berr = 72 - berr
		brglft = not brglft
		brgrgt = not brgrgt
	end if
 

AllyCat

Senior Member
Hi,

When recently converting some code for calculating the times of sunrise/sunset to PICaxe Basic, I found that the maths became much simpler when scaling 0 - 360 degrees up to a word of 0 - 65535 (i.e. multiply by ~182). Then all the "MOD 360"s (moving through 360 degrees) in the original code just disappeared. ;)

Cheers, Alan.
 

AndyGadget

Senior Member
Hi,

When recently converting some code for calculating the times of sunrise/sunset to PICaxe Basic, I found that the maths became much simpler when scaling 0 - 360 degrees up to a word of 0 - 65535 (i.e. multiply by ~182). Then all the "MOD 360"s (moving through 360 degrees) in the original code just disappeared. ;)

Cheers, Alan.
Thanks, yes, I was considering that. As you say, the rollround problems go away.
However I thought there must be a simple fix with my scaled values and I'm running short of variables.

Andy.
 

g6ejd

Senior Member
This is how I do it with my weather display on an Arduino:

// Minimum angle is either ABS(AngleA- AngleB) or (360-ABS(AngleA-AngleB))
int veering = min(360 - abs(startV - endV), abs(startV - endV)); // Take the minimum of the two solutions

It's the easiest way I've found for that 0 to 359 transition.
 
Last edited:

AllyCat

Senior Member
I'm running short of variables.
Hi Andy,

Don't overlook the extra 6 - 8 s_w? words with M2s (Manual 2 page 13). Also @bptr can introduce many more variables for "direct" access and sometimes useful for passing (different) variables to/from a subroutine.

Cheers, Alan.
 

AndyGadget

Senior Member
Hi Andy,

Don't overlook the extra 6 - 8 s_w? words with M2s (Manual 2 page 13). Also @bptr can introduce many more variables for "direct" access and sometimes useful for passing (different) variables to/from a subroutine.

Cheers, Alan.
Thought transference!!! I remembered about those about 5 minutes ago and have relocated some of my word variables.
I'll be using bptr as well, for storing distance readings.

Also . . . I've got a couple of 18m2+ on order (or is that cheating?)
 
Top