Assume for this discussion that a heading , psi , is a floating point variable derived from integrating heading rate. It is referenced to the initial heading and can have absolute values greater than 360 degs. A [ 0 to 360] version of this , psi_360 is formed and used for driving a compass rose.
psi_360 = psi – 360.* int(psi/360.) ;
int (x) function just truncates x to the nearest integer.
(note: if we were using integer math, a modulus (remainder) operator % could have been used and psi_360 = psi % 360. )
This value is not continuous when crossing zero and will cause problems when generating an error signal for use in PID heading hold controller loops so we must use psi as a signal rather than psi_360. (unless error handling logic is included as discussed later)
The error signal
psi_error = psi_ref – psi
is typically multiplied by a gain Kp and fed to the motors as an asymmetrical command. i.e.
turn_cmd = Kp*psi_error . (>0 turn right, <0 turn left)
left_motor = speed_cmd + turn_cmd,
right_motor =speed_cmd – turn_cmd
When holding an existing heading, psi_ref = current psi when the mode is engaged and held constant until the mode is disengaged again. There is no problem with psi values greater than 360.
Question: Suppose we want to turn to a specific compass heading rather than just hold heading . How should the error signal be generated?
Well, we simply generate an psi_error that goes between [ -180 to + 180] . Call this error psi_error_180.
This is done as follows:
First generate an error that is restricted to [0 , 360] interval.
psi_error_360 =psi_error -360.*int( psi_error /360.) ;
Next test if this exceeds 180. If so subtract 360 to create a negative error.
if(psi_error_360 > 180 ) then psi_error_180 = psi_error_360- 360
Next test if less than -180 . If so add 360 to create a positive error.
if(psi_error_180<-180) then psi_error_180 = psi_error_360+ 360
else do nothing
psi_error_180 = psi_error_360 .
If psi_error_180 is used in place of psi_error in the PID loop, then the robot will turn in a direction that will get to the commanded heading the fastest. Eg
Suppose you are heading East , psi = 90. You command a new heading of psi_ref = 300 deg. The fastest way is to turn counter clockwise (left) rather than clockwise (right)…although either way would work.
So psi_error = 300 – 90 = 210 deg
psi_error_180 = 210 – 360 = – 150 deg
turn_cmd = kp*psi_error_180 < 0 … so we tun left .
Note that in the equation for psi_error_360 , psi_error can be generated using psi or psi_360 heading signals without any problems in continuity. The inertial nav function could then just output psi_360 for all to use.