Tuesday, September 15, 2015

Linux Experience

This Year I was very busy with developing Andruav.com an RC related android application. This September  2015 I finish 1 year working on it. It is a sophisticated app to develop, however I hope it is alot easier to be used.

I have been a Windows developer for more than +15 years, a C++ anti-java person. Well I regret that, I do love windows, however Java is a very nice language, especially when it comes to Android. Also Linux is an excellent platform that I was away from. Now my Laptop is Ubuntu :)

Anyway when I was looking for an inexpensive server for Andruav.com I had to go to Linux, at first I thought buying a VPS Ubuntu means a remote desktop with GUI, but I found my self lonely in front of back screen and all I have is ssh to connect :(.  That was horrible at the beginning, but later I was very comfort with it. I had to go through alot of issues that I will summaries here for other people who want to use Linux easily.


The platform I use is Ubuntu 12.04


Steps of installing LAMP on Ubuntu 14.04


1. Install Apache

sudo apt-get install apache2

2. Install MySQL

sudo apt-get install mysql-server
* reconfig
/usr/sbin/dpkg-reconfigure: mysql-server-5.5

3. Install PHP

sudo apt-get install php5 libapache2-mod-php5

4. Restart Server

sudo /etc/init.d/apache2 restart

5. Check Apache

http://localhost/.



7. Installing MySQLAdmin

apt-get install php5-mysql http://www.thetechrepo.com/main-articles/488.html
sudo apt-get install phpmyadmin

*reconfig phpmyadmin database:
sudo dpkg-reconfigure phpmyadmin

Add extension=mysqli.so (near other 'extension=' lines) line to your php.ini


8. Restart Server

sudo /etc/init.d/apache2 restart


If you execute these commands in order you will have LAMP on your Ubuntu


A Python Experiment in Gyro Calibration & Drift Cancelling






This article is to share my findings in gyro calibration. I tried to make a python script that uses data comes from mobile phone [S5 that has 6050 IMU]. It was a good chance to write simple code and visualize results much easier than doing so on Arduino with IMU 6050 attached. But still the concept is the same.
I used this mobile app, this is not the only one, so you can choose other if you want.

I used three approaches for Gyro calibration.
First Approach:
This is the normal approach where you read values from Gyro and then divide total by number of reads. This is the average value. Almost all quadcopter firmwares do this step before arming to make sure that the value that is read from gyro represents zero rad/s i.e. no rotations.
gyro from value will be:
     Gyro = (gyro_raw - gyro_avg ) * timediff
      where gyro_avg is the average measured by reading a sample values and divide over count.
There is nothing wrong in above approach, it is simple, and really can make your quad flies. 
Second Approach:
The second approach was almost the same, but in this trial, I took values from First averaging algorithm as a first guess,  then I ran an inner loop that tries to get error based on the original average, and tries to add correction to it.
gyro of value will be:
      Gyro = (gyro_raw - gyro_avg_dyn) * timediff
      where gyro_avg_dyn: is the best average with least error.
I find this is a equivalent of running the first approach method multiple time and try to get the best average with least error. It could be smarter than blindly get an average after n counts -first method- where you can test some values in between. This also may be more useful when using integers rather than decimal in 8-bit chips.
Third Approach:
This approach was a bit different. It assumes that original average will give zero error for the noise, however drift will still occur, and it is in one direction. So if you integrate values from gyro it will keep either increasing or decreasing in one direction -as an overall behaviour-. 
The idea was to try to read the rate of which drift occurs and store it in a value called Gyro_AvrDriftRate. The unit of this value will be rad/sec^2.
So the Value of Gyro will be:    

      Gyro = (gyro_raw - gyro_avg - (gyro_avgdriftrate * timediff) ) * timediff
     
       where gyro_avg is the average measured by reading a sample values and divide over count.
      and gyro_avgdriftrate is drift rate per second

Drift already affects the original average value, as gyro_raw is not random values with normal distribution around an offset value; as it has drift component inside. and so the original average has a skew from the zero offset. In the third approach the drift will appear clearly in the integration of readings and using time-difference one can estimate a rate for the drift.

Actual Results:

Script Logic:
     1- calculate average and drift using the three methods.

    
 2- while loop:
              a. read gyro_raw
              b. calculate gyro_value_method1,2,3

              c. integrate these values into three different variables gyro_x1_int, gyro_x3_int, gyro_x3_int

              d. print result
The best approach should have the least absolute value i.e. the nearest to zero.

Results:

I ran the code several times, the result is not the same every time. but in general the third method is promising, as in
many cases it gives less drift than the other methods.

the third method is the one has x_rad/s2 label.
Also the second method sometimes get stuck in local minimum, as it reads less values in the inner loop, and sometimes get worst that normal average. 
This is not always the case. Anyway I hope this could be a small start for others to make much more investigation.

Note: sometimes values sent from mobile jumps in crazy way without movements, so I just added a skip statement to skip those values, and I assume you run this script while leaving your mobile with no movement at all.
imu_gyroCalibration.py

Latest Code version is available at this link

Thursday, August 7, 2014

ZERO_PID Tunes for Multirotors Part#2

Introduction


                From more than a month from now Aug 2014, I posted a blog titled ZERO_PID Tune for Multicopter and also here on my blog, It was about an algorithm I developed that allows quadcopter pilot to reset Gyro PIDs to ZERO, and fly! Using this algorithm, the quadcopter was able to generate valid values for P & I, and can successfully fly, the demo was on Multiwii code. Since then I was working on enhancing this algorithm, as I discovered some opportunities for improvment.
 Multiwii_GTune1.0.2.zip

First Version Issues

                 Although as you can see in the video that quadcopter really flies, there was a devil in the details. P-factor & I-factor would saturate if you fly long enough – I discovered this later few days after the video-. Although quadcopter was safely taking off, but flying for a long time -1 to 2 min- in this mode will make P & I saturate especially if you play with sticks hard.
Another issue I have discovered in the first version was calculating I-factor assuming it is a percentage of P-factor. Although it was calculated separately from P-factor, it ended up as a percentage of P-factor.

ZERO_PID Model


       Although my background is engineering, but I am not a big fan of mathJ, so I started to simplify the problem as much as possible, and avoid complex math. The model depends on angular velocity of gyros only. It assumes there is no interference between different axes –which is true theoretically at least- as actual Gyro MEMS has slight interactions between axes.

       First, let us start by a quad arm with a motor on it as in the figure, initially the motor is running, but you do not know if it is running fast enough to generate exact thrust to keep the arm with angular velocity zero. Again let’s assume that the motor is not producing enough thrust, so when reading the gyro we will get V1 = v1. With single value you cannot judge, you need to read two values, so you wait until the next IMU loop and read V2=v2.
       Now assume that both V1 & V2 have the same signs. This means the same direction, which is falling down –counter clockwise in the figure-.
Condition#1:     If V2 > V1 : this means the arm is accelerating and the quad in danger of flip. So let us reduce the P value by one if the difference is high enough.
Condition#2:     If V2 < V1 : this means the arm is decelerating and the quad is trying to adjust itself to reach angular velocity zero. However there is a danger here that the deceleration is so fast so that it will not only stop the arm but will move in the other direction and start oscillations. So let’s decrease P value by one if the difference is high enough.
Please note that by design, quad stability is active stability, i.e. you need to monitor and adjust to keep it stable, so whatever the P value is, it will oscillate, the idea here is the oscillation amplitude should be minimal.
Condition#3: What if V1 & V2 has different signs, i.e. our quad arm has reversed its rotation direction. This case is ignored as it will always happen due to oscillations as I mentioned in the previous paragraph. And we rely on condition 2 to minimize oscillations.   
What if the arm –in the figure- rotation direction is clockwise, in this case we are calculating the right arm that is falling down. That is why we use abs(Error) . So we always consider the falling arm.
ZERO_PID algorithm can be written now as follows:
void G_Tune (Error)  {
                If (Sign(Error) == Sign (OldError))
               {
                       If ((abs(Error) - abs (OldError)) > High_Enough _1) // we are falling down.
                      {
                           P = P + 1;
                      }
                      If ((abs(Error) - abs (OldError)) <  - High_Enough_2 ) // we may oscillate.
                     {
                         P = P - 1;
                     }
                }
               OldError = Error;
}

Simple :) and yet it works. Well almost :)
As we can see the above algorithm changes P-factor only. In the first version I used a parallel condition for I-factor that increases and decreases it with 0.1 steps. Later I discovered this was not the best approach.

Challenges
Algorithm idea looks simple; however we need to consider some points that affect the performance of the algorithm.
      1. Aerodynamic Factor:  The above algorithm updates OldError = Error, actually it is not reasonable to assume that applying the updated P factor once in the PID and read the very next value will reflect the instant correct response, we need to consider motor acceleration and deceleration, as well as frame/propellers inertia, a lot of factor that makes our assumption not solid enough. In the first version I tried to use complementary filter, and give a large weight for the new value, however keeping the old values so that we can have idea of the average performance. However we still read the very next action and update P-factor after one read. In the latest version I have defined time_skip parameter that make sure that the algorithm is executed one time each ntimes of PID calls. See the figure, the curve are the values from gyro while the bar chart shows samples taken by the algorithm. In practice I found time_skip  from 7 to 20 i.e. 14ms to ms gives the correct response, out of this range the quad still flies but the model assumption is not valid and it either saturate or stay in Zero. For low value of time_skip it stays in zero because V2 – V1 is small and within a safe range so quad assumes that this is a normal speed. Also V2 in this case does not represent the effect of previous updated P. also when time_skip is high, quad reads random values with large difference, and in this case condition #1 wins and P saturates, also still this is a fake reading. So time_skip is a critical parameter, which works well as long as we are in this range 7 – 20.


      2. Taking-Off & Landing: When you land with your quadcopter, once the quad hit the ground –even smoothly-, you will be able to see sudden peeks on the accelerometer graph. It is more clear on the Acc-Z, and this means calculated P could be corrupted during take-off or landing especially. Handling this was relatively easy. I just added a condition not to calculate P when Thrust is less than one third throttle. So now you can land and then switch from G_Tune to ACRO mode, you don’t need to do that in the air, however you can still do it safely as in the first version.

         
      3. What is Error Input Parameter: PID takes Error as an input, error could be gyro reading as in hefnycopter firmware , or as in multiwii it is the difference between stick value and correspondent gyro, i.e. between aileron value and the x-axes gyro. In the first version as used the same Error value as in multiwii, but this was not very good, as it means that when you make sudden stick change it will give error and the algorithm will correct P accordingly, although the current P value could be enough and correct. First I tried to define high and low boundaries for Error values, but after more thinking I was convinced that we need to consider only gyro reading as an input, and leave stick for multiwii algorithm.

         
      4. Condition #2: Please re-read that condition again “However there is a danger here that the deceleration is so fast so that it will not only stop the arm” there is a vague word “danger”. Well how can we know if the deceleration is healthy and arm is slowing down in a proper speed, or it is slowing down too fast, well I don’t J. I tried some ranges and I found thatHigh_Enough _2 should be 1.5 to 2 times larger than High_Enough _1. Lower value for High_Enough_2 will make P always ZERO which is logic. And high value of it will make P saturate as condition #2 is hardly achieved because of the large barrier value.


Calculating I-Factor

              
                As I mentioned in the beginning of this article, I was calculating I as a percentage of P. Well in an article about sensors & PIDs I mentioned that I component in PID for gyro is used for restoring, it acts as a memory, because it is based on many values not the most recent one as P or the difference as D. I think about I component as the DC current in a signal or as a trim in your TX. Try to fly in Acro mode with I factor equal to Zero, you will find that if you tilt your try then take your hand of the sticks it will not get back, if you add some values to I-factor, it will tend to restore itself. And this is obvious because the I-component –i.e. the integration value not the I factor value- that was accumulated to overcome your stick need to be reduced back to zero. To have this effect I used a simple average variable to calculate the average value of gyro readings in a time window, and if the value is not Zero –higher or lower than a certain range- I is incremented otherwise it is decremented.

A Workable Code


                I attached a working code, you can use it to fly in ACRO mode only, as I used other PID parameters to as ZERO_PIDs parameters in order to study the effects above.

                LEVEL_P: represents High_Enough _1
                LEVEL _I: represents High_Enough_2
                ALT_P: represents time_skip counts.
                ALT_I: represents number of samples used for averaging Error for calculating I factor.
Note: if you put value ALT_I = 0.01 this means actually 10, as the minimum value is 0.001 which is 1. Same for other values, for example LEVEL_P = 1.0 means 10 and value 0.1 means 1 as there is no 0.001 and this is the minimum value for P, so take care when you play with values.

My Samples

              These figures are taken from MultiWii EZ-GUI for 3 flights:

                 as we can see High_Enough_1 = 10 & High_Enough_2 = 15 works very nice. time_skip here is 10 and values that are read for averaging to calculate I-factor is 100.
                 as I increased High_Enough_2 to 20 P values started to raise, especially in the third trial where I played more aggressive with quad compared to the calm first two flights.


