|
by Natalius Kiedro
Summary
A number of previous ANs have described the AR7212, a 12-channel
receiver for Spektrum DX7 radios (AN# 172), the need and a solution for numeric
data compression in sensor-sensor and air-ground serial communication (AN#
174), and some basic information of how to do matrix algebra using Bascom’s
1D-arrays (AN# 176).
This AN describes an Electronic Flight Information System
(EFIS) for autopilots in RC planes and amateur drones. The Bascom code utilizes
the 9DOF Razor board which is based on an Atmega 328p interfaced to LPR530AL
and LY530ALH-gyros, ADXL345 as a 3D-accelerometer as well as to HMC5843 as an
3D-magnetometer. It is based on the Mahoney-Premerlani Direction Cosine Matrix
(DCM) algorithm which was adopted for Sparkfun’s 9DOF Razor IMU by Doug Weibel
and others. The Kixrazor-SW expands this portation by feeding in GPS without
HW-changes. It utilizes Premerlani’s method to compute airspeed from GPS and DCM.
The
Kixrazor outputs GPS-position, groundspeed, airspeed and plane orientation
(yaw, pitch, roll) in the Kixline-format. Together with the AR7212 it allows
building autopilots for advanced multi-servo RC-airplanes for less than 200
Euro.
Introduction
For decades, the only affordable way to do flight stabilization of model
airplanes was based on gyros. Early gyros were mechanical devices and measured
angular rotation based on the conservation of spin of a rotating mass inside. Gyro-stabilized
RC became popular among model helicopter enthusiasts but did not enable long
term stabilization – there was no way that the pilot could leave the sticks for
longer than a fraction of a second. The
problem of gyros – in general – is that they drift. As a consequence, early
autopilots for model aircrafts were based on thermopile sensors.
Thermopile-based flight stabilizers came to the market in the beginning of the
nineties. By now they have replaced almost completely horizon sensors working in
the visual light. Thermopiles measure temperature differences based on infrared
radiation. As the sky (space) is always colder than the earth, six thermopiles
arranged like the axes of a 3D-cordinate system (left-right, front-rear,
up-down) are sufficient to compute pitch and roll angles in the earth frame of
reference. Pitch means nose up-down, while roll means right-wing up-down.
Compared to horizon sensors based on visible light, IR-based sensors also allow
flight stabilization during the night. The problem of any “horizon sensor” (IR
or visible light) is that they have to “look” from outside of the fuselage and
thus add air resistance next to the problem that a collision with an insect
might be the end of the model plane.
3D-MEMS-gyros,
3D-MEMS-accelerometer, and 3D-magnetometer chips
3D-orientation sensors are currently coming into a price range that will
allow their integration into all kinds of mobile devices in the near future.
From cars, boats, planes, cell phones, game controllers, medical and sports devices
to almost anything that moves and turns or is kicked.
Even soccer balls, skies,
tennis with embedded intelligence are
not beyond imagination for the upcoming years – just to know more about the
physics of sports. As mentioned, 3D-MEMS-gyros (currently 1D + 2D) measure the
speed of rotation around the three axes – to obtain angles one has also to
measure time and to integrate the rotation rate. 3D-MEMS-accelerometers measure
the mechanical force in three dimension, and as force is mass by acceleration and
the mass of micromechanical sensor elements is known, they output acceleration
components in three directions. 3D magnetometers measure the 3D components of a
magnetic field. While 3D-MEMS-accelerometers and magnetometers have a constant
“offset”, caused by gravity and the earth’s magnetic field, 3D-gyros have no
“absolute” frame of reference. A gyro simply does not allow to compute angles
with respect to the surface of the earth without additional information. For a
gyro, an angle change from 0 to 30° gives out the same signal as an angle
change from 90° to 120° assuming that the turning rate was identical. Gyros
only offer measurements with respect to a local frame of reference. On the
other side, 3D accelerometer output data allow to calculate tilt-angles (=pitch
and roll) accurately, but only if the
accelerometer doesn’t sense anything else except of earth gravity. In practice:
Tilt angles are correct, if the object doesn’t change its speed in any
direction. This will hold for a train at constant speed or a still standing
quadrocopter in the air. Finally, a 3D magnetometer does not allow to compute
magnetic heading if the orientation in 3D space is unknown. A DIY-compass can
be build from a 3D-magnetometer and a 3D-accelerometer quite easily. It will
work correctly, however, only if there is no move (or a move with constant
speed in any direction).
The
need for sensor fusion
While speed, position, and heading comes from GPS, pitch, roll, and yaw has to be derived from sensor
fusion. Sensor fusion means the mathematical conjunction of information from
individual sensors, which by their own cannot provide the whole picture. Yaw is
the direction in which the nose of an airplane is pointing, while heading(bearing)
is the direction of move. In wind-free situations, yaw and heading are
identical. Wind-free days are rare however. Sidewinds may cause a substantial
deviation.
There are numerous recipes for sensor fusion. The most known is the
Kalman-filter which is an iterative process predicting the move of an airplane
at time t+dt from sensor-readouts at time t and adjusting the controls (servos)
at time t (plus a little bit more for computation) accordingly. Kalman
filtering is heavily based on matrix algebra and may be an overkill for 8 bit
AVRs especially if a physical model of the airplane comes in.
Matrixnavigation
by the Direction Cosine Matrix (DCM)
An ingenious method to do Kalman-like filtering without a physical model
of airplane dynamics was developed by Robert Mahoney and implemented for
RC-airplanes by Bill Premerlani. By now, even Intel Engineers have discovered
that this method is a remarkably efficient way to do orientation sensing of
embedded gadgets. The method itself is long known being heavyly employed in
3D-computer games today. It is fully within the scope of computational power of
small AVRs and thus ready for Bascomization even when based on floating point
algebra.
Prelermani's Direction Cosine Matrix (DCM) algorithm continuously
utilizes a 3x3 rotation matrix R (=DCM) defined by the roll(r) pitch(p) and
yaw(y)-angles with respect to the fixed reference of earth's directions:
|rxx rxy rxz|
R = |ryx ryy ryz|
|rzx rzy rzz|
|cos(r)cos(y) sin(p)sin(r)cos(y)-cos(p)sin(y) cos(p)sin(r)cos(y)+sin(p)sin(y)|
= |cos(r)sin(y)
sin(p)sin(r)sin(y)+cos(p)cos(y) cos(p)sin(r)sin(y)-sin(p)cos(y)|
| -sin(r) sin(p)cos(r) cos(p)cos(r) |
Rotation matrices are widely employed in coordinate transformations. The
latter are needed, for example, in 3D computer graphics to allow viewing a
3D-scenery from a fixed frame of reference defined by the viewer. Any object in
3D-space can be characterized by a collection of points Pi(xi,yi,zi) where
xi,yi,zi are the coordinates in the frame of reference of the scenery.
Coordinate transformation means mapping these points to pixels of the computer
screen. For the case there is no difference in orientation, mapping primarily
means scaling. If there is a difference in orientation, the angles r,p,y
describing this difference can be used to transform a point Pi(xi,yi,zi) into
Pi'(xi',yi',zi'), where the z'-Axis is perpendicular to the x',y'-plane of the
computer screen. Points are vectors in 3D space. What holds for PO-I-nts (=
POsitional Information) holds for any other quantity which can be broken into
x,y,z-components describing a vector: speed, acceleration, and so on. The
recipe is simple: For each Vector Vi(xi,yi,zi) do a matrix multiplication with
the rotation matrix to arrive at the Vector Vi' in the new frame of reference.
The DCM is a "combined matrix" (matrix product) of three
individual matrices describing the rotation around the x-axis, y-axis, and
z-axis. With respect to an aircraft these axes are parallel to the fuselage
(x), parallel to the wings (y) and parallel to the rudder (z). Gyros can only
measure the rotation around these axis in the frame of reference of the
aircraft, rotation angles are obtained by integrating angular rates. Coordinate
transformation with the DCM maps these local angular moves to the Euler angles
yaw, pitch and roll with respect to the earth.
Sparkfun's 9DOF Razor board brings in three sources of sensor information:
(1) LPR530AL and LY530ALH gyros measuring roll-, pitch-, and yaw-rates
with respect to the local reference of the plane. Sensor information comes in
as analog information and needs to be integrated to find the difference with
respect to the last measurements: Alpha = Sum (ralpha * dt), where ralpha is
the angular rate and dt is the time increment defined by the cycle time (20 ms
+ x (around 400 µs)). The Kixrazor SW averages ralpha and updates dt from 16
bit Timer1 in the main loop. Analog information is averaged for the whole time
the main loop has nothing else to do.
(2) ADXL345, a 3D-accelerometer measuring acceleration in the x,y, and z
axis of the plane. This info comes in via I2C, triggered by the timer.
(3) HMC5843, a 3D-magnetometer measuring magnetization with respect to
the x,y,z-axis of the plane. This info comes in via I2C every 5th cycle.
Sensor infos (1)..(3) define 3 vectors: The "Gyrovector" (gx,
gy, gz), the Accel-vector (ax, ay, az), and the "Magnetovector" (mx,
my, mz). These vectors are organized as a Sensor matrix S:
|gx
gy gz|
S = |ax ay az|
|mx
my mz|
The DCM algorithm consists of 4 steps:
(a) Matrix_update
(b) Normalize
(c) Drift_correction
(d) Euler_angles
Matrix_update means a matrix multiplication of the DCM with matrix G
from the gyro integrals gx, gy, gz written as:
| 0 -gz gy|
G = | gz 0 -gx|
|-gy gx 0|
Gyro drift correction is considered by the option to replace the gyro
inte-
grals with PId-corrected counterparts. The new DCM is derived from the
matrix product R(t).G(t) as follows:
R(t+dt) = R(t).G(t) + R(t)
At the beginning (t = 0) the DCM (R(0)) comes in as a identity matrix.
The update thus means adding off-diagonal elements representing gyro
information. Note that the matrix update is the core of the DCM - steps (b) to
(c) could be omitted if gyros were drift free. Accelerometer and Magnetometer
information are added for a gyro drift estimate in steps (b) and (c). At the
end (d) the Euler angles are computed from the DCM by:
Pitch = -
Asin(rzx)
Roll =
Atn2(rzy, rzz)
Yaw = Atn2(ryx , rxx)
Please find more information on the DCM algorithm on the web. The best
info is available if you go to Premerlani's homepage on DIYDRONES. Bill has
recently published a consideration of wind in matrixnavigation which allows to
estimate airspeed from temporal changes of speed over ground (GPS),
GPS-heading, GPS-altitude and changes in the DCM. I have adopted this method
for inclusion in the Kixrazor-software. 
AR7212, Unilog, Razor 9DOF, GPS, and UM96 integrated into a “flight
sausage”.
Program:
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ KIXRAZOR @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ (AHRS+GPS+KIX) @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ This is the BASCOM portation of Bill @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ Premerlani's DCM algorithm based on @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ the portation to Arduino by Jordi @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ Munoz, Doug Weibel, and Jose Julio. @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ Hardware required: Sparkfun 9DOF @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ Razor by Nathan Seidle. Kixline plus @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ GPS extensions and code changes... @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ by Natalius Kiedro, April 2010 @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@ @@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'Tip from Ben Zijlstra (search Bascom and Arduino on Bascom forum)
'How to use Arduino-HW inside Bascom:
'
'Bascom: external programmer
'Program: C:\arduino-0018\arduino-0018\hardware\tools\avr\bin\avrdude.exe
'Parameters : -v -f -cc : \ Avrdude.conf -p M328p -p Com2 -c Stk500v1 -b 57600 -u Flash : W : {file} : A
'The fusebits of a Arduino Atmega328p are as follows:
'lockbit 65 00
'lockbit 43 11
'lockbit 21 11
'C 1 divide clock by 8 disabled
'B 1 clock output disabled
'KLA987 111111 Ext, clock Osc.freq. 8.0- Mhz
'
'K 1
'J 1
'I 0 SPI enabled
'H 1
'G 1
'
'R 10 bootsize 256 words (this is strange, the bootstrap starts at &H7800)
'Q 1 Select Reset Vector
'
'Put in your Bascom-AVR code the $loadersize statement: $loadersize = &H800
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'NOTE: THE RAZOR BOARD CAN BE ALSO EQUIPPED WITH A STANDARD 6-PIN-ISP HEADER.
'THIS WAY IT CAN BE FLASHED FROM BASCOM WITHOUT ARDUINO BOOTLOADER. DON'T FORGET
'TO SET THE RESET VECTOR TO ZERO (LOCKS N FUSES) IN THIS CASE.(This is what I did).
'NOTE THAT OTHERS HAVE EXPERIENCED A PROBLEM WITH THE ARDUINO BOOTLOADER ON THE
'RAZOR 9DOF - PLEASE UPDATE THE BOOTLOADER FOR AN AT328 - THERE ARE VERSIONS OF
'THE RAZOR EQUIPPED WITH AN AT168.
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'
'How to connect GPS and the RAZOR 9DOF to the AR7212x connected to a Radiomodem
'==============================================================================
'NOTE THAT ALL PORTS CONNECTED THIS WAY MUST USE THE SAME BAUDRATE. The slowest
'device defines the bottleneck of the whole chain - in this case it is the HAC
'UM96 (9600 bps). In other cases it may be the GPS unit. In a 3rd case it may
'be be the Razor itself, because it is not equipped with a baud rate crystal
'driving it's AT328p.
'
'
' NMEA-> KIX:NMEA+AHRS-> KIX:NMEA+AHRS+X->
' nc ________________ ________________ _________________ nc
' | | | | | | | |
' ___|___|___ ___|___|___ ___|___|___ ___|___|___
' | RX TX | | RX TX | | RX0 TX0 | | RX TX |
' | | | | | | | |
' | GPS | |RAZOR 9DOF | | AR7212X | | HAC UM96 |))) GS
' | Talker | |AHRS/Filter| |RX/Autopilot | Telemetry |
' | | | | | | | |
' |__Gnd_Vcc__| |__Gnd_Vcc__| |__Gnd_Vcc__| |__Gnd_Vcc__|
' | | | | | | | |
' |___|________________O___|________________O___|_________________| |
' | | | |
' |____________________O____________________O_____________________|
'
'
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'Fundamentals
'============
'Prelermani's Direction Cosine Matrix (DCM) algorithm continuously utilizes a
'3x3 rotation matrix R (=DCM) defined by the roll(r) pitch(p) and yaw(y)-angles
'with respect to the fixed reference of earth's directions:
'
' |rxx rxy rxz|
' R = |ryx ryy ryz|
' |rzx rzy rzz|
'
' |cos(r)cos(y) sin(p)sin(r)cos(y)-cos(p)sin(y) cos(p)sin(r)cos(y)+sin(p)sin(y)|
'=|cos(r)sin(y) sin(p)sin(r)sin(y)+cos(p)cos(y) cos(p)sin(r)sin(y)-sin(p)cos(y)|
' | -sin(r) sin(p)cos(r) cos(p)cos(r) |
'
'Rotation matrices are widely employed in coordinate transformations. The latter
'are needed, for example, in 3D computer graphics to allow viewing a 3D-scenery
'from a fixed frame of reference defined by the viewer. Any object in 3D-space
'can be characterized by a collection of points Pi(xi,yi,zi) where xi,yi,zi are the
'coordinates in the frame of reference of the scenery. Coordinate transformation
'means mapping these points to pixels of the computer screen. For the case
'there is no difference in orientation, mapping primarily means scaling. If there
'is a difference in orientation, the angles r,p,y describing this difference can
'be used to transform a point Pi(xi,yi,zi) into Pi'(xi',yi',zi'), where the z'-
'Axis is perpendicular to the x',y'-plane of the computer screen. Points are
'vectors in 3D space. What holds for PO-I-nts (= POsitional Information) holds
'for any other quantity which can be broken into x,y,z-components describing a
'vector: speed, acceleration, and so on. The recipe is simple: For each Vector
'Vi(xi,yi,zi) do a matrix multiplication with the rotation matrix to arrive at the
'Vector Vi' in the new frame of reference.
'
''The DCM is a "combined matrix" (matrix product) of three individual matrices
''describing the rotation around the x-axis, y-axis, and z-axis. With respect to
''an aircraft these axes are parallel to the fuselage (x), parallel to the wings
''(y) and parallel to the rudder (z). Gyros can only measure the rotation around
''these axis in the frame of reference of the aircraft, they ask for the integration
''of angular rates. Coordinate transformation with the DCM maps these local angular
''moves to the Euler angles yaw, pitch and roll with respect to the earth.
''
''Sparkfun's 9DOF Razor board brings in three sources of sensor information:
''(1) Gyros measuring roll-, pitch-, and yaw-rates with respect to the local
'' reference of the plane. Sensor information comes in as analog information and
'' needs to be integrated to find the difference with respect to the last mea-
'' surements: Alpha = Sum (ralpha * dt), where ralpha is the angular rate and dt
'' is the time increment defined by the cycle time (20 ms + x (around 400 µs)).
'' The Kixrazor SW averages ralpha and updates dt from 16 bit Timer1 in the main
'' loop. Analog information is averaged for the whole time the main loop has
'' nothing else to do.
''(2) Accelerometers measuring acceleration in the x,y, and z axis of the plane.
'' This info comes in via I2C, triggered by the timer.
''(3) A magnetometer measuring magnetization with respect to the x,y,z-axis of
'' the plane. This info comes in via I2C every 5th cycle.
''
''Sensor infos (1)..(3) define 3 vectors: The "Gyrovector" (gx, gy, gz), the "Accel-
''vector (ax, ay, az), and the "Magnetovector" (mx, my, mz). These vectors are
''organized as a Sensor matrix S:
''
'' |gx gy gz|
'' S = |ax ay az|
'' |mx my mz|
''
''The DCM algorithm consists of 4 steps:
''
'' (a) Matrix_update
'' (b) Normalize
'' (c) Drift_correction
'' (d) Euler_angles
''
''Matrix_update means a matrix multiplication of the DCM with matrix G from
''the gyro integrals gx, gy, gz written as:
''
'' | 0 -gz gy|
'' G = | gz 0 -gx|
'' |-gy gx 0|
''
''Gyro drift correction is considered by the option to replace the gyro inte-
''grals with PId-corrected counterparts. The new DCM is derived from the matrix
''product R(t).G(t) as follows:
''
'' R(t+dt) = R(t).G(t) + R(t)
''
''At the beginning (t = 0) the DCM (R(0)) comes in as an identity matrix. The
''update thus means adding off-diagonal elements representing gyro information.
''Note that the matrix update is the core of the DCM - steps (b) to (c) could be
''omitted if gyros were drift free. Accelerometer and magnetometer information
''are added for a gyro drift estimate in these steps. At the end the Euler
''angles are computed from the DCM by:
''
'' Pitch = - Asin(rzx)
'' Roll = Atn2(rzy, rzz)
'' Yaw = Atn2(ryx , rxx)
''
''Please find more information on the DCM algorithm on the web. The best info is
''available if you go to Premerlani's homepage on DIYDRONES. Bill has recently
''published a consideration of wind in matrixnavigation whose practical value is
''to estimate airspeed from ground speed (GPS) and DCM information without air-
''speed sensor. The method is build in here.
''
''
$regfile = "m328pdef.dat"
$crystal = 8000000
$baud = 9600
$lib "i2c_twi.lbx" 'HW instead of SW emulated TWI
$loadersize = &H800 'bootloader compatibility
Const Outputmode = 1 'one for drift-corrected data, zero for no correction
Const Print_dcm = 0 'set one for output of dcm matrix
Const Print_analogs = 0 'set one for output of raw gyro data
Const Print_euler = 0 'set one if Euler is to be streamed in nonkixed mode
Const Print_kixeul = 1 'set zero for no kixlined Euler angles
Const Print_kixgps = 1 'set zero for no kixlined GPS
Const Acceladdress = &H0053
Const Compassaddress = &H001E '; //0x3C //0x3D; //(0x42>>1);
Const Kix2 = 2047 'for making KI2 integers
Const Kix3 = 131071 'for making KI3 integers
Const Kix4 = 8388607 'for making KI4 integers
Const Kix5 = 536870911 'for making KI5 integers
Const Gpsstmax = 85
Const Gpsspmax = 13
Declare Function Read_adc(byval Selecti As Integer) As Integer
Declare Sub Compass_heading()
Declare Sub Drift_correction()
Declare Sub Euler_angles()
Declare Sub Gpstime2millisec()
Declare Sub Kixencode()
Declare Sub Matrix_update()
Declare Sub Normalize()
Declare Sub Printdata()
Declare Sub Read_accel()
Declare Sub Get_adc_raw()
Declare Sub Aver_adc_raw()
Declare Sub Read_compass()
Declare Sub Gpsinout()
Declare Sub Ggaparse()
Declare Sub Rmcparse()
Declare Sub Late_lone_dec_conversion()
$hwstack = 255
$swstack = 63
$framesize = 127
'******************************* From original code ****************************
'// Sparkfveun 9DOF Razor IMU AHRS
'// 9 Degree of Measurement Attitude and Heading Reference System
'// Firmware v1.0
'//
'// Released under Creative Commons License
'// Code by Doug Weibel and Jose Julio
'// Based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel
'
'// Axis definition:
' // X axis pointing forward (to the FTDI connector)
' // Y axis pointing to the right
' // and Z axis pointing down.
'// Positive pitch : nose up
'// Positive roll : right wing down
'// Positive yaw : clockwise
'
'/* Hardware version - v13
'
' ATMega328@3.3V w/ external 8MHz resonator
' High Fuse DA
' Low Fuse FF
'
' ADXL345: Accelerometer
' HMC5843: Magnetometer
' LY530: Yaw Gyro
' LPR530: Pitch and Roll Gyro
''**************************** Changed in Bascom *******************************
''
''(1) No free running ADC interrupting the program, instead: Single ADC and
'' averaging is done when main loop is not busy with DCM, GPS, KIX. Sampling
'' of the three analog sensors (gyros) occurs around 7 times per 20 ms cycle,
'' 16 times when KIX and GPS processing is disabled (for comparison). Timebase
'' from 16-bit Timer1 at 1µs resolution. Code philosophy: "Let it flow, but
'' know the time precisely"
''(2) General overwritables instead of local variables. This needs more care
'' during coding but saves the processor from avoidable stack pushs and pops.
''(3) Bascomized variants of Arduino commands for ADC and I2C operations.
''(4) 1D equivalents for Matrix operations as described in AN#176 at MCSelec
'' (Mini Matrix Algebra)
''(5) Kixline support for GPS and Euler Angles as described in AN#174 at MCselec
'' (Kixlines - Hexasexagesimal number compression to speed up serial communi-
'' cations)
''(6) AR7212x compatibility (AN#172)
''******************************************************************************
'From Doug Weibels code
'// ADXL345 Sensitivity(from datasheet) => 4mg/LSB 1G => 1000mg/4mg = 256 steps
'// Tested value : 248
'#define GRAVITY 248 //this equivalent to 1G in the raw data coming from the accelerometer
'#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
'
'#define ToRad(x) (x*0.01745329252) // *pi/180
'#define ToDeg(x) (x*57.2957795131) // *180/pi
'
'// LPR530 & LY530 Sensitivity (from datasheet) => (3.3mv at 3v)at 3.3v: 3mV/Âş/s, 3.22mV/ADC step => 0.93
'// Tested values : 0.92
'#define Gyro_Gain_X 0.92 //X axis Gyro gain
'#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
'#define Gyro_Gain_Z 0.92 //Z axis Gyro gain
'#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
'#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
'#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
'
'#define Kp_ROLLPITCH 0.02
'#define Ki_ROLLPITCH 0.00002
'#define Kp_YAW 1.2
'#define Ki_YAW 0.00002
Dim Gravity As Single
Dim Accel_scale As Single
Dim Gyro_gain_x As Single
Dim Gyro_gain_y As Single
Dim Gyro_gain_z As Single
Dim Gyro_scaled_x As Single
Dim Gyro_scaled_y As Single
Dim Gyro_scaled_z As Single
Dim Kp_rollpitch As Single
Dim Ki_rollpitch As Single
Dim Kp_yaw As Single
Dim Ki_yaw As Single
Dim Torad As Single 'Could be also done by DEG2RAD in Bascom
Dim Todeg As Single 'Could be also done by RAD2DEG in Bascom
Gravity = 248
Accel_scale = Gravity / 9.81
Torad = 0.01745329252
Todeg = 57.2957795131
Gyro_gain_x = 0.92
Gyro_gain_y = 0.92
Gyro_gain_z = 0.92
Gyro_scaled_x = Torad * Gyro_gain_x
Gyro_scaled_y = Torad * Gyro_gain_y
Gyro_scaled_z = Torad * Gyro_gain_z
Kp_rollpitch = 0.02
Ki_rollpitch = 0.00002
Kp_yaw = 1.2
Ki_yaw = 0.00002
'int8_t sensors[3] = {1,2,0}; // Map the ADC channels gyro_x, gyro_y, gyro_z
'int SENSOR_SIGN[9] = {-1,1,-1,1,1,1,-1,-1,-1}; //Correct directions x,y,z - gyros, accels, magnetormeter
'
'float G_Dt=0.02; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible
'
'long timer=0; //general purpuse timer
'long timer_old;
'long timer24=0; //Second timer used to print values
'int AN[6]; //array that store the 3 ADC filtered data (gyros)
'int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors
'int ACC[3]; //array that store the accelerometers data
'
'int accel_x;
'int accel_y;
'int accel_z;
'int magnetom_x;
'int magnetom_y;
'int magnetom_z;
Dim Sensors(3) As Byte
Dim Sensor_sign(9) As Integer
Dim G_dt As Single
Dim Timer_new As Word
Dim Timer_old As Word
Dim An(6) As Integer
Dim An_offset(6) As Integer
Dim Acc(3) As Integer
Dim Accel_x As Integer
Dim Accel_y As Integer
Dim Accel_z As Integer
Dim Magnetom_x As Integer
Dim Magnetom_y As Integer
Dim Magnetom_z As Integer
'float MAG_Heading;
'
'float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
'float Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vector
'float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
'float Omega_P[3]= {0,0,0};//Omega Proportional correction
'float Omega_I[3]= {0,0,0};//Omega Integrator
'float Omega[3]= {0,0,0};
'
'// Euler angles
'float roll;
'float pitch;
'float yaw;
'
'float errorRollPitch[3]= {0,0,0};
'float errorYaw[3]= {0,0,0};
'
'unsigned int counter=0;
'byte gyro_sat=0;
Dim Mag_heading As Single
Dim Accel_vector(3) As Single
Dim Gyro_vector(3) As Single
Dim Omega_vector(3) As Single
Dim Omega_p(3) As Single
Dim Omega_i(3) As Single
Dim Omega(3) As Single
'
'// Euler angles
Dim Roll As Single
Dim Pitch As Single
Dim Yaw As Single
Dim Errorrollpitch(3) As Single
Dim Erroryaw(3) As Single
Dim Cnt As Word
Dim Gyro_sat As Byte
'
'float DCM_Matrix[3][3]= {
' {
' 1,0,0 }
' ,{
' 0,1,0 }
' ,{
' 0,0,1 }
'};
'float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
'
'
'float Temporary_Matrix[3][3]={
' {
' 0,0,0 }
' ,{
' 0,0,0 }
' ,{
' 0,0,0 }
'};
'
Dim Dcm_matrix(9) As Single
Dim Update_matrix(9) As Single
Dim Temporary(9) As Single
Dim Buff(6) As Byte
'//ADC variables
'volatile uint8_t MuxSel=0;
'volatile uint8_t analog_reference;
'volatile uint16_t analog_buffer[8];
'volatile uint8_t analog_count[8];
'//ADC variables
'Dim Muxsel As Byte
'Dim Analog_reference As Byte
Dim Analog_buffer(9) As Word
Dim Analog_count(9) As Byte
'General Overwritables
Dim Iby As Byte , I0by As Byte , Ijby As Byte , Ikby As Byte
Dim Jby As Byte , J0by As Byte , Kby As Byte , K0by As Byte , Kjby As Byte
Dim Lby As Byte , Mby As Byte , Xby As Byte , Yby As Byte , Zby As Byte
Dim Iin As Integer ', Jin As Integer , Kin As Integer , Lin As Integer
Dim Iwo As Word , Jwo As Word , Kwo As Word ', Lwo As Word
Dim Ilo As Long , Jlo As Long , Klo As Long , Llo As Long ', Jlo As Long , Klo As Long , Llo As Long
Dim Isi As Single , Jsi As Single , Ksi As Single , Lsi As Single
Dim Xsi As Single , Ysi As Single , Zsi As Single
Dim Ist As String * 16 , Jst As String * 16 , Kst As String * 16 , Lst As String * 16
Dim Xst As String * 16 , Yst As String * 16
'Globals Kix
Dim Kixinlo As Long
Dim Kixlo(5) As Long
Dim Kixloby(6) As Byte
Dim Kixout As String * 5 At Kixloby(1) Overlay
Dim Kixinby(6) As Byte
Dim Kixin As String * 5 At Kixinby(1) Overlay
Dim Kixint1 As Integer At Kixinby(1) Overlay
Dim Kixint2 As Integer At Kixinby(3) Overlay
Dim Kixdeclo As Long At Kixin Overlay
Dim Kixlineout As String * 50
Dim Kixlinein As String * 50
Dim Intflag As Byte
'Globals Gps
Dim Gpsst As String * Gpsstmax
Dim Gpsiby As Byte
Dim Commas(16) As String * Gpsspmax
Dim Gpsreadyflag As Byte
Dim Rmcvalidflag As Byte
Dim Ggatime As Long
Dim Ggalate As Long
Dim Ggalone As Long
Dim Ggaalte As Long
Dim Ggahode As Long
Dim Ggafixe As Long
Dim Ggasate As Long
Dim Oldggaalte As Long
Dim Oldggatime As Long
Dim Rmcspeed As Long
Dim Rmcheading As Long
Dim Gps_bearing As Single
Dim Dots(2) As String * 15
Dim Gpsimax As Byte
Dim Loopist As String * 1
Dim Scaled_omega_i(3) As Single
Dim Scaled_omega_p(3) As Single
Dim Adc_cnt As Word
Dim Gps_speed As Single
Dim Speed3d As Single
Dim Airspeed As Single
Dim Minrotation As Single
Dim Speedvec(3) As Single
Dim Oldspeedvec(3) As Single
Dim Fuselvec(3) As Single
Dim Oldfuselvec(3) As Single
Minrotation = 0.02
'
Sensors(1) = 2 ''int8_t sensors[3] = {1,2,0};
Sensors(2) = 3 '// Map the ADC channels gyro_x, gyro_y, gyro_z
Sensors(3) = 1
Sensor_sign(1) = -1 'int SENSOR_SIGN[9] = {-1,1,-1,1,1,1,-1,-1,-1};
Sensor_sign(2) = 1 '//Correct directions x,y,z - gyros, accels, magnetormeter
Sensor_sign(3) = -1
Sensor_sign(4) = 1
Sensor_sign(5) = 1
Sensor_sign(6) = 1
Sensor_sign(7) = -1
Sensor_sign(8) = -1
Sensor_sign(9) = -1
G_dt = 0.02 '; // Integration time (DCM algorithm)
'We will run the integration loop at 50Hz if possible
For Iby = 1 To 9
Update_matrix(iby) = Iby
Next Iby
Dcm_matrix(1) = 1
Dcm_matrix(5) = 1
Dcm_matrix(9) = 1
Gpsimax = 80
'float DCM_Matrix[3][3]= {
' {
' 1,0,0 }
' ,{
' 0,1,0 }
' ,{
' 0,0,1 }
'};
'float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
'
'========================= Serial initialisation (UART0) =======================
Config Com1 = 9600 , Synchrone = 0 , Parity = None , Stopbits = 1 , Databits = 8 , Clockpol = 0 'USB
Config Serialin = Buffered , Size = 128 'was 40 before
Config Serialout = Buffered , Size = 128
Enable Interrupts
Enable Serial
'========================= Timer initialisation ================================
Config Timer1 = Timer , Prescale = 8 '16 bit timer checked in main loop
Start Timer1
'========================== Port initialisation ================================
'Special port assignments Atmega168/328p
' PB0
' PB1
' PB2
' PB3 MOSI
' PB4 MISO
' PB5 SCK/LED
' PB6
' PB7
'
' PC0 ADC0 ZOUT 4X
' PC1 ADC1 XOUT 4X
' PC2 ADC2 YOUT 4X
' PC3 ADC3
' PC4 SDA
' PC5 SCL
' PC6 ADC0 VREF1
' PC7 ADC0 VREF2
'
' PD0 RX In
' PD1 TX Out
' PD2 ST
' PD3 PD
' PD4 HP
'
Ddrb = &B00101000
Portb = &B00010000 'setup internal pull up resistors at input pins
Status_led Alias Portb.5
'========================== ADC initialsation (gyros) ==========================
Config Adc = Single , Prescaler = Auto , Reference = Avcc
Start Adc
'========================= General I2c initialisation ==========================
Config Sda = Portc.4
Config Scl = Portc.5
Config I2cdelay = 10
I2cinit ' we need to set the pins in the proper state
' // SCL freq = F_CPU/(16+2*TWBR*4^TWPS)
' TWBR = 12
' // SCL freq = F_CPU/(16+2*TWBR))
' = 8000000/40 = 200000
Config Twi = 400000 ' wanted clock frequency
'will set TWBR and TWSR
'Twbr = 12 'bit rate register
'Twsr = 0 'pre scaler bits
'======================= I2c accelerometer initialisation ======================
I2cstart : I2cwbyte &HA6 : I2cwbyte &H2D : I2cwbyte &H08 'power register: measurement mode
I2cstart : I2cwbyte &HA6 : I2cwbyte &H31 : I2cwbyte &H08 'data format: full resolution
I2cstart : I2cwbyte &HA6 : I2cwbyte &H2C : I2cwbyte &H09 'rate: 50 Hz CHECK!!!!! better free
Read_accel
Print "KIXRAZOR 1.0: SFE RAZOR 9DOF wth KIX/GPS "
Reset Status_led
Waitms 1500
'======================= I2C magnetometer initialization =======================
I2cstart : I2cwbyte &H3C : I2cwbyte &H02 : I2cwbyte &H00 : I2cstop 'continuous mode
'Read_adc_raw
Waitms 20
'========== Find averaged offsets for ADC gyros and I2C-Accelerometers =========
For Kby = 1 To 32 '32 Cycles a 20 ms
For Lby = 1 To 50 '50 ADC samples per cycle
Get_adc_raw
Next Lby
Aver_adc_raw 'No Read_adc here
Read_accel
For Mby = 1 To 6
An_offset(mby) = An(mby) + An_offset(mby)
Next Mby
Waitms 20
Next Kby
For Jby = 1 To 6
An_offset(jby) = An_offset(jby) / 32
Next Jby
Iin = Gravity * Sensor_sign(6)
An_offset(6) = An_offset(6) - Iin 'ATTENTION is 249
Print "Offset: ";
For Jby = 1 To 6
Print " " ; Str(an_offset(jby));
Next Jby
Print
Waitms 2000
Set Status_led
Cnt = 0
Read_compass
Print "end of pre test"
Waitms 500
'###############################################################################
'################################ MAIN LOOP START ##############################
'###############################################################################
Do
Timer_new = Timer1
If Timer_new >= 20000 Then '20000 for 20 ms cycle time, it may be better to set
Incr Cnt '30000 for more oversampling
Timer1 = 0
G_dt = Timer_new
G_dt = G_dt / 1000000 '/ / Real Time Of Loop Run. We Use This On The Dcm Algorithm(gyro Integration Time)
Aver_adc_raw '; // This read gyro data
Read_accel '; // Read I2C accelerometer
If Cnt >= 5 Then ' // Read compass data at 10Hz... (5 loop runs)
Cnt = 0
Read_compass '; // Read I2C magnetometer
Compass_heading '; // Calculate magnetic heading
End If
' // Calculations...
Matrix_update
Normalize
Drift_correction
Euler_angles
' // ***
'
'Print Adc_cnt ; " ";
Printdata
Adc_cnt = 0
' //Turn off the LED when you saturate any of the gyros.
Isi = Torad * 300
Kby = 0
For Iby = 1 To 3
Jsi = Abs(gyro_vector(iby))
If Jsi >= Isi Then
Incr Kby
End If
Next Iby
If Kby >= 1 Then
If Gyro_sat < 50 Then
Gyro_sat = Gyro_sat + 10
End If
Else
If Gyro_sat >= 1 Then
Gyro_sat = Gyro_sat - 1
End If
End If
If Gyro_sat >= 1 Then
Reset Status_led
Else
Set Status_led
End If
End If
Gpsinout
Get_adc_raw 'Get adc if nothing else to do
Incr Adc_cnt
Loop
'*******************************************************************************
'******************** Read GPS and convert into Kixline ************************
'*******************************************************************************
Sub Gpsinout
Do
Iby = Ischarwaiting()
If Iby <> 0 Then
Iby = Inkey()
Loopist = Chr(iby)
If Loopist = "$" Then
Gpsst = Loopist
Gpsiby = 1
Elseif Loopist = Chr(13) Then '
Ist = Left(gpsst , 6)
If Ist = "$GPGGA" Then
'If Gpsreadyflag = 0 Then Print Gpsst
Ggaparse
'$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
Elseif Ist = "$GPRMC" Then
'
'Print Gpsst
Rmcparse
'$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
'Make Kixline:
'ID = } 1
'GGASTATUS = GGAFIXE + 2*RMCValidflag + 4*Ggasate --> KW1.0
'GGAHODE (0..6.3 m) --> KW1.1
'GGATIME (0..86400000 ms) --> KW5.0
'GGALATE (0..+/- 90000000 (10^6°) --> KI5.0
'GGALONE (0..+/-180000000 (10^6°) --> KI5.0
'GGAALTE (0..+/-13107.1 m --> KI3.1
'RMCHEADING (-180.0..+180.0 --> KI2.1
'RMCSPEED (0..409.5 km/h) --> KW2.1
'+CRLF 2
'---------------------------------------------------------
' 27 Byte (of ca. 136 Byte = ca. 20%)
Kixlineout = "}" 'Message Type GGA+RMC
If Ggafixe >= 1 Then Ggafixe = 1
If Ggasate >= 15 Then Ggasate = 15
Ilo = 4 * Ggasate : Jlo = 2 * Rmcvalidflag : Ilo = Ilo + Jlo : Ilo = Ilo + Ggafixe
Kixinlo = Ilo : Kixencode : Kixlineout = Kixlineout + Left(kixout , 1) 'KW1.0
Ilo = Ggahode
Kixinlo = Ilo : Kixencode : Kixlineout = Kixlineout + Left(kixout , 1) 'KW1.1
Kixinlo = Ggatime : Kixencode : Kixlineout = Kixlineout + Kixout ' 'KW5.0
Kixinlo = Ggalate + Kix5 : Kixencode : Kixlineout = Kixlineout + Kixout 'KI5.0
Kixinlo = Ggalone + Kix5 : Kixencode : Kixlineout = Kixlineout + Kixout 'KI5.0
Kixinlo = Ggaalte + Kix3 : Kixencode : Kixlineout = Kixlineout + Left(kixout , 3) 'KI3.0
Kixinlo = Rmcheading + Kix2 : Kixencode : Kixlineout = Kixlineout + Left(kixout , 2) 'KI2.1
Kixinlo = Rmcspeed : Kixencode : Kixlineout = Kixlineout + Left(kixout , 2) 'KW2.1
'THIS TO BE ADDED FOR 3D-SPEED + AIRSPEED BY DCM.
Kixinlo = Speed3d * 36 : Kixencode : Kixlineout = Kixlineout + Left(kixout , 2) 'KW2.1
Kixinlo = Airspeed * 36 : Kixencode : Kixlineout = Kixlineout + Left(kixout , 2) 'KW2.1
Print Kixlineout
Else
'Do nothing
End If
Elseif Loopist = Chr(10) Then 'Line feed
'Do nothing
Else
If Gpsiby < Gpsstmax Then
Incr Gpsiby
Gpsst = Gpsst + Loopist
Else
Print "Error: Gpslen"
Gpsst = ""
Gpsiby = 0
End If
End If
End If
Loop Until Ischarwaiting() = 0
End Sub
'*******************************************************************************
'************************** Get ADC raw data ******************************
'************************** if nothing else to do ******************************
'*******************************************************************************
Sub Get_adc_raw()
'Read_adc_raw
For Iby = 1 To 3
Jby = Iby - 1
Iwo = Getadc(jby)
Jby = Analog_count(iby)
If Jby < 63 Then
Incr Analog_count(iby)
Analog_buffer(iby) = Analog_buffer(iby) + Iwo
End If
Next Iby
End Sub
'*******************************************************************************
'************************** Average ADC raw data *******************************
'************************** every 20 ms cycle *******************************
'*******************************************************************************
Sub Aver_adc_raw()
For Iby = 1 To 3
Jby = Sensors(iby)
Jwo = Analog_buffer(jby) '; / / Sensors[] Maps Sensors To Correct Order
Xby = Analog_count(jby) ';
If Xby <> 0 Then An(jby) = Jwo / Xby
Analog_buffer(jby) = 0
Analog_count(jby) = 0
Next Iby
End Sub
'*******************************************************************************
'************************* Sign correction of ADC ****************************
'************************* Came in from Bill's axis ****************************
'*******************************************************************************
Function Read_adc(byval Selecti As Integer) As Integer
If Sensor_sign(selecti) < 0 Then
Read_adc = An_offset(selecti) - An(selecti)
Else
Read_adc = An(selecti) - An_offset(selecti)
End If
End Function
'*******************************************************************************
'***************************** Compass_heading *********************************
'*******************************************************************************
Sub Compass_heading()
Isi = Cos(roll)
Jsi = Sin(roll)
Ksi = Cos(pitch) 'checked for double occupation Ki
Lsi = Sin(pitch)
' // Tilt compensated Magnetic filed X:
' MAG_X = magnetom_x*cos_pitch + magnetom_y*sin_roll*sin_pitch + magnetom_z*cos_roll*sin_pitch;
Xsi = Magnetom_x * Ksi ' * cos (pitch)
Ksi = Jsi * Lsi ' = sin (roll) * sin (pitch)
Ksi = Ksi * Magnetom_y
Xsi = Xsi + Ksi
Ksi = Isi * Lsi ' = cos (roll) * Sin (Pitch)
Ksi = Ksi * Magnetom_z
Xsi = Xsi + Ksi
' // Tilt compensated Magnetic filed Y:
' MAG_Y = magnetom_y*cos_roll - magnetom_z*sin_roll;
Ysi = Magnetom_y * Isi '* cos (roll)
Ksi = Magnetom_z * Jsi '* sin (roll)
Ysi = Ksi - Ysi
' // Magnetic Heading
Mag_heading = Atn2(ysi , Xsi)
End Sub
'*******************************************************************************
'********************************* Normalize ***********************************
'*******************************************************************************
Sub Normalize()
Lsi = 0 'contains error
For Iby = 1 To 3
Jby = 3 + Iby
Isi = Dcm_matrix(iby) * Dcm_matrix(jby)
Lsi = Lsi + Isi
Next Iby
Lsi = Lsi * .5
Lsi = 0 - Lsi
'
' Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
For Iby = 1 To 3
Jby = Iby + 3
Temporary(iby) = Dcm_matrix(jby) * Lsi
Next Iby
' Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
For Iby = 4 To 6
Jby = Iby - 3
Temporary(iby) = Dcm_matrix(jby) * Lsi
Next Iby
' Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
' Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
For Iby = 1 To 6
Temporary(iby) = Temporary(iby) + Dcm_matrix(iby)
Next Iby
'Sub Vector_cross_product(vectorout(1) As Single , V1(1) As Single , V2(1) As Single) '2 x ===> discrete
'//Computes the cross product of two vectors
' Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
Isi = Temporary(2) * Temporary(6)
Jsi = Temporary(3) * Temporary(5)
Temporary(7) = Isi - Jsi
Isi = Temporary(3) * Temporary(4)
Jsi = Temporary(1) * Temporary(6)
Temporary(8) = Isi - Jsi
Isi = Temporary(1) * Temporary(5)
Jsi = Temporary(2) * Temporary(4)
Temporary(9) = Isi - Jsi
' renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
' Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
Lsi = 0 'contains renorm
For Iby = 1 To 3
Isi = Dcm_matrix(iby) * Dcm_matrix(iby)
Lsi = Lsi + Isi
Next Iby
Lsi = 3 - Lsi
Lsi = .5 * Lsi
For Iby = 1 To 3
Dcm_matrix(iby) = Temporary(iby) * Lsi
Next Iby
' renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
' Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
Lsi = 0
For Iby = 4 To 6
Isi = Dcm_matrix(iby) * Dcm_matrix(iby)
Lsi = Lsi + Isi
Next Iby
Lsi = 3 - Lsi
Lsi = .5 * Lsi
For Iby = 4 To 6
Dcm_matrix(iby) = Temporary(iby) * Lsi
Next Iby
' renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
' Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
Lsi = 0
For Iby = 7 To 9
Isi = Dcm_matrix(iby) * Dcm_matrix(iby)
Lsi = Lsi + Isi
Next Iby
Lsi = 3 - Lsi
Lsi = .5 * Lsi
For Iby = 7 To 9
Dcm_matrix(iby) = Temporary(iby) * Lsi
Next Iby
End Sub
'*******************************************************************************
'***************************** Drift correction ********************************
'*******************************************************************************
Sub Drift_correction()
' //*****Roll and Pitch***************
' // Calculate the magnitude of the accelerometer vector
' Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
' Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
Isi = Accel_vector(1)
Isi = Isi * Isi
Jsi = Isi
Isi = Accel_vector(2)
Isi = Isi * Isi
Jsi = Jsi + Isi
Isi = Accel_vector(3)
Isi = Isi * Isi
Jsi = Jsi + Isi
Jsi = Sqr(jsi)
Jsi = Jsi / Gravity 'Accel magnitude scaled to g
' // Dynamic weighting of accelerometer info (reliability filter)
' // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
' Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
Jsi = 1 - Jsi
Jsi = Abs(jsi)
Jsi = Jsi * 2
Jsi = 1 - Jsi
If Jsi < 0 Then
Jsi = 0
Elseif Jsi >= 1 Then
Jsi = 1
End If
Lsi = Jsi 'Lsi is Accel_Weight
' Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
'//Computes the cross product of two vectors
'void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
'{
'vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
'vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
'vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
'}
'cross-product needed 2 times: thus no code splitting
Isi = Accel_vector(2) * Dcm_matrix(9)
Jsi = Accel_vector(3) * Dcm_matrix(8)
Errorrollpitch(1) = Isi - Jsi
Isi = Accel_vector(3) * Dcm_matrix(7)
Jsi = Accel_vector(1) * Dcm_matrix(9)
Errorrollpitch(2) = Isi - Jsi
Isi = Accel_vector(1) * Dcm_matrix(8)
Jsi = Accel_vector(2) * Dcm_matrix(7)
Errorrollpitch(3) = Isi - Jsi
' Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
For Iby = 1 To 3
Isi = Errorrollpitch(iby) * Lsi
Omega_p(iby) = Isi * Kp_rollpitch
Next Iby
' Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
' Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
'
For Iby = 1 To 3
Isi = Scaled_omega_i(iby) * Lsi
Scaled_omega_i(iby) = Isi * Ki_rollpitch
Omega_i(iby) = Omega_i(iby) + Scaled_omega_i(iby)
Next Iby
' //*****YAW***************
' // We make the gyro YAW drift correction based on compass magnetic heading
'
' mag_heading_x = cos(MAG_Heading);
' mag_heading_y = sin(MAG_Heading);
' errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
' Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Xsi = Cos(mag_heading)
Ysi = Sin(mag_heading)
Isi = Dcm_matrix(1) * Ysi
Jsi = Dcm_matrix(4) * Xsi
Zsi = Isi - Jsi
For Iby = 1 To 3
Jby = Iby + 6
Erroryaw(iby) = Dcm_matrix(jby) * Zsi
Next Iby
' Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
' Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
' Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
' Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
For Iby = 1 To 3
Scaled_omega_p(iby) = Erroryaw(iby) * Kp_yaw
Omega_p(iby) = Omega_p(iby) + Scaled_omega_p(iby)
Scaled_omega_i(iby) = Erroryaw(iby) * Ki_yaw
Omega_i(iby) = Omega_i(iby) + Scaled_omega_i(iby)
Next Iby
End Sub
'*******************************************************************************
'****************************** Matrix-Update **********************************
'*******************************************************************************
Sub Matrix_update()
Iin = Read_adc(1) : Isi = Iin
Gyro_vector(1) = Isi * Gyro_scaled_x '; //gyro x roll
Iin = Read_adc(2) : Isi = Iin
Gyro_vector(2) = Isi * Gyro_scaled_y '; //gyro y pitch
Iin = Read_adc(3) : Isi = Iin
Gyro_vector(3) = Isi * Gyro_scaled_z '; //gyro z yaw
Accel_vector(1) = Accel_x
Accel_vector(2) = Accel_y
Accel_vector(3) = Accel_z
' Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
For Iby = 1 To 3 : Omega(iby) = Omega_i(iby) + Gyro_vector(iby) : Next Iby
' Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
For Iby = 1 To 3 : Omega_vector(iby) = Omega(iby) + Omega_p(iby) : Next Iby
' //Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
' Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
' Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
'@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
'Yes we have (NK):
'Extension for 3D speed - info calculated in RMCparser - airspeed is also calculated there
'Will be used later.
Isi = Speed3d * Omega(3)
Accel_vector(2) = Accel_vector(2) + Isi
Isi = Speed3d * Omega(2)
Accel_vector(3) = Accel_vector(3) - Isi
'
#if Outputmode = 1
Update_matrix(1) = 0
Update_matrix(2) = G_dt * Omega_vector(3) ';//z
Update_matrix(2) = 0 - Update_matrix(2) ';//-z
Update_matrix(3) = G_dt * Omega_vector(2) ';//y
Update_matrix(4) = G_dt * Omega_vector(3) ';//z
Update_matrix(5) = 0
Update_matrix(6) = G_dt * Omega_vector(1) ';//x
Update_matrix(6) = 0 - Update_matrix(6) ';//-x
Update_matrix(7) = G_dt * Omega_vector(2) ';//y
Update_matrix(7) = 0 - Update_matrix(7) ';//-y
Update_matrix(8) = G_dt * Omega_vector(1) ';//x
Update_matrix(9) = 0
#else ' // Uncorrected data (no drift correction)
Update_matrix(1) = 0
Update_matrix(2) = G_dt * Gyro_vector(3) ';//z
Update_matrix(2) = 0 - Update_matrix(2) ' ;//-z
Update_matrix(3) = G_dt * Gyro_vector(2) ';//y
Update_matrix(4) = G_dt * Gyro_vector(3) ';//z
Update_matrix(5) = 0
Update_matrix(6) = G_dt * Gyro_vector(1)
Update_matrix(6) = 0 - Update_matrix(6)
Update_matrix(7) = G_dt * Gyro_vector(2)
Update_matrix(7) = 0 - Update_matrix(7)
Update_matrix(8) = G_dt * Gyro_vector(1)
Update_matrix(9) = 0
#endif
'Matrix_multiply(dcm_matrix , Update_matrix , Temporary_matrix) '; //a*b=c
'Sub Matrix_multiply(a(1) As Single , B(1) As Single , Mat(1) As Single) 'just 1x
For Iby = 1 To 3 'Rows
I0by = Iby - 1 '0,1,2
I0by = I0by * 3 '0,3,6
For Jby = 1 To 3 'Columns
Ijby = I0by + Jby 'holds sum
Temporary(ijby) = 0 'summing up elements E(i,k) * E(k,j) in Temp(i,j)
For Kby = 1 To 3
K0by = Kby - 1 '0,1,2
K0by = K0by * 3 '0,3,6
Kjby = K0by + Jby 'Element (k,j)
Ikby = I0by + Kby 'Element (i,k)
Jsi = Dcm_matrix(ikby) * Update_matrix(kjby)
Temporary(ijby) = Temporary(ijby) + Jsi
Next Kby
Next Jby
Next Iby
'//Matrix Addition (update)
For Iby = 1 To 9
Dcm_matrix(iby) = Dcm_matrix(iby) + Temporary(iby)
Next Iby
End Sub
'*******************************************************************************
'******************************** Euler_angles *********************************
'*******************************************************************************
Sub Euler_angles()
Pitch = 0 - Asin(dcm_matrix(7))
Roll = Atn2(dcm_matrix(8) , Dcm_matrix(9))
Yaw = Atn2(dcm_matrix(4) , Dcm_matrix(1))
End Sub
'*******************************************************************************
'******************* Reads x,y and z accelerometer registers *******************
'*******************************************************************************
Sub Read_accel()
For Iby = 1 To 6
I2cstart
I2cwbyte &HA6
Jby = &H31 + Iby
I2cwbyte Jby
I2cstart
I2cwbyte &HA7
I2crbyte Xby
I2crbyte Yby , Nack
Buff(iby) = Yby
Next Iby
Acc(2) = Makeint(buff(1) , Buff(2))
Acc(1) = Makeint(buff(3) , Buff(4))
Acc(3) = Makeint(buff(5) , Buff(6))
An(4) = Acc(1)
An(5) = Acc(2)
An(6) = Acc(3)
Accel_x = An(4) - An_offset(4) : Accel_x = Accel_x * Sensor_sign(4)
Accel_y = An(5) - An_offset(5) : Accel_y = Accel_y * Sensor_sign(5)
Accel_z = An(6) - An_offset(6) : Accel_z = Accel_z * Sensor_sign(6)
End Sub
'*******************************************************************************
'******************************** Read Compass *********************************
'*******************************************************************************
Sub Read_compass()
'Test on readability of status reg
I2cstart
I2cwbyte &H3C
I2cwbyte &H02
I2cwbyte &H00
I2cstart
I2cwbyte &H3C
I2cwbyte &H09
I2cstart
I2cwbyte &H3D
I2crbyte Xby , Ack
I2crbyte Yby , Nack
Yby = Xby And &B00000001
If Yby = 0 Then Exit Sub 'Bit 0 is decisive - needs to be 1
I2cstart
I2cwbyte &H3C
I2cwbyte &H02
I2cwbyte &H00
I2cstart
I2cwbyte &H3D
For Iby = 1 To 6
I2crbyte Buff(iby) , Ack
Next Iby
Magnetom_x = Makeint(buff(4) , Buff(3))
Magnetom_y = Makeint(buff(2) , Buff(1))
Magnetom_z = Makeint(buff(6) , Buff(5))
Magnetom_x = Magnetom_x * Sensor_sign(7)
Magnetom_y = Magnetom_y * Sensor_sign(8)
Magnetom_z = Magnetom_z * Sensor_sign(9)
'atn2 (Magnetom_y, Magnetom_x) gives heading for sensor without tilt
Xsi = Magnetom_x
Jsi = Magnetom_y
Xsi = Magnetom_x
Ysi = Jsi / Isi
Isi = Atn2(ysi , Xsi)
Jsi = Atn2(xsi , Ysi)
Isi = Rad2deg(isi)
Jsi = Rad2deg(jsi)
End Sub
'*******************************************************************************
'********************* Output similar to original Razor ************************
'*******************************************************************************
Sub Printdata()
'{
#if Print_kixeul = 1
'Make Kixline:
Kixlineout = "~" 'for Roll, pitch, yaw
Isi = Todeg * Roll : Jsi = Todeg * Pitch : Ksi = Todeg * Yaw
Isi = Isi * 10 : Jsi = Jsi * 10 : Ksi = Ksi * 10
Ilo = Isi : Jlo = Jsi : Klo = Ksi
Kixinlo = Ilo + Kix2 : Kixencode : Kixlineout = Kixlineout + Left(kixout , 2) 'KI2.0
Kixinlo = Jlo + Kix2 : Kixencode : Kixlineout = Kixlineout + Left(kixout , 2) 'KI2.0
Kixinlo = Klo + Kix2 : Kixencode : Kixlineout = Kixlineout + Left(kixout , 2) 'KI2.0
Print Kixlineout
#endif
#if Print_euler = 1
Print "ANG:";
Isi = Todeg * Roll
Jsi = Todeg * Pitch
Ksi = Todeg * Yaw
Lsi = Todeg * Mag_heading
Ist = Fusing(isi , "#.#")
Ist = " " + Ist
Ist = Right(ist , 6)
Jst = Fusing(jsi , "#.#")
Jst = " " + Jst
Jst = Right(jst , 6)
Kst = Fusing(ksi , "#.#")
Kst = " " + Kst
Kst = Right(kst , 6)
Lst = Fusing(lsi , "#.#")
Lst = " " + Lst
Lst = Right(lst , 6)
Print Ist ; Jst ; Kst
#endif
#if Print_analogs = 1
Print ",AN:";
For Iby = 1 To 3
Jby = Sensors(iby)
Iin = An(jby)
Ist = Str(iin)
Print Ist ; ",";
Next Iby
For Iby = 1 To 3
Iin = Acc(iby)
Ist = Str(iin)
Print Ist ; ",";
Next Iby
Ist = Str(magnetom_x)
Jst = Str(magnetom_y)
Kst = Str(magnetom_z)
Print Ist ; "," ; Jst ; "," ; Kst
#endif
#if Print_dcm = 1
Print ",DCM:";
For Iby = 1 To 9
Isi = Dcm_matrix(iby)
Print Fusing(isi , "#.#####") ; " ";
Next Iby
Print Chr(13);
#endif
End Sub
'*******************************************************************************
'*************************** Parse NMEA GGA string *****************************
'*******************************************************************************
Sub Ggaparse()
'Sentence GGA
'Function: Global Positioning Fix Data
'Example: $GPGGA,120757,5152.985,N,00205.733,W,1,06,2.5,121.9,M,49.4,M,,*52 (64)
' 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
'EB85-A: '$GPGGA,100717.561,5125.654242,N,00724.612089,E,0,0,,184.153,M,47.499,M,,*4A
' 2 3 4 5 6 7 8 9 10 11 12 13 14
'Synopsis:
' 2 time of fix (hhmmss)
' 3 latitude
' 4 N/S
' 5 longitude
' 6 E/W
' 7 Fix quality (0=invalid, 1=GPS fix, 2=DGPS fix)
' 8 number of satellites being tracked
' 9 horizontal dilution of position
'10 altitude above sea level
'11 M (meters)
'12 height of geoid (mean sea level) above WGS84 ellipsoid
'13 time in seconds since last DGPS update
'14 DGPS station ID number
'15 checksum
Iby = Split(gpsst , Commas(1) , ",")
If Iby <> 14 Then
Print
Print "Gga!" ; Iby
'Exit Sub
End If
Oldggatime = Ggatime
Kst = Commas(2) : Gpstime2millisec 'Time
Kst = Commas(3) : Late_lone_dec_conversion : Ggalate = Ilo 'Latitude
If Commas(4) = "S" Then Ggalate = 0 - Ggalate 'N/S
Kst = Commas(5) : Late_lone_dec_conversion : Ggalone = Ilo 'Longitude
If Commas(6) = "W" Then Ggalone = 0 - Ggalone 'E/W
Ggafixe = Val(commas(7)) 'Fix quality
Ggasate = Val(commas(8)) 'Number of sats
Isi = Val(commas(9)) : Isi = Isi * 10 : Ggahode = Isi
Oldggaalte = Ggaalte
Isi = Val(commas(10)) : Isi = Isi * 10 : Ggaalte = Isi
End Sub
'*******************************************************************************
'*************************** Parse NMEA RMC string *****************************
'*******************************************************************************
Sub Rmcparse()
'Sentence RMC
'Function: Recommended minimum specific GPS transit data
'eg3. $GPRMC,220516,A,5133.82,N,00042.24,W,173.8,231.8,130694,004.2,W*70
' 2 3 4 5 6 7 8 9 10 11 12 13
'
' 2 220516 Time Stamp
' 3 A validity - A-ok, V-invalid
' 4 5133.82 current Latitude
' 5 N North/South
' 6 00042.24 current Longitude
' 7 W East/West
' 8 173.8 Speed in knots
' 9 231.8 True course
' 10 130694 Date Stamp
' 11 004.2 Variation
' 12 W East/West
' 13 *70 checksum
Rmcvalidflag = 0
Iby = Split(gpsst , Commas(1) , ",")
If Iby <> 11 Then
Print
Print "Rmc!" ; Iby
End If
'Kst = Commas(2): Gpstime2millisec 'Time
If Commas(3) = "A" Then Rmcvalidflag = 1 'Validity
'Kst = Commas(4) : Late_lone_dec_conversion : Ggalate = Ilo 'Latitude
'If Commas(5) = "S" Then Ggalate = 0 - Ggalate 'N/S
'Kst = Commas(6) : Late_lone_dec_conversion : Ggalone = Ilo 'Longitude
'If Commas(7) = "S" Then Ggalone = 0 - Ggalone 'E/W
Isi = Val(commas(8)) : Isi = Isi * 0.5147 : Gps_speed = Isi : Isi = Isi * 36 : Rmcspeed = Isi 'speed in 10 * Km/h
Isi = Val(commas(9)) : Gps_bearing = Isi : Isi = Isi * 10 : Rmcheading = Isi 'heading in 10 * °
'Estimation of Speed3d over Ground
Isi = Ggaalte - Oldggaalte 'in 10 * m
Jsi = Ggatime - Oldggatime 'in 1000 * s (ms units)
If Jsi = 0 Then Exit Sub 'should never happen, but safe is safe
Isi = Isi / Jsi : Zsi = Isi * 100 'vspeed in m/s
Isi = Zsi * Zsi 'vspeed^2
Jsi = Gps_speed * Gps_speed 'hspeed^2
Isi = Isi + Jsi
Speed3d = Sqr(isi) 'speed3d
'Estimation of Airspeed
' |Speedvec - Oldspeedvec|
' Premerlani: Airspeed = ------------------------
' |Fuselvec - Oldfuselvec|
For Iby = 1 To 3
Oldspeedvec(iby) = Speedvec(iby)
Oldfuselvec(iby) = Fuselvec(iby)
Next Iby
Isi = Deg2rad(gps_bearing)
Speedvec(1) = Gps_speed * Sin(isi) 'vx
Speedvec(2) = Gps_speed * Cos(isi) 'vy
Speedvec(3) = Zsi 'vz
Fuselvec(1) = Dcm_matrix(2)
Fuselvec(2) = Dcm_matrix(5)
Fuselvec(3) = Dcm_matrix(8)
Xsi = Speedvec(1) - Oldspeedvec(1) 'dvx
Ysi = Speedvec(2) - Oldspeedvec(2) 'dvy
Zsi = Speedvec(3) - Oldspeedvec(3) 'dvz
Xsi = Xsi * Xsi
Ysi = Ysi * Ysi
Zsi = Zsi * Zsi
Isi = Xsi + Ysi
Isi = Isi + Zsi
Isi = Sqr(isi) 'magnitude of airspeed vector difference
Xsi = Fuselvec(1) - Oldfuselvec(1)
Ysi = Fuselvec(2) - Oldfuselvec(2)
Zsi = Fuselvec(3) - Oldfuselvec(3)
Xsi = Xsi * Xsi
Ysi = Ysi * Ysi
Zsi = Zsi * Zsi
Jsi = Xsi + Ysi
Jsi = Jsi + Zsi
Jsi = Sqr(jsi) 'magnitude of fuselage vector difference
If Jsi >= Minrotation Then 'Value of minrotation needs to be checked
Airspeed = Isi / Jsi
End If
End Sub
'*******************************************************************************
'**************************** GPS Time to millisec *****************************
'*******************************************************************************
Sub Gpstime2millisec()
'Find Time and convert into seconds (long)
'Ggatime = 3600 * Val(mid(gpsparseist , 1 , 2))
'+ 60 * Val(mid(gpsparseist , 3 , 2))
'+ Val(gpsparseist , 5 , 2))
'Ggatim0 = 0
Ist = Mid(kst , 1 , 2) 'h
Jlo = Val(ist)
Jlo = 3600 * Jlo
Ilo = Jlo
Ist = Mid(kst , 3 , 2) 'm
Jlo = Val(ist)
Jlo = 60 * Jlo
Ilo = Ilo + Jlo
Ist = Mid(kst , 5 , 2) 's
Jlo = Val(ist)
Ilo = Ilo + Jlo
Ggatime = 1000 * Ilo '>1Hz GPS in Millisec
Iby = Instr(kst , ".")
If Iby = 0 Then
Ilo = 0
Else
Ist = Right(kst , 3)
Ist = Ist + "000"
Ist = Left(ist , 3)
Ilo = Val(ist)
End If
Ggatime = Ggatime + Ilo
End Sub
'*******************************************************************************
'******** Latitude and longitude conversion from °°MM.SSSS to °°xxxxxx *********
'*******************************************************************************
Sub Late_lone_dec_conversion()
'$GPGGA,100717.561,5125.654242,N,00724.612089,E,0,0,,184.153,M,47.499,M,,*4A
'$GPRMC,100717.561,V,5125.654242,N,00724.612089,E,0.000,0.00,080410,,,N*4F
'Lby = Len(kst) '5125.654242 = 51° 25.654242' has len 11
'00724.612089 = 7° 24.612089' has len 12
Iby = Split(kst , Dots(1) , ".") 'Dots(1) = "5125" Dots(2) = "654242"
Lby = Len(dots(1)) 'Lby = 4 for late, 5 for lone
Lby = Lby - 2 ' 2 for late, 3 for lone
Ist = Left(dots(1) , Lby) 'Ist = "51" for late, "007" for lone
Jst = Right(dots(1) , 2) 'Jst = "25" for late, "24" for lone
Jst = Jst + Dots(2) 'Jst = "25654242" for late, "24612089" for lone
Lby = Len(jst) 'Lby = 8
If Lby < 8 Then
Jst = Jst + "00000000" 'Jst = "2565424200000000" 'for the case of zero suppression
Jst = Left(jst , 8) 'Jst = "25654242"
End If
Ilo = Val(ist) 'Ilo = 51
Ilo = Ilo * 1000000 'Ilo = 51000000
Jlo = Val(jst) 'Jlo = 25654242
Jlo = Jlo / 60 'Jlo = 427571
Ilo = Ilo + Jlo 'Ilo = 51427571 'means resolution of .2m per unit
End Sub
'*******************************************************************************
'********************************** Kix encoder ********************************
'*******************************************************************************
Sub Kixencode()
Kixlo(1) = Kixinlo And &B00000000000000000000000000111111
Kixlo(2) = Kixinlo And &B00000000000000000000111111000000
Kixlo(3) = Kixinlo And &B00000000000000111111000000000000
Kixlo(4) = Kixinlo And &B00000000111111000000000000000000
Kixlo(5) = Kixinlo And &B00111111000000000000000000000000
Jby = 0
For Iby = 1 To 5
Rotate Kixlo(iby) , Right , Jby
Jby = Jby + 6
Kby = Kixlo(iby)
Kixloby(iby) = Kby + 48
Next Iby
'Print Kixout
End Sub
|
