Thursday, 28 March 2024
     
 
Main Menu
Home Home
Shop Shop
News News
BASCOM-AVR BASCOM-AVR
BASCOM-8051 BASCOM-8051
Products Products
Application Notes Application Notes
Publications Publications
Links Links
Support Center Support Center
Downloads Downloads
Forum Forum
Resellers Resellers
Contact Us Contact Us
Updates Updates
MCS Wiki MCS Wiki
Online Help
BASCOM-AVR Help BASCOM-AVR Help
BASCOM-8051 Help BASCOM-8051 Help
Contents in Cart
Show Cart
Your Cart is currently empty.
Search the Shop

Products Search

User Login
Username

Password

If you have problem after log in with disappeared login data, please press F5 in your browser

RSS News
 
     
 

 
   
     
 
AN #177 - Kixrazor – Bascomer’s Electronic Flight Information System for Sparkfun’s 9DOF Razor Print

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 = .* 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 = .* 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 = .* 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