Final Notice
                Still everything is experimental, and try it on your OWN RISK, try to fly smoothly and dont take off suddenly, let it leave ground easy.
 Latest Code is Here

Monday, June 2, 2014

ZERO - PIDs Tuner for Multirotors







This topic is about PID auto tuning algorithm that can tune your Quadcopter from PIDs ZERO as a start value.
Introduction
       Adjusting PIDs is a problem for many of Quadcopter’ pilots, it would be nice to get your quadcopter to adjust PIDs for itself. I made some investigation in this area, and found many complex algorithms that are related to Adaptive PIDs, and Fuzzy Logic. Although my background is engineering but I don’t like complex math when you can make simple solutions :)


        I started with multiwii version 2.3, I chose it because its code is not that complex, while it has many GUI tools and debug facilities to trace, and it has one of the largest community that can test and verify the results. Anyway I have developed a new mode that I called G_TUNE, which enables quadcopter to calculate flyable PIDs values for ACRO mode. i.e. PIDs for GYRO.
How to Use

        G_Tune mode starts by resetting all Gyro' PIDs for Roll, Pitch & Yaw to Zero’s.  Then pilot needs to takeoff !!  yes it will take off as the algorithm will start instantly to correct errors and update PIDs parameters accordingly. Pilot should try not to make aggressive maneuvers. That is because the error in multiwii is calculated as (RC_Stick - Gyroreading) as stick position acts as rotation speed target i.e. the target rpm, so moving stick suddenly generates false errors.

       

         The algorithm mainly works as our human eye. what you do is that if you find your quad tilt you give thrust to motor to adjust this tilt. 
      
      
      Below is the state machine for the new mode.


State Diagram of G-Tune
State Diagram of G-Tune


 1- As we can see from above state diagram, You need to disarm your quadcopter, then you switch to G_Tune mode. once you do that all PIDs will be Zero and you can test that by switch back to ACRO mode again and try to fly "be careful as you have no control because all PIDs are ZERO"
  2- You can then switch back to G_TUNE the arm your quadcopter. Try to fly carefully, do not make aggressive moves, your quadcopter should fly reasonably stable.
  3- Now switch to GYRO mode in AIR to stop updating the values. dont try to land first. 
  4- Now fly again in ACRO mode and this time it will fly. 
The algorithm was able to generate the following PIDs values for ACRO mode:


 Both values in the two screen shots flythe quadcopter


















Caution
 1- This is a test mode that I tested only on my quadcopter with Simonk ESC, 10x4.5 props and Turnigy 1100 KV. It should work on other configuration, but I have not try it myself.
 2- Moving ELE & ALE sticks just before changing from G_TUNE to ACRO mode could corrupt the values, so please try to fly as stable as possible and switch to ACRO while sticks are almost released for best results.
 3- Switching back from GYRO to G_Tune mode in AIR could be dangerous -however the correction is very fast and it normally works- because PIDs are reset to ZEROs in AIR.













Monday, May 19, 2014

Porting MultiWii to Arduino DUE

Porting MultiWii to Arduino DUE

GitHub is here  please compile code on Arduino-1.5.6r2 or later.

Arduino DUE is a microcontroller board based on the Atmel SAM3X8E ARM Cortex-M3 CPU. It is the next generation of Arduino boards “UNO, Nano, Mini”…etc. It is a very attractive platform for building quadcopter firmware on for the following reasons:
  •  A 32-bit core, that allows operations on 4 bytes wide data within a single CPU clock.

·         CPU Clock at 84Mhz compared to 16MHz for other Arduino.
  •          96 KBytes of SRAM compared to 8Kbytes for Arduino Mega 2560.
  •  512 KBytes of Flash memory for codecompared to 256Kbytes for the nearest giant Arduino Mega, and this number is much less in other Arduinos.
  • DMA controller, that can relieve the CPU from doing memory intensive tasks.
All these advantages do not come without a cost. The main two disadvantages from current quadcopters firmware and hardware are:

1- This warning message on Arduino website “Warning: Unlike other Arduino boards, the Arduino Due board runs at 3.3V. The maximum voltage that the I/O pins can tolerate is 3.3V. Providing higher voltages, like 5V to an I/O pin could damage the board.” This means that you cannot just attach your current 5V logic hardware to Arduino DUE.

2-      There is EEPROM memory in Arduino DUE L. And this is another very sad news as you need to have EEPROM in order to be able to save PIDs, sensors calibration values and many other values used for quadcopters. They need to be saved somewhere and without EEPROM you need to use external flash memory which is extra hardware and extra interface.

