hello!

I`m using Rotomotion Attitude and Heading Referance system (AHRS) for my RC airplane stabilization. This AHRS product is based on a inertial measurement unit (IMU) and a three axis magnetometer which output sensors reading on euler angle, quaternion, angular rates, magnetic field and accelerations.

The AHRS outputs ASCII data on its serial port at 38400 N81 with no flow control. Each line consists of a marker indicating the type of data and a comma separated list of values. e.g: i want to read the euler angle line

E:-0.004,0.0123,3.14

The three Euler angles roll, pitch and yaw are output in degrees.
(roll range:-180 < roll angle < 180 , zero deg mean level with positive angle is right wing down)
(pitch range: -90 < pitch angle < 90, positive angle = nose up)
(yaw range: -180
i`m using HSEROUT & HSERIN command to transmit and receive AHRS output since the AHRS have a hardware USART and using simple IF and THEN command, to specifies where direction the servo should move. the code is like this:

DEFINE OSC 20
define HSER_BAUD 38400
DEFINE HSER_CLROERR 1
DEFINE HSER_RCSTA 90h
DEFINE HSER_TXSTA 20h

ADCON1 = 7

kirikanan var portd.0
nose var portd.1
ekor var portd.3

rollH var byte
rollHi var byte
pitchH var byte
pitchHi var byte
yawH var byte
yawHi var byte

b0 var word
b1 var word
b2 var word
b3 var word
b5 var word

low portd.0
low portd.1
low portd.3

b1 = 747
b2 = 750
b3 = 850

pause 1000
hserout ["!00001",13]
pause 2000

hserin [WAIT("E:"),dec rollHi,skip 6,dec pitchHi,skip 6,dec yawHi]

pause 500

start:

hserin [WAIT("E:"),dec rollH,skip 6,dec pitchH,skip 6,dec yawH]

servo:
pulsout ekor,b1
pulsout kirikanan,b2
pulsout nose,b3

;IFs goes here
b0 = b1 + b2 + b3
b5 = b0 / 500
b0 = 23 - b5

pause b0

if rollH < rollHi then
b2 = b2 - 5
goto nex1
endif

if rollH > rollHi then
b2 = b2 + 5
endif

nex1:
if pitchH < pitchHi then
b3 = b3 + 5
goto nex2
endif

if pitchh > pitchHi then
b3 = b3 - 5
endif

nex2:
if yawH < yawHi then
b1 = b1 + 5
goto start
endif

if yawH > yawHi then
b1 = b1 - 5
endif

goto start
end

the problem i`m facing right now is that, the servo is moving only in one direction when i move around the AHRSmovement. for e.g, the roll servo will only roll left, pitch servo will only move so that the aircraft nose moving up. can anyone help me on this.


regards,
Syariful.