I can see that these two reasons strongly affected firmware developers to use Arduino for quadcopters. But as we will see below these were only illusions.




Problem Solved


                Naze32 was the first 32-bit board I see for MultiWii. When I found the code for the first time, the first thing I searched for is that EEPROM problem and how can we solve it. Well the solution was obvious; you can use Flash memory instead of EEPROM. You can store the data together with the code.
WAW; I said to myself. It is very nice simple approach. So I searched for similar code for Arduino DUE, and I found one written by cmaglie. That is 50% of the problem solved J

Now back to the 3.3V issue, well it seems someone has the courage to try the 3.3V and guess what, it worked J. I contacted Rouan to make sure that there is no 3.3V tweeks that I need to make, and the answer was no. I believe the 3.3V works with no problems for the following reasons:
1-      For ESC logical 1 is less than 3.3V so 3.3V as input signal for ESC is more than enough to control it.
2-      For RX seems that logical 1 is either less than 5V of maybe because the input is a pulse of with 2ms max with frequency 50Hz makes the average DC is not enough to burn the input pin.
Again the 3.3V is not an issue anymore J



Why MultiWii?


                Well, I am no longer the first one –I know- to write quadcopter code on Arduino DUE, Rouan has already made it. So I decided to pick for myself a new challenge, and I chose to convert MultiWii firmware for the following reasons:
1-      It is a very famous firmware, well known and not so complex.
2-      It is already built to talk with Arduino boards, so supporting Arduino DUE is a natural extend for the firmware.
3-      Arduino community as well as MultiWii community might find this conversion useful to start with to build much powerful updates, as the current code takes only 9% of the available Flash memory.



Steps of porting MultiWii to Arduino DUE


                My target is to build MultiWii code that you can still compile it on 8-bit Arduino boards. It is not a dedicated version for Arduino DUE –although a next version might be so-.
                So I first need to figure out where to put changes and where to keep old code. In the remaining part, I will mention the major changes as well as major challenges I faced during this process.

#if defined (ARDUINO_DUE)


As in all MultiWii code, you need to define specific code for specific board, and I have chosen to use ARDUINO_DUE as the mark. All code under ARDUINO_DUE is specifically compiled when you choose to compile for Arduino DUE only. Some code in MultiWii is generic for all boards, but I can only be executed for 8-bit microprocessors not ARM, for these parts I use #if !defined (ARDUINO_DUE)

Def.H


This is the file you need to edit to define now sensors, vehicle, board ...etc. It is a core file. I started with it and defined:
1-      When ARDUINO_DUE is active.

#if defined (ARDUINO_ARCH_SAM)
                #define ARDUINO_DUE
#endif

ARDUINO_ARCH_SAM is what you need to look for to detect an ARDUINO_DUE.

2-      Define alternatives for pgmspace by including <arm/ pgmspace.h>

3-      ARDUINO_DUE board pin assignments.
THR:      PIN 62  - A8
ALE:       PIN 63  - A9
ELE:        PIN 64  - A10
RUD:      PIN 65  - A11
AUX1:   PIN 66  -DAC0
AUX2:   PIN 67 - DAC1

Motor 1:  PIN 34
Motor 2:  PIN 36
Motor 3:  PIN 38
Motor 4 / Servo 4:  PIN 40
                Other parts of the definition are just code from ARDUINO_MEGA that will make error if activated as it is not converted yet.

    Code Changes

       I used Multiwii version 2.3 as my starting point. Most of the other code is same as MultiWii, Dramatic changes where in EEPROM.cpp Sensors.cpp, RX.cpp and Output.cpp

EEPROM.cpp I used cmaglie Flash class –I updated a paging issue in it- to save configuration structures.

RX.cpp          I used Rouan code, and it was very useful and straight forward.

Output.cpp Again I used Rouan code, but this time it was more tricky to embed with original code of MultiWii due to logic of motor/servo selection and different PWM signal generation.

Sensors.cpp I used original wire.h library, however I renamed it Wire_DUE.h and included as part of the project as I intend to update it later to make it fast. I replicated the same I2C functions that is used in MultiWii so that sensors logic stay untouched. Functions used Wire_DUE.h instead of original code when compiling for Arduino_DUE.




Main Issues I Faced


  1.     I first used __CM3_REV instead of ARDUINO_ARCH_SAM to check for DUE, and it made linking errors. I have not run through the root cause yet, but I took couple of days to solve this issue.
  2.  Types from uint8_t & int16_t in functions _getADC() in sensors.cpp . You need to make explicit type casting. Without doing so you will get rubbish data from GYRO. This is due the << operation acts without caring about the sign.
  3. Alignment !!!!! This was a very wicked tricky issue that I faced when trying to connect to WiiGUI. The application displays data correctly but gives false alarm of suiting to BARO mode –Altitude Hold- for example. And I found that this error was due to serializing structures in protocol.cpp, and the 32bit compiler stored structure items 4byte-aligned, This makes gaps between different variables and translated wrongly by WinGUI. This is why you found __attribute__ ((packed)) at each and every structure and union. This was a very time consuming bug, and very random and distributed between C# code on PC and C++ code in firmware
  4. GYRO Hopping. 
            scale = (currentT - previousT) * (GYRO_SCALE * 65536);
Scale variable that is vital for measuring angular velocity of Gyro was giving hopping from 0 to 100 even without touching any sensor. The behavior disappeared when I used int32_t for both currentT & previousT as used in the main loop in Multiwii.cpp




Code Status


                The code is not 100% converted i.e. not all features, however you compile it for Quadcopter or Tricopter and fly in different modes. I have not test GPS, sonar. I only used GY_80 as well as the HK 328 board.

                Code is still under development, and as any firmware it can contain serious issues, so fly safe and on your own risk.

Good Luck J



Sunday, June 30, 2013

KK2 v1.5 Firmware Explained Part#1

Yes, it has been a long time since I published my Article "Quadrocopter Firmware (4 Rotor) Explained" . Actually it was long enough to develop my own system on KK2 and get it listed in KK2FlashTool.

HefnyCopter-II Firmware for KK2 Board

Although my firmware has lots of new features, but KK2 code is very mature and respectable and I know that a lot of us wants to explore.

Well KK2 firmware is much more complicated that its parent firmware of KK. This is mainly because KK2 has more sensors, also has LCD which consumes big part of memory and code. So in this article I will only concentrate on stabilization algorithms of KK2.

KK2 has two flying modes, first the ACRO mode where we use only gyros to make balancing. This is fairly easy, all  you need is to read data from gyro and scale it then use a simple PID control to control motors. the other mode is Stabilization mode. This mode uses gyros and accelerometers together to calculate the angle of quadcopter "pitch-roll" and then uses PID control to calculate motor actions.

I will not go into details of gyro and acc here as you can find many articles in talks about this subject in details. but let us take an idea that enable us to proceed with code.

Gyro: sensors that detect quadcopter rotation, its output is deg/sec. so it detects rotation of quadcopter in three dimensions. Gyros are great, accurate and fast, so you can integrate the reading to get the absolute angle, but this is not the real case :( . as gyros has a problem called drift, if you leave the board not touchable and try to integrate gyro readings it will not be zero, although it is not moving or rotating in any way. but for a short time -less than a second- gyro drift can be considered zero.

Acc: sensors that detect acceleration i.e. forces. because "g" is earth acceleration you can use acc sensor to detect pitch and roll directly using simple calculations. however because quadcopter is not fixed, there are other forces such as linear acceleration and centrifugal forces that make acceleration sensors not accurate for fast readings, so you need to apply low pass filter to get a reliable value. 

So what we do is to calculate angles using gyro, then use accelerometers to correct the drift.



Imu:       ;--- Get Sensor Data ---
Step#1: Read Gyro & Acc values and refer them to Zero
                call AdcRead                                                                      ;Calculate gyro output
                ;---M.Hefny
                ;--- Gyro = Gyro - GyroZero for Roll,Pitch & YAW
                b16sub GyroRoll, GyroRoll, GyroRollZero
                b16sub GyroPitch, GyroPitch, GyroPitchZero
                b16sub GyroYaw, GyroYaw, GyroYawZero

                ;---M.Hefny
                ;--- Acc = Acc - AccZero for X Y & Z
                b16sub AccX, AccX, AccXZero                                     ;remove offset from Acc
                b16sub AccY, AccY, AccYZero
                b16sub AccZ, AccZ, AccZZero                     

                ;---M.Hefny
                ;--- AccX = AccX + AccTrimPitch
                b16add AccX, AccX, AccTrimPitch                                              ;add trim
                b16add AccY, AccY, AccTrimRoll

Step#2: Apply complementary filter to Acc to remove noise.
                ;---M.Hefny
                ;--- Low Pass Filter
                b16ldi Temper, 0.03                                                        ;SF LP filter the accerellometers

                ;---M.Hefny
                ;--- AccXfilter = AccXfilter + Temper * (AccX - AccXFilter)
                b16sub Error, AccX, AccXfilter
                b16mul Error, Error, Temper
                b16add AccXfilter, AccXfilter, Error

                ;---M.Hefny
                ;--- AccYfilter = AccYfilter + Temper * (AccY - AccYFilter)
                b16sub Error, AccY, AccYfilter
                b16mul Error, Error, Temper
                b16add AccYfilter, AccYfilter, Error

                ;---M.Hefny
                ;--- AccZfilter = AccZfilter + Temper * (AccZ - AccZFilter)
                b16sub Error, AccZ, AccZfilter
                b16mul Error, Error, Temper
                b16add AccZfilter, AccZfilter, Error


Step#3: Correct Gyro-Based calculated angles using Acc readings.

                ;---  calculate tilt angle with the acc. (this approximation is good to about 20 degrees) --

                b16ldi Temp, 0.7
                b16mul AccAngleRoll, AccYfilter, Temp
                b16mul AccAnglePitch, AccXfilter, Temp


                ;--- Add correction data to gyro inputs based on difference between Euler angles and acc angles ---

                b16ldi Temp, 20                                ;skip correction at angles greater than +-20
                b16cmp AccAnglePitch, Temp
                longbrge im40
                b16cmp AccAngleRoll, Temp
                longbrge im40 ;M.Hefny: Skip correction

                b16neg Temp  ;M.Hefny compare -20
                b16cmp AccAnglePitch, Temp
                longbrlt im40 ;M.Hefny: Skip correction
                b16cmp AccAngleRoll, Temp
                longbrlt im40 ;M.Hefny: Skip correction

                b16ldi Temp, 60                ;skip correction if vertical acceleration is outside 0.5 to 1.5 G
                b16cmp AccZfilter, Temp
                longbrge im40 ;M.Hefny: Skip correction

                b16neg Temp
                b16cmp AccZfilter, Temp
                longbrlt im40 ;M.Hefny: Skip correction
               
                ;---- M.Hefny
                ;--- GyroRoll = GyroRoll + (EulerAngleRoll - AccAngleRoll) / 2;
                b16sub Temp, EulerAngleRoll, AccAngleRoll         ;add roll correction
                b16fdiv Temp, 2
                b16add GyroRoll, GyroRoll, Temp

                ;---- M.Hefny
                ;--- GyroPitch = GyroPitch + (EulerAnglePitch - AccAnglePitch) / 2;b16sub Temp, EulerAnglePitch, AccAnglePitch                ;add pitch correction
                b16fdiv Temp, 2
                b16add GyroPitch, GyroPitch, Temp

                ;rvsetflagtrue flagDebugBuzzerOn
                rjmp im41

im40:     ;rvsetflagfalse flagDebugBuzzerOn
                                ;---- M.Hefny
                                ;--- im40 skipping to im40 means skipping correction.
                               
Step#4: Calculate Gyro-Based calculated angles.
im41:

                ;--- Rotate up-direction 3D vector with gyro inputs ---
                call Rotate3dVector

                call Lenght3dVector


                call ExtractEulerAngles

                ;--debug



                ;--- Calculate Stick and Gyro  ---

                rvbrflagfalse flagThrottleZero, im7           ;reset integrals if throttle closed
                b16clr IntegralRoll
                b16clr IntegralPitch
                b16clr IntegralYaw

im7:      
                b16fdiv RxRoll, 4                                               ;Right align to the 16.4 multiply usable bit limit.
                b16fdiv RxPitch, 4
                b16fdiv RxYaw, 4

                b16mul RxRoll, RxRoll, StickScaleRoll        ;scale Stick input.
                b16mul RxPitch, RxPitch, StickScalePitch
                b16mul RxYaw, RxYaw, StickScaleYaw
                b16mul RxThrottle, RxThrottle, StickScaleThrottle
;----- Self level ----

                rvbrflagtrue flagSelflevelOn, im31            ;skip if false
                rjmp im30           

im31:    

;--- M.Hefny
;--- Calculate Self Leveling PID using P only factor.

;--- Roll Axis Self-level P ---

                b16neg RxRoll
                ;--- M.Hefny
                ;--- RxRoll = RxRoll / 2
                b16fdiv RxRoll, 1
                ;--- M.Hefny
                ;--- TargetAngleRoll = (QuadRoll - StickRollAngle)              / 16
                b16sub Error, EulerAngleRoll, RxRoll        ;calculate error
                b16fdiv Error, 4
                ;--- M.Hefny
                ;--- calculate P term of PID.
                b16mul Value, Error, SelflevelPgain         ;Proposjonal gain
                ;--- M.Hefny
                ;---- Limit value to SelflevelPlimit
                b16mov LimitV, SelflevelPlimit                   ;Proposjonal limit
                rcall limiter
                ;--- M.Hefny
                ;--- RxRoll = Limit(SelfLevelPgain * (EulerAngle-RXRoll)/16,SelflevelPlimit)/2
                b16mov RxRoll, Value
                b16fdiv RxRoll, 1


;--- Pitch Axis Self-level P ---

                b16neg RxPitch
                ;--- M.Hefny
                ;--- RxPitch = RxPitch / 2
                b16fdiv RxPitch, 1
                ;--- M.Hefny
                ;--- TargetAnglePitch = (QuadPitch - StickPitchAngle) / 16             
                b16sub Error, EulerAnglePitch, RxPitch   ;calculate error
                b16fdiv Error, 4
                ;--- M.Hefny
                ;--- calculate P term of PID.
                b16mul Value, Error, SelflevelPgain         ;Proposjonal gain
                ;--- M.Hefny
                ;---- Limit value to SelflevelPlimit
                b16mov LimitV, SelflevelPlimit                   ;Proposjonal limit
                rcall limiter
                ;--- M.Hefny
                ;--- RxPitch = Limit(SelfLevelPgain * (EulerAngle-RXPitch)/16,SelflevelPlimit)/2
                b16mov RxPitch, Value
                b16fdiv RxPitch, 1

                ;--- M.Hefny
                ;--- EOF leveling procedure.

               
im30:


;--- Roll Axis PI ---
               
                ;--- M.Hefny
                ;--- Calculate GYRO ... Gyro is used for stabilization only.
               
                b16sub Error, GyroRoll, RxRoll                    ;calculate error
                b16fdiv Error, 1

                b16mul Value, Error, PgainRoll                   ;Proposjonal gain

                b16mov LimitV, PlimitRoll                             ;Proposjonal limit
                rcall limiter
                b16mov CommandRoll, Value

                b16fdiv Error, 3
                b16mul Temp, Error, IgainRoll                     ;Integral gain
                b16add Value, IntegralRoll, Temp

                b16mov LimitV, IlimitRoll                              ;Integral limit
                rcall limiter
                b16mov IntegralRoll, Value

                b16add CommandRoll, CommandRoll, IntegralRoll


;--- Pitch Axis PI ---

                b16sub Error, RxPitch, GyroPitch               ;calculate error
                b16fdiv Error, 1

                b16mul Value, Error, PgainPitch                 ;Proposjonal gain

                b16mov LimitV, PlimitPitch                          ;Proposjonal limit
                rcall limiter
                b16mov CommandPitch, Value

                b16fdiv Error, 3
                b16mul Temp, Error, IgainPitch                  ;Integral gain
                b16add Value, IntegralPitch, Temp

                b16mov LimitV, IlimitPitch                            ;Integral limit
                rcall limiter
                b16mov IntegralPitch, Value

                b16add CommandPitch, CommandPitch, IntegralPitch


;--- Yaw Axis PI ---

                b16sub Error, RxYaw, GyroYaw                  ;calculate error
                b16fdiv Error, 1

                b16mul Value, Error, PgainYaw                  ;Proposjonal gain

                b16mov LimitV, PlimitYaw                            ;Proposjonal limit
                rcall limiter
                b16mov CommandYaw, Value

                b16fdiv Error, 3
                b16mul Temp, Error, IgainYaw                    ;Integral gain
                b16add Value, IntegralYaw, Temp

                b16mov LimitV, IlimitYaw                             ;Integral limit
                rcall limiter
                b16mov IntegralYaw, Value

                b16add CommandYaw, CommandYaw, IntegralYaw


;------
                ret


;--- M.Hefny
;--- Function to limit value between high and low limits
limiter:
                b16cmp Value, LimitV    ;high limit
                brlt lim5
                b16mov Value, LimitV

lim5:      b16neg LimitV                   ;low limit
                b16cmp Value, LimitV
                brge lim6
                b16mov Value, LimitV


lim6:      ret