Technical Adventures

Connecting Raspberry Pi Zero W with Raspberry Pi Zero 2 W via USB-Ethernet

2022-04-03T07:34:00.005-07:00

Surprisingly it is easy!!!

In this article we will learn how to connect two raspberry pi zeros together.

In this example we will use one Raspberry Zero W & the other Raspberry Zero 2 W

you need to power only the Raspberry Zero W & the cable will power Raspberry Zero 2 W, of course you can provide a separate power source especially if you want to connect sensors and other hardware on the other board.

Same configuration can be implemented between two Raspberry Zero W.

Steps :

For you need to enable Ethernet over USB.

sudo nano /boot/config.txt

as in the above picture at the end of the file just add

dtoverlay=dwc2

Now you need to edit cmdline.txt. Look for rootwait and add modules-load=dwc2,g_ether after it immediately.

Now we will assign IPs to both boards. One of the boards will be the gateway. In our case it could be any board. but if you are using a RPI-4 where you can connect many boards together then the main board should be the gateway.

sudo nano /etc/dhcpcd.config

You need to edit the file by adding the following to the end

for the router:

interface usb0

static ip_address=192.168.11.1 # should be equal to router as this is the router

static routers=192.168.11.1

static domain_name_Servers=192.168.11.1

metric 400 # determines priority of using this net compared to wifi and others

for other boards:

interface usb0

static ip_address=192.168.11.123 # can be any ip within the subnet domain.

static routers=192.168.11.1

static domain_name_Servers=192.168.11.1

metric 400 # determines priority of using this net compared to wifi and others

After Restarting

sudo reboot now

Both boards you can see the usb0 section with the static IP

It is very easy and very useful. As serial communication will allows only one app to communication. But connecting via Ethernet means there is a many-to-many communication between applications on both boards in a reliable fashion.

Thank you....

3N+1 Problem

2022-01-11T00:54:00.000-08:00

This topic is very different than many of my previous topics. It is about math!! First I am not a mathematician and not a big fan of fan. It is hard.

The story started here when I saw this video. I love this channel and I have watched most of its video.

I was excited to know that such simple problem has not been solved. So I decided to start to play with it.

No No I have not solved it. But I came with nice graph that it might be useful.

First let's mention the rule here, take any positive integer number:

if number is odd then multiply it by 3 and add 1.

N−>3N+1

if number is even then divide it by two.

N - > N / 2

let us take some examples:

7 : 22, 11, 34, 17, 52, 26, 13, 40, 20, 10, 5, 16, 8, 4, 2, 1

11: 34, 17, 52, 26, 13, 40, 20, 10, 5, 16, 8, 4, 2, 1

easy !

Now let us focus only on the last digit of any number not the whole number. Let's rewrite the above number again:

7 : 22, 11, 34, 17, 52, 26, 13, 40, 20, 10, 5, 16, 8, 4, 2, 1

11: 34, 17, 52, 26, 13, 40, 20, 10, 5, 16, 8, 4, 2, 1

We can deduce a sequence here:

A digit ends with 1 will always gives a digit ends with 4. because

n n n n n . . 1 * 3 N + 1 = m m m m . . . 4

We can apply same technique to other odd numbers and we get the following table:

1	x3 + 1	4

3	x3 + 1	0

5	x3 + 1	6

7	x3 + 1	2

9	x3 + 1	8

For even numbers; we can deduce them using the following tables:

2	/2	1
12		6
22		11
32		16
42		21
52		26
62		31
72		36
82		41
92		46
102		51

4	/2	2
14		7
24		12
34		17
44		22
54		27
64		32
74		37
84		42
94		47
104		52

6	/2	3
16		8
26		13
36		18
46		23
56		28
66		33
76		38
86		43
96		48
106		53

8	/2	4
18		9
28		14
38		19
48		24
58		29
68		34
78		39
88		44
98		49
108		54

0	/2	0
10		5
20		10
30		15
40		20
50		25
60		30
70		35
80		40
90		45
100		50

Now we have all the information we need to draw a state machine diagram for these numbers:

you can verify this with any number generated by the 3N+1 problem and the sequence will always be valid.

What can we see in this graph:

1- The blue lines are where we divide by 2 hence reduce the value. The red lines are where we multiply by 3 and add 1 hence increase the value.

2- Blue lines are 10 and red lines are only 5. That means overall the value should decrease even if it is increased for sometime.

3- Loops with same number of arrows of different colors leads to increase in values. for example loop {6,3,0,5,6,3,0,5,6...} but the loop will break as divide by two at a moment will give a number ended by 8.

4- Loops {6,3,0,5,6,...} can never be broken except at number 6 only. Loops {9,8,9,8...} can be broken at number 8 only. Loops can only be broken at even numbers only.

5- Loop {1,4,2,1,4,2,....} there are two blue lines and one red line. The result is 1 when n =1 that is why it loops forever.

6- If someone can prove that loop {6,3,0,5,6} can be repeated forever which I do not believe it is possible, as each time you multiply you get the following assuming you do not loop in the Zero loop.

which is

The number will either loop in Zero or get out in 6 to 8 because incrementing by 10/4 is 2.5

Similar rules is applied to {8,9} and {4,7,2,1} loops. remember only balanced loop is {4,2,1} when n is {4,2,1}.

7- If you get out of a loop then to get in again you pass one or more blue lines. if you are in loop 6 and move to loop 8 and come back to 6 using worst path you need to move on {6, 8, 9, 4, 7, 2, 6} which is 2 red lines and five blue lines. The number is ALWAYS reduce.

So you have thre main rules guide the behavior here.

A- You cannot loop in one loop forever except {4,2,1}.

B- Jumping from one loop to another reduces the value.

C- Moving back and forth from loop 6 to loop 8 requires passing on loop 4. It is a Must.

So as value gets smaller and passes via 4 from 8 that explains the 4 ,2 ,1.

I know this is NOT a scientific mathematical proof. But it explains to certain extend the behavior of this formula.

How to make your Own SD Raspberry Image

2021-05-09T14:17:00.002-07:00

Why Do I need This ?

Here is the problem. Raspberry Pi SD image is less than 4GBs and you copy it on SD memory card of size 8, 16, 32 GB. It expands and you can install and customize your raspberry pi.

Now you have your own SD card with your own software that you want to reproduce. If you use dd command only for your 16GB SD card you will get a 16 GB image file. You need a 16 GB SD card to restore it again. Although the software itself can be restored in 8GB card -assuming the card is not full- and the remaining size is unused space !!!

Here are the steps to produce a compressed raspberry pi image version:

Steps:

1- Insert SD-Card you want to clone into your laptop.

2- Run lsblk

you should see something like:

sdb\sdb1 \media\$USER\boot

sdb\sdb2 \media\$USER\rootfs

3- Unmount partitions

sudo umount \media\$USER\boot

sudo umount \media\$USER\rootfs

4- Now copy the full sd-card:

sudo dd if=/dev/sdX of=~/my_large_image.img

5- Now Compress Image using pishrink

pishrink.sh ~/my_large_image.img ~/my_compressed_image.img

Further more you can compress it to zip file.

How to Get pishrink

1- Download:

wget https://raw.githubusercontent.com/Drewsif/PiShrink/master/pishrink.sh

2- Change Permission:

chmod +x pishrink.sh

3- Move it:

sudo mv pishrink.sh /usr/local/bin

Mavlink 3D-Map

2020-09-26T08:22:00.000-07:00

This is nice and easy. And you can expand it as you need.

Mavlink is communication protocol used by many autopilot including the very famous ardupilot.

Three.js is a very famous corss-browser JavaScript library used to develop animated 3D computer graphics in web browser using WebGL.

If we combine these two things together we get Mavlink3DMap, this is a simple static site that receives parsed mavlink messages and create correspondent vehicles and set their position and orientation.

SITL or a real vehicle sends Mavlink messages received by a UDP listener socket. the UDP socket redirects these messages to the static web page via websocket connection.

The 3D Map create a vehicle with a camera attached to each vehicle. The main screen shows 4 view ports -that you can change easily- each view port has a world camera that you can move using mouse.

P & O buttons used to switch between cameras. If there is SITL connected to the map then it will have its own camera and you can switch to it using P & W. then you can control it using W, S , A , D. You can press R to reset camera orientation.

The image shows 4 view ports the top left is from a world camera, then the top right is from a plane camera, the bottom left shows both a plane and a drone, then the bottom right is the drone camera.

The next image shows a quadcopter with a camera pointing forward. while the image below it shows a plane. These vehicles are defined automatically in the world as mavlink messages are received by the static webpage.

Different vehicles should have different SYSID_THISMAV

Running the simulation is very easy. you need to run udp2websocket.js using node. and use http-server to run the static website. That is it.

Then you need to run simulators or redirect data from a real vehicles. running SITL could be like the following sample:

sim_vehicle.py -j4 -v ArduPlane -M --map --console --instance 80 --out=udpout:127.0.0.1:14550 --out=udpout:192.168.1.139:16450

Port 16450 is used by the 3DMapView and port 14550 I keep it for QGC.

Your are welcome to join to enhance this simulator.

Raspberry Pi-Zero ArduPilot PCA9685

2020-08-21T20:22:00.006-07:00

Raspberry Pi-Zero ArduPilot

RZero

Index

Index 2

Introduction 3

DIY Raspberry Pi Zero Ardupilot “R-ZERO” 6

Design Decisions 6

I2C ServoDriver 7

Technical Specification: 9

Introduction

Flight Control Board FCB is essentially a microcontroller board that reads input from sensors and

remote control and processes these inputs based on algorithms that determine the output signals

that rotate servos and motors to drive/fly the vehicle.

FCB board normally contains beside a microcontroller some of sensors mainly gyro, acc, mag

& barometer on the same board. While GPS normally comes as a separate part and connects

to a socket on the board.

So almost any board can be turned to FCB if we attach the right sensors and provide a way for

RC-in & RC-out. This is true. But from a Software perspective writing autopilot systems using

today's features is almost an impossible task for one person. And even if we have the talent.

we will never have the time spent in testing and validating features by all ardupilot users.

Fortunately, Ardupilot, the best open source autopilot already supports running on Linux.

Please check this here we find the following list:

The very nice thing about the above list is the existence of Raspberry Pi based boards such as NAVIO2. That is what we are looking for because since ardupilot is open source then we will find many reusable
modules for RPI and a model to follow to define our board.

Even better we can find PXFmini board. It is a Raspberry PI Zero based board.

DIY Raspberry Pi Zero Ardupilot “R-ZERO”

From this point it seems we have a very good start. We will see how there are many reusable

Raspberry Pi codes. Most of them are very critical and not an easy task to write from scratch.

However as we mention above in our pseudo definition of FCB, FCB is processor & sensors

which is not the case for RPi-Zero. PXFmini consists of two boards, one of them is Raspberry Pi Zero

and the other is a board that holds sensors, RC-in & RC-out pins.

Now we only have Raspberry Pi Zero, and we need to make our second board.

Our second board should contains:

RC-in: to read signals from RX.
RC-out: to output signals to motors.
Sensors: to identify vehicle status and location.

Gyro
Acc
Compass
Barometer
GPS
Volt & Amp sensor for battery.

LEDS to display status.

Design Decisions

Let us first start with GPS as it is the easiest part. We only need to connect GPS TX/RX with

Raspberry Pi Zero TX/RX and it will work with no code changes. We only need to add a parameter

in the ardurover command to initialize the serial port. So no extra hardware is needed here.

For RC-In the good news is that Ardupilot already has a reusable code, a class called RCInput_RPI

for this part, you can even edit GPIO ports to use and even determine if you want to use 4 or 8 channels

or a number in between. This class is complex and advanced. I am glad I found it ready to use.

Again no extra hardware is needed here.

For LEDs again we can find a reusable code, a class called GPIO_RPI that is ready to use.

Although there is no extra hardware interface but we had to add LEDs each LED is connected in

series with a 10K resistor to limit the current and avoid burning out GPIO bin.

Now let's discuss harder parts. I used GY-80 with the following sensors:

L3G4200D (3Axis Gyroscope 2.4-3.6V)
ADXL345 (3 Axis Accelerometer. 2v- 3.6v)
HMC5883L (3-Axis Magnetic compass 2.16v-3.6v)
BMP085 (Pressure Sensor. 1.62v- 3.6v)

Again we are lucky as all these drivers already exist in Ardupilot code. Unfortunately Gyro & Acc

composite driver has some issues in implementation in the original Ardupilot repository.

So I had to rewrite it in a new way that is compatible with the way Ardupilot handles drivers

and their definitions. Technical details of these changes can be found here.

The reason I believe why this driver was somewhat buggy is due to it not used at all in the code so

it seems to be obsolete. However I found later an upgrade for this driver in another forked repository

but I believe my update is more compatible with the latest code style they use to call drivers.

This part was the first part I work on in this project, so I spent some time figuring out how to define a

board and attach sensors to it. Also what a correct driver software layout is. The problem I faced here

was that I use a composite sensor with I2C connection. PXFmini uses SPI connection and another

MPU sensor which is by the way available to buy but it was not available in the store at that time.

RC-out: This should not be the hardest part. Actually it should be a trivial part. Raspberry Pi boards

uses a 16-channel-pwm-servo-driver called PCA9685 this part has a defined driver in Ardupilot code

in a class named RCOutput_PCA9685. The last piece of this driver was sold moments before I asked

for it. And I failed to find it in electronic shops where I live. So I decided to build my own RC-output.

The system I have built is actually heavily based on multiwii code.

Multiwii is a very famous quadcopter firmware that can run other vehicles. It has been left with no

updates since 2016. CrazyFlight and similar firmwares are descended from Multiwii but support

STM32FX based flight controllers.

I2C ServoDriver

Well, this is considered an independent project that can be used in many other projects.

However, I had to build it as part of R-Zero project.

As I mentioned Multiwii is a very famous flight control firmware. However its feature is

not our concern here. What we really care about is its code for controlling actuators such as

servo and motors. And its built in configurations for many vehicles.

Besides it is a relatively very easy code to explore compared to Ardupilot.

I used to play with Multiwii many years ago, I have even transferred it to ArduinoDUE.

Anyway this was in the past, now we need to think of this code as a motor driver,

that we can send to it commands via I2C and it translates it to meaningful signals for

motors and servos. So I started by removing all unnecessary code from Multiwii.

And I added I2C slave code that receives code from Raspberry Pi via I2C.

Received values are directly injected into motors and servos with no mixing.

A safety feature was added to send MINCOMMAND value to motors if no signal is received

from I2C with a defined timeout.

I also had to write a driver basically cloned from RCOutput_PCA9685

called RCOutput_MW_I2C. It uses simpler commands but basically the same approach.

Using config.h we can define our vehicle actuators. I had to add MW_I2C_Rover as rovers

were not defined in this code. But many other vehicles are defined. However I must highlight that

I tested only on rover. So please check it carefully before you connect motors and propellers.

You can use simple small blue servos for testing, and you might need to remap output channels

in a function called mixTable().

Code for this project can be found here.

Problems

I found some problems with I2CServoDriver maybe because of conflicts between i2c slave

and timers used to generate pwm. Servos glitches with no good reason.

Anyway I could finally get PC9685 board and I activated RCOutput_PCA9685 and it worked

like a charm.

Technical Specification:

MPU GY-80 communication using I2C protocol with I2C Device Addresses:

I2C Address 0x1E is the HMC5883L
I2C Address 0x53 is the ADXL345
I2C Address 0x69 is the L3G4200D
I2C Address 0x77 is the BMP085

GPS UBlox connected to TX/RX directly.
RC-In: using GPIO: GPIO 5, 6, 12, 22, 27, 20, 21, 26.
RC-out: using Arduino D11, D9, D3 & D10. where D9 is a motor output for Throttle. & D11 for Steering in the rover.
For PCA9685 channels 1 & 3 are used and GPIO27 is used for enable the chip (not used in my code).

How to Start

First we need to compile ardupilot RZero branch. Using command: $ make rzero-rover

Then we can find the binary in subfolder ardupilot/build/rzero/bin/ardurover

We need to copy this file to Raspberry Pi Zero in /home/pi you can copy it in many ways, one of them is to open SDCard on your laptop and manually copy the file. Another easier way is to copy it using scp command.

scp ./build/rzero/bin/ardurover pi@YourRPI_IP:.

That is it.

When I run ardurover I choose it not to start until I connect to it using MissionPlanner or QGroundControl -my favorite- so that I can monitor each and every step.

sudo ./ardurover -A tcp:MyLapIP:wait -C:/ttyAMA0

You can also use other options such as:

sudo ./ardurover -A udp:MyLapIP:bcast -C:/ttyAMA0

Notice: If you make your mobile Wifi Hotspot it will take IP 192.168.43.246 which so you can add this network to your RPI zero and make it connect to your QGC on your mobile phone.

You can choose to remove “wait” and ignore initializing GPS in your first tests.

AutoRun Service

Create a file at: /lib/systemd/system/ardurover.service

[Unit]

Description=My Sample Service

After=multi-user.target

[Service]

Type=forking

ExecStart=/home/pi/ardurover -A udp:192.168.1.100:10100:bcast -C udp:192.168.1.139:14550:bcast

Restart=on-failure

[Install]

WantedBy=multi-user.target

Important Notice:

In the above version I ran ardupilot on pure linux. But Ardupilot need RealTime OS so we need to update the Raspberry PI OS and add Real Time Features.

To do that you need to follow steps from this link. It is straightforward yet not easy.

IMPORTANT real-time OS is not mandatory if you only run ardupilot without running extra apps. At least for rovers.

The Fatal Bug

This was totally unexpected. I found a bug that crashes ardupilot on RPIZero randomly. I was due to using RCInput_RPI class. I took me a while to figure it out but I could finally fix it in this
[AP_HAL_Linux: Fix RCInput_RPI Segmentation Fault #14842].

Conclusion

In this project I learnt how to define a board of in Ardupilot, adding and updating sensors. It also required decisions about design and wiring.

Useful Links

Building an autopilot from scratch using Raspberry Pi Zero: Mini Zee.
PXFmini.
Code with PCA9685 enabled with segmentation fault fixed https://github.com/HefnySco/ardupilot/tree/ppr_RaspberryZ_PCA9685
Code with Arduino MW-I2C enabled with segmentation fault fixed

https://github.com/HefnySco/ardupilot/tree/ppr_RaspberryZ

Compiling WebRTC on Ubuntu

2017-10-01T12:41:00.002-07:00

In this topic I want to share an experience in details. Maybe it helps someone trying to compile WebRTC. If you do not know aht is WebRTC then this topic is definitely not interested for you.

Compiling WebRTC is a nightmare experience that you do not need to go through in order to use WebRTC in your application. But some rare times you have to and this where this topic becomes vital.

The following steps are for compiling WebRTC branch 50 on Ubuntu 16.04.

1- Preparing Environment:

1.1 Goto page: https://cs.chromium.org/chromium/src/build/install-build-deps.sh and download script from it named install-build-deps.sh you cannot use wget, you need to open the page and copy the script into a file.
1.2 Run this script that will install all prerequisites needed for compiling process.

$ ./build/install-build-deps.sh --no-chromeos-fonts

2- Install Depot Tools

1.1 Download depot_tools

$ git clone --depth 1 https://chromium.googlesource.com/chromium/tools/depot_tools.git

1.2 now add path of depot tools to $PATH by running the following command

$ export PATH=`pwd`/depot_tools:"$PATH"

3- Download & Compile WebRTC

1.1 Download Source Code

this steps takes looooong time to download source code, without a good Internet connection you have no hope in completing this. I myself decided to get a VPS on net with extra 50GB HD for about $10 per month to be able to compile this code.

$ mkdir webrtc-checkout

$ cd webrtc-checkout

$ gclient config https://chromium.googlesource.com/external/webrtc.git@branch-heads/50 --name=src

$ gclient sync --force --with_branch_heads

$ gclient sync --force --with_branch_heads --nohooks

1.2 Define Building Parameters

$ export GYP_DEFINES="target_arch=x64 host_arch=x64 build_with_chromium=0 use_openssl=0 use_gtk=0 use_x11=0 include_examples=0 include_tests=1 fastbuild=1 remove_webcore_debug_symbols=1 include_pulse_audio=0 include_internal_video_render=0 clang=1 "

given you are in ./webrtc-checkout

$ cd src

$ git checkout branch-heads/50

1.3 Generate Build Script

$ gclient runhooks

1.4 Compile Code

after running above commands you can find two folders ./webrtc-checkout/src/out/Release and ./webrtc-checkout/src/out/Debug contain build scripts for building WebRTC based on the GYP_DEFINES that you chose above.

$ ninja -C ./out/Release

$ninja -C ./out/Debug

You will wait for a 2923 files to be compiled. On that small machine I used it took around 30 min to compile.

1.5 Generate Headers & Libraries

Now you finally have WebRTC compiled. Assuming you chose to compile Release the you will find binaries in /src/out/Release

Now you need to archive all .o objects into libwebrtc_full.a and you need to have .h headers in a directory.

to generate libwebrtc_full.a in a folder lib assuming you are still in ./webrtc-checkout/src

$ mkdir ../lib/

$ find ./ -name "*.o" -and -not -name do_not_use -and -not -name protoc -and -not -name genperf -exec ar crs ../lib/libwebrtc_full.a {} +

$ mkdir ../include

$ find ./ -name *.h -exec cp --parents '{}' ../include ';'

Please Check GitHUB for more details on sources & samples covering this topic

For higher branch numbers such as 58, 60 ...etc. you need to replace step "1.3 Generate Build Scripts" commands with:

$ gn clean out/Release

$ rm -rf ./out

$ gn gen out/Release

Problems does not end here... but above steps should work straight forward .

Linux Experience

2015-09-15T23:35:00.001-07:00

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

2015-09-15T22:58:00.000-07:00

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

ZERO_PID Tunes for Multirotors Part#2

2014-08-07T06:16:00.000-07:00

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.

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

ZERO - PIDs Tuner for Multirotors

2014-06-02T22:38:00.000-07:00

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

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.

Porting MultiWii to Arduino DUE

2014-05-19T20:17:00.001-07:00

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

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

KK2 v1.5 Firmware Explained Part#1

2013-06-30T11:52:00.000-07:00

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

HefnyCopter2 Gyro Noise Filtering

2012-10-06T02:20:00.002-07:00

HefnyCopter

HefnyCopter is a firmware specially designed for KK boards. There are two types of this firmware.

HefnyCopter

This version targets the green and blue KK boards from HobbyKing. The main feature of this version is that it allows awitching your Quadcopter from X to PLUS mode without the need to reorient the board. The main concept of this feature explained in this topic “quadcopter-stabilization-control-system”.

The hex & sourcecode is available here . Please make sure that you read the manual well before flying as POTS functions are not the same as the original firmware of KK boards.

HefnyCopter2

This is a complete new version and goes in parallel with HefnyCopter, i.e. there are still updates on HefnyCopter so they are two parallel tracks.

This firmware targets KK2 board. The board has LCD, Accelerometers, and Gyro. As well as a beter processor ATMega324. This means you have better capabilities here to help you to build a better firmware.

HefnyCopter2 still has the feature of mode switching from X to Plus. But now you can connect your board via XBEE to your PC and receive telemetry data, you can work better on you stabilization algorithm and enhance your quadcopter stability.

HefnyCopter2 also has the feature of supporting two TX, that means you can fly it with a friend and train him/her how to fly the quad.

How HefnyCopter2 Helps to Improve Stabilization

Because of the ability of sending data to PC using XBEE, I was able to visualize the data and try different approaches to filter out the noise.

The Windows application that connects to XBEE has nice GUI interface for realtime display, however the more useful part is the CSV file that it dumps for the received data. In the below figure we can see the received data. Columns in order are:

Gyro_X, Gyro_Y, GyroX, Acc_X, Acc_Y, Acc_Z, M1, M2, M3, M4

Raw Data as received from XBEE in csv format

Please note that Acc_X represent the pitch so it goes with Gyro_Y, also Gyro_Y increase when quadcopter rotates forward, while Acc_X increase while quadcopter rotates backward. i.e. Acc_X is related to Gyro_Y and with negative sign. Same applied for Acc_Y & Gyro_Y.

Acc_Z represents the gravity effects that is why it is not 0, it is 100 which is equivalent to 9.8 m/s, Gyro_Z is used to detect YAW so it has nothing to do with Acc_Z.

Values ranges from 500 to -500, while motor ranges from 0 to 1000. To keep logged data at reasonable size data is only transmitted when quadcopter is armed and motors are on. that is why the minimum values of motors Ms is 100 here in the sheet as below this values quadcopter ignores TX signal and send Zero to motors.

Let now study Gyro_Y values and apply some math on it.

Gyro_Y Data received from XBEE

as we can see above this is Gyro_Y data, I added three more columns:

Move Avg

This is a moving average column, which means each value is added to the total and then divide the total over two.

Gyro_Y_Total = (Gyro_Y_Total + Gyro_Y ) /2

LPF

LPF stands for Low-Pass-Filter, here i used a complementary filter to reduce noise. Complementary filter is not a complex math at all, at least if we focus on the formula not the derivation, and try to understand it from the simple view point. The formula is as follows:

Gyro_Y_Total = (0.95 * Gyro_Y_Total) + (0.05 * Gyro_Y)

Each time we take small part of the signal and sum it to the total. assuming that the signal has high frequency noise, so we need to smooth it by taking part of the signal and accumulate it, to avoid taking peaks that represents high frequency.

Yes there are better ways to filter noise and at specific frequencies -as we can find in sound equalizer-, but computation is difficult and time consuming. If you are interested for more complex algorithms try Kalman Filter, or read topics about Fast Fourier Transform.

LPF2

This is the same as LPF. I only changed the factors, and took double the amount in LPF.

Gyro_Y_Total = (0.90 * Gyro_Y_Total) + (0.10 * Gyro_Y)

Gyro_Y under different filters

You can see clearly from above how complementary filter does remove noise from signal. There are two lines red & blue in the complementary filter the one with some noise is LPF2 and the red smoother one is LPF where we take only 0.05 of the signal value.

HefnyCopter2 now implements complementary filters for Gyro_X, Gyro_Y & Gyro_Z and the result was a much smooth flying.

I am still working on studying different behaviors and values received from my quadcopter. I believe there is a much yet to be discovered -at least for me-.

Data Analysis Files can be downloaded from here.

Please follow up with us on HefnyCopter.net

QuadCopter Stabilization & Control System "X & Plus Configuration"

2012-09-04T15:28:00.000-07:00

QuadCopter Balancing & Controlling Separation

This topic discusses how to separate between balancing quadcopter and controlling it.

First let us discuss the balancing control techniques in different quadcopter modes. Quadcopter can fly in (+) configuration and (X) configuration. The only difference between these modes is where the front of the quad is.

The following diagram illustrates the difference.

Quadcopter Configuration "X & Plus"

Plus (+) Configuration Control

In this configuration the control as follows:

// Pitch Control
M1 = M1 + RX(Elevator);
M4 = M4 - RX(Elevator);

// Roll Control
M2 = M2 + RX(Aileron);
M3 = M3 - RX(Aileron);

This is very simple control and is found in all quadcopter code. Sure there are some checks here and there to avoid motor stopping or saturating. But the flying logic is always as above. There is also a scaling factor that is used to determine the sensitivity of the sticks and a Divide factor that limits the value range.

X Configuration Control

In this configuration the control as follows:

// Pitch Control
M1 = M1 + RX(Elevator) /2;
M3 = M3 + RX(Elevator) /2;
M4 = M4 - RX(Elevator) /2;
M2 = M2 - RX(Elevator) /2;

// Roll Control
M1 = M1 + RX(Aileron)/2;
M2 = M2 + RX(Aileron) /2;
M3 = M3 - RX(Aileron) /2;
M4 = M4 - RX(Aileron) /2;

As we can see this is the same logic, we only assumes that M1 & M3 together acts as a single virtual motor in the front, and M2 & M4 together acts as a virtual motor in the rear. Other than that it is the very same logic.

Same considerations and factors applied in the PLUS configuration go here as well.

Stabilization System

Now let us see the stabilization system. The idea is simple, if the quad is falling to the right then speed up the right motor and slow the left one with the same amount and vice versa. Also if the quad is falling down from the front arm then speed up the M1 motor and slow down M4. The rule is written as follows.

M1 = M1 + PitchAmount * PitchGain
M4 = M4 - PitchAmount * PitchGain

M2 = M2 + RollAmount * RollGain
M3 = M3 - RollAmount * RollGain

This is the basic rule that should be found in all quadcopter programs. The main difference between the different firmware approaches is how to calculate PitchAmount and RollAmount. This can simply be read from a Gyro sensor and multiply it by a constant factor to adjust the range, or we can use PID approach or even Kalman filter and combine Gyro with Acc sensors to get the exact degree.

The Gain factor is used to determine the sensitivity. Other checks such as trimming are common in quadcopter code.

Do we need Different Stabilization for X-Configuration?

Well in almost all quadcopter programs you will find that stabilization and control calculation follows the same concept. i.e. if we use X-Quadcopter then we add the PitchAmount to both motors M1 & M2. Also RollAmount is added to motors M1 & M3. This is valid approach but in fact it is not necessary at all, also there is a major drawback here.

It is not necessary because the stabilization control of Plus configuration can stabilize quadcopter if there is a simultaneous picth & roll forces, that means if the Control System is Plus of X the Stabilization System can stabilize both using the same PLUS configuration.

The main drawback is that Stabilization control requires the orient the control board so that its onboard gyro and acc are in the right directions, so you cannot switch between PLUS configuration to X-Configuration without reorient the board. So if we can keep the Stabilization System and only change the Control System we don’t need to reorient the board as Control System has nothing to do with the board sensors it only change the signal values sent to different motors.

HefnyCopter has implemented this idea successfully as in the below video.

Also please read my other topic about Gyro Noise Filtering.

See newest article Quadcopter Flight-Control Framework

My X-525 QuadCopter

2012-07-31T18:34:00.004-07:00

This is my second trial of building a quadcopter. The first quadcopter -is the one we can see below- was totally homemade, the arms were too thin and U-shape so motors were twisting the bars and it was unstable.

early steps in building frame

My hand-made frame

My second quadcopter uses exactly the same motors and board, I decided to use Hobby King X-525 Quadcopter Frame.

Building the body was easy it took 40 min to assemble it. and another 4 hours in mounting motors and soldering connections.

rubber to reduce vibrations

Flying results was different, QuadCopter is much stable now.

X-525 Frame with folding bars

Still I need training, below was my first trial, now I fly it much better however still a beginner.

If you are interested in building your quad, below is a complete list of items you need to buy. Building a quadcopter is an interesting task, although it does not require as high skills as building a scale RC plane or a helicopter where mechanics are too complex. Quadcopter are very easy to balance as well, they mainly depends on their control board for balancing so minor mismatch in CG is not an issue at all.

Bill of Materials:

- Quadcopter Frame Hobbyking X666 Glass Fiber Quadcopter Frame 666mm

- Propellers you need two propellers CW & two propellers CCW. I tried both GWS 9x5 CW & CCW as well as SF 10x45 CW & CCW. if you are a newbie here, then you need to get at least extra 4 propeller of each type i.e. CW & CCW as you should expect that the first thing you will break here are propellers.

- 4 x Brushless FC28-22

- 4 x ESC HobbyKing 20A BlueSeries Brushless Speed Controller or Mystery 30A BEC Brushless Speed Controller (Blue Series)

- Hobby King Multi-Rotor Control Board V3.0 (Atmega 328 PA) or the newer Hobbyking KK2.0 Multi-rotor LCD Flight Control Board with LCD and additional accelerometer sensors.

- 1 x LIPO Battery Turnigy 2200mAh 3S 40C Lipo Pack . The quad flies for +10 min.

- 1 x Receiver 6CH such as OrangeRx R610 Spektrum DSM2 6Ch 2.4Ghz Receiver (w/ Sat Port) it is fully compatible with Spektrum & JR yet it costs lest that 1/10 of the price of original receivers.

Other Misc. Stuff are:

- 1 x 2mm Gold Connectors 10 pairs (20pc) These connectors are slim and very suitable for your FC 28-22 wires. You will need them to connect motor to ESC.

- 1 x 10CM Male to Male Servo Lead (JR) 26AWG (10pcs/set) the set contains 10 pieces which are enough for the quadcopter . . . actually you will need four or five based on the control board you choose.

- 1 x Hobby King Quadcopter Power Distribution Board This links your power wires from ESCs together to connect them to the battery. You can also use Single Male to 4 x 3.5mm Female adapter instead based on your quadcopter size.

- 1 x Turnigy 5mm Heat Shrink Tube - BLACK (1mtr) This is important and easy to use as well. It keeps your connections tight and clean. you need a simple air drier to shrink them. It is great to isolate connectors and keep them together.

- 1 x HobbyKing Programming card for BlueSeries Brushless Speed Controller This is important to avoid the hassle of programming your ESC using transmitters and magical beeps. The quad has four ESC, you need easy and accurate programming tool to make your life easier.

Extra Stuff:

- 1 x On Board Lipoly Low Voltage Alarm (2s~4s) actually I would not say it is all extra, it is important if you plan to fly your quad high. . . you need an alarm to tell you that your battery is about to finish and you need to bring your quad down before it gets down by itself :)

- LEDs exists in different types RED - GREEN - BLUE they are great if you plan to fly quad at night

HefnyCopter Code

The video below show my quad flying using my own Hefny Copter Firmware. The new feature in this firmware is that your ability to switch between X-Quad configuration and + Quad Configuration using your remote TX only. no need to reorient the board or recompile the code.

N-Channel Switch

2012-07-18T09:57:00.000-07:00

What is N-Channel Switch

N-Channel Switch is a hardware device that is very much like multiplexer that takes input from two sources and based on a select signal it outputs on of these two signals.

General Function

Domains of Application

This is a general purpose device, so it can be used in different fields; however it was specifically designed to target RC planes.

In UAV planes you may need to take-off and land using your own skills not autonomously using plane computer. You can easily and instantly take control from your plan. Same need when testing your UAV you need to make sure that if something went wrong you are still able to get it back again. The idea here is that this switch is completely isolated from your complex UAV logic, so if main processor hangs due to software bug you can still access this switch and get your plan back.

Another use of this switch is when trainer a new pilot on RC Planes. You no longer need to have the same type of transmitters; you can even use your FM together with his 2.4 GHz in a master slave combination. All you need to do is to put two receivers on that plane, connect them to the switch and you can start your training session safely.

Features and Specification

Takes Throttle, Elevator, Rudder & Aileron input from two sources. This can be two receivers or a receiver and a UAV board.
Use PWM signal to switch from first input to the second. You can use gear, Aux1, Aux2 …etc. to make switching.
Two digital output ports can be used to turn lights or other devices on and off by using a control signal.
Failsafe Switching. In case one source stops sending valid data the switch automatically switches to the other source.

Pin Assignment

Input Ports

Output Ports

Circuit Diagram

Source Code & Hex

HEX file is here
Also Source Code is here

Handmade Arduino

2012-06-20T13:21:00.001-07:00

When I first read about Arduino, I liked how compact it is, and how much processing power I can get. Also I loved the library and the idea of standard pins. I decided to buy a Mini-Arduino to play with. It costs me 160 L.E. in Egypt, that is about 27 USD. I decided to buy an ATMega 328 chip and try to imitate the board. I bought a crystal and couple of ceramic capacitors and some resistors, and started to build up an identical circuit.

Circuit Diagram using Fritzing

My Actual Circuit identical to the diagram

[extra wires you can see connected to Arduino are for AVR programming]

The end result is using ATMEGA-326 alone with nothing connecting to it except a resistor in the reset pin.

to make sure that they run at the same speed I developed a simple application that blinks the LED on a certain rate, and I uploaded the application to both chips using AVR. LEDs started to blink on the same rate.

Application Source Code

The good side is that the handmade version costed me only 40 L.E. which 6.67 USD only.

Application can be downloaded from here.

What is Inside Spektrum DX7 Receiver

2012-06-11T03:36:00.002-07:00

In this document I try to go through my findings and my expectation of board structure and behavior.

Overview
My Tiger Trainer plane was completely destroyed in a crash. Even the muffler and the receiver were destroyed. I took this as an opportunity to know what is inside Receiver DX7, and how much is it different than much cheaper receivers such as Orange Receiver.

Boards StructureAfter some investigation I came out with the following outputs:
1- The receiver of DX7 consists of two main boards attached to each other as in figure 1

Figure 1: DX7 Receiver Board

The two boards are:

a. The smaller upper board is the actual receiving board and has two antenna attached to it – one of them was detached in my plane crash-.
b. The bottom bigger board is the main board. It takes receiver output and decodes it and sends output signals through output pins to servos.

Figure 2: Block diagram of the main board of DX7

Things to notice on this board:

1- The first thing I noticed is that the satellite external circuit is identical to the receiver circuit –smaller upper board- inside the DX7 body. To prove that I took the receiver out and solder satellite circuit wires to pins where the receiver board was connected. When I turned on the receiver and the transmitter they could connect to each other.

2- There is three receiver points on this board. One in the middle where the main board connects, one on the left where the satellite socket exists, and another one on the right that is active but cannot be used unless you take the cover and solder your wires there.

I expect this will give extra coverage because you will be able to a receiver board for each dimension and you will cover reliably 3D, so whatever the position of your plane the link will be very reliable.

Figure 3: Main Board, and pin assignment of each receiver.

Figure #3 shows how receivers are connected to CY8C27443 microcontroller.

Datasheet for this microcontroller can be found here

Output Pins:

Throttle: Pin 26 P0[6]
Aileron: Pin 4 - P0[1]
Elevator: Pin 1 - P0[7]
Rudder: Pin 24 - P0[0]
Gear: Pin 25 - P0[2]
Aux 1: Pin 3 - P0[3]
Aux 2: Pin 6 – P2[5]

What is can we conclude here is that the microcontroller receive signals from multiple receivers and determine which signal is valid. Then it simply converts serial data to PWM signal for each output. I found interesting topic here discussing details of this point by connecting satellite to Arduino http://www.dogfight.no/2011/01/spectrum-receiver-satellite-to-arduino.html

Orange Receiver

Although Orange receiver looks much more simple than DX7, but infact we can see many commons here. There is only two ICs the receiver, we should expect three here, but it seems that the second IC performs the decoding function as well, because at the end of the day they are microcontrollers and it is a matter of software updates.

The regulator is another difference, we have only one capacitor and a small regulator which gives an indication of the maximum current they can stand.

Another interested part I noticed is the signals that you receive from channels. Signals are originally send and received in series, then these signals are demultiplexed into their original channels. This means we never get any overlap between any two PWM signals if we measure them from RX output channels. Channels can never overlap.

Throttle - ALI"green-red"

The above figure shows how signals Throttle and ALI are adjacent signals, but they never over lap, as we can see in the next figure

No Signal Overlapping

The same characteristics appears between ELE & Rudder signals, if you choose THR & ELE or rudder you will notice that the distance between these to signals are far.

If you are interested in more details about signal handling in your firmware and how to protect your quadcopter from losing TX signal while armed please check this link Receiver Handling and Signal Lost Detection Explained

2012-04-07T04:18:00.001-07:00

Controlling your RC Plane from Computer using SoundCard

Controlling your RC Plane using your PC is surprisingly easy

The main idea is that RC Transmitter generates audio signals that are carried by the Radio signals using FM or spread spectrum. The receiver decode these signals and output audio signals PMM that servo understands and moves accordingly.

This link explains every thing in good details.

However the sample is for Linux, another person who helped to develop a window version in this link

What is New ?

Well the C# code above reads from a joystick and generates an audio signal. My application is basically depends on the SoundPlay.cs but allows you to control your plane from the screen directly.

I tested it on my Spektrum DX7, and an Orange RX and it worked great.

Main Things to Remember:

1- Remember to set your voice setting to 192000 Hz in your Windows settings as in the image below.
2- Set your TX to in the Trainer menu P-Link.
3- Change Windows sound volume till DX7 detects that it is connected to another Remote.

Then you can enjoy controlling your RC

You can download source code from here

and binaries

Thanks for the guys who wrote the original linux and windows version.

Quadrocopter Firmware (4 Rotor) Explained

2012-03-25T11:12:00.000-07:00

I just downloaded the code for my QuadCopter with hobbyking board from http://www.kkmulticopter.com .
I went through the code, and tried to understand each line as this is my first experience with ATmega micro controllers.

I simply downloaded a data sheet for ATmega here and went through the code line by line, it took me a while but I could understand it, it is not that complex.

The code is the same as the original I only added comments -alot of comments - so that the reader can understand what happens. a minor change i did is adding a function FlashLEDnTimes that is a replacement of code in many places.

If you need to know what items exactly you need to build a quadcopter please check here

ADC Routine Description

I also reviewed the board looking for extensions that we can connect extra sensors to hobbyking board and I found the following ports are free:
Port B: PB3,PB4, PB5.
Port D: PD0, PD4, PD5, PD6

Below is the complete code:

;-*- Quadrocopter (Quadrotor) Controller V4.5 -*-
;-*-     All Code And Hardware Design         -*-
;-*-      By Rolf R Bakke,  April, juli 2010  -*-

; (I have not peeked in others projects or code :-)

; Code is best viewed in monospace font and tab = 8


; Added gyro direction reversing.

; Added calibration delay.

; Added potmeter reversing

; Added stick scaling

; Added Yaw command limiting

; Changed gain pot scaling

; Added arming/disarming




;   View from above

;      Forward

;       M1,CW
;         *
;         |
; M2,CCW  |   M3,CCW
;   *-----+-----*
;         |
;         |
;         *
;       M4,CW





;******************* SETTINGS **************************

; This one determines the range of the gain potmeters.
; Reducing the value by one doubles the range.
; Increasing the value by one halves the range.

.equ    PotScaleRoll    = 10
.equ    PotScalePitch    = 10
.equ    PotScaleYaw    = 10


; This one determines the stick sensitivity.
; Reducing the value by one doubles the sensitivity.
; Increasing the value by one halves the sensitivity.

.equ    StickScaleRoll    = 11
.equ    StickScalePitch    = 11
.equ    StickScaleYaw    = 11


; This one determines the maximum Yaw command applied, in percent.
; Less gives less yaw autority, but also less possibility of motor saturation.
; More gives more yaw autority, but also more possibility of motor saturation during full rudder stick.

.equ    YawLimit    = 30


; This one should be set to the chip you are using.
; Atmega48  = "m48def.inc"
; Atmega88  = "m88def.inc"
; Atmega168 = "m168def.inc"
; Atmega328 = "m328def.inc"

.include "m48def.inc"    

;*******************************************************
    


;---  16.16 bit signed registers ---

.equ    Temp                =0

.equ    RxInRoll            =1
.equ    RxInPitch            =2
.equ    RxInYaw                =3
.equ    RxInCollective        =4

.equ    RxChannel            =5

.equ    GyroZeroRoll        =9
.equ    GyroZeroPitch        =10
.equ    GyroZeroYaw            =11

.equ    MotorOut1            =12
.equ    MotorOut2            =13
.equ    MotorOut3            =14
.equ    MotorOut4            =15

.equ    GyroRoll            =17
.equ    GyroPitch            =18
.equ    GyroYaw                =19

.equ    GainInRoll            =26
.equ    GainInPitch            =27
.equ    GainInYaw            =28



.equ    B16_RegAdd=0x0100    ;base address for the math library register array

.equ    B16_WorkAdd=0x0200    ;base address for the math library work area


.def    RxChannel1StartL    =r0
.def    RxChannel1StartH    =r1

.def    RxChannel2StartL    =r2
.def    RxChannel2StartH    =r3

.def    RxChannel3StartL    =r4
.def    RxChannel3StartH    =r5
    
.def    RxChannel4StartL    =r6
.def    RxChannel4StartH    =r7

.def    RxChannel1L            =r8
.def    RxChannel1H            =r9

.def    RxChannel2L            =r10
.def    RxChannel2H            =r11

.def    RxChannel3L            =r12
.def    RxChannel3H            =r13

.def    RxChannel4L            =r14
.def    RxChannel4H            =r15



.equ     FlagGyroCalibrated    =b16_workadd+24

.equ    RollGyroDirection    =b16_workadd+26
.equ    PitchGyroDirection    =b16_workadd+27
.equ    YawGyroDirection    =b16_workadd+28

.equ    CalibrationDelayCounter    =b16_workadd+29

.equ    PotReverser            =b16_workadd+30

.equ    FlagFcArmed            =b16_workadd+31
.equ    CounterFcArm        =b16_workadd+32
.equ    CounterFcDisarm        =b16_workadd+33

.equ    FlagCollectiveZero    =b16_workadd+34



.def    t=r16                ;Main temporary register

;r16,r17,r18,r19 is destroyed by the math lib


.def    counterl                =r20
.def    counterh                =r21

.def    RxChannelsUpdatingFlag    =r22    ;ISR (do not read channels while this is true) 

.def    tisp                    =r23            ;ISR temporary register

                    ;the registers marked ISR is not to be used for any other purpose 

    


.macro led0_on            ;macros for the LED's
    sbi portb,6
.endmacro
.macro led0_off
    cbi portb,6
.endmacro
.macro led2_on
    sbi portd,5
.endmacro
.macro led2_off
    cbi portd,5
.endmacro
.macro led3_on
    sbi portd,6
.endmacro
.macro led3_off
    cbi portd,6
.endmacro


; Motor-Out Ports/Pins
#define motor_out_pin_1 portb,2    ;motor output pin assignments
#define motor_out_pin_2 portb,1
#define motor_out_pin_3 portb,0
#define motor_out_pin_4 portd,7


.include "1616mathlib_ram_macros_v1.asm"

.org 0

; ------------------------------------- Interrupt Table
#if defined(__ATmega48__) || defined(__ATmega88__)
#message "rjmp"
    rjmp reset
    rjmp RxChannel2
    rjmp RxChannel3
    rjmp RxChannel4
    rjmp unused
    rjmp RxChannel1
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused
    rjmp unused

#elif defined(__ATmega168__) || defined(__ATmega328__)
#message "jmp"
    jmp reset
    jmp RxChannel2        ;IRQ 0 Handler
    jmp RxChannel3        ;IRQ 1 Handler
    jmp RxChannel4        ;PCINT0 Handler: will be triggered if any enabled PCINT07..0 pin toggles.
    jmp unused            ;PCINT1 Handler: will be triggered if any enabled PCINT14..8 pin toggles.
    jmp RxChannel1        ;PCINT2 Handler: will be triggered if any enabled PCINT23..16 pin toggles.
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
    jmp unused
#else
#error "Unsupported part:" __PART_NAME__
#endif
; ------------------------------------- Interrupt Table END

; ------------------------------------- Interrupt Handling Functions
; -- Handle unused Intrrupts
unused:    
    reti

; -- Handle Reset Intrrupt    
reset:    
    ; -- Initialize Stack Pointer
    ldi t,low(ramend)    ;initalize stack pointer
    out spl,t
    ldi t,high(ramend)
    out sph,t


    ; ------------ Setup IO
    ;
    ; ------- Port B
    ; Motor 3    :    PB0-w
    ; Motor 2    :    PB1-w
    ; Motor 1    :    PB2-w
    ; LED        :    PB6-w
    ; Rudder    :    PB7-r
    ;       76543210
    ldi t,0b01111111
    out ddrb,t 
    
    
    ; ------- Port C
    ; Gyro-Yaw    :    PC0-r ADC
    ; Gyro-Pitch:    PC1-r ADC        
    ; Gyro-Roll    :    PC2-r ADC
    ; Gain-Roll    :    PC3-r ADC
    ; Gain-Pitch:    PC4-r ADC
    ; Gain-Yaw    :    PC5-r ADC
    ;       76543210
    ldi t,0b11000000
    out ddrc,t
    
    ; ------- Port D
    ; Motor 4    :    PD7-w
    ; Roll        :     PD1-r
    ; Pitch        :     PD2-r
    ; Throttle    :     PD3-r
    ;       76543210
    ldi t,0b11110001
    out ddrd,t    
    
    
    
    
    ; ------------ EOF Setup IO

    
    ;-- Setup pin change interrupt on PD1, PD2, PD3, PB4
    ;
    
    ; Bit(0) = 1 : Pin Change Interrupt is enabled [PCINT07..0 pin will cause an interrupt.]
    ; Bit(2) = 1 : Pin Change Interrupt is enabled [PCINT23..16 pin will cause an interrupt.]
    ; PCMSK0 enables those pins individually.
    ;       76543210
    ldi t,0b00000101    ;PB7, PB1
    sts pcicr,t

    ; PCMSK0 enables those pins individually. [only PB7 makes interrupt]
    ;       76543210    
    ldi t,0b10000000    ;PB7
    sts pcmsk0,t
    
    ; PCMSK1 enables those pins individually. [only PD1 makes interrupt]
    ; please note that PD2 & PD3 is handled throuhj INT0 & INT1 directly.
    ;       76543210
    ldi t,0b00000010    ;PD1
    sts pcmsk2,t

    ; for INT0 bit [1,0]: value = 01: any logical change generate interrupt
    ; either from high to low or from low to high. This is done to detect rising and falling edges.
    ; for INT0 bit [3,2]: value = 01: any logical change generate interrupt
    ;       76543210
    ldi t,0b00000101    ;PD2, PD3
    sts eicra,t
    
    ; Enable both INT0 & INT1
    ;       76543210
    ldi t,0b00000011    ;PD2, PD3
    out eimsk,t        ;STS? OUT?  Come on!



    ;-- Setup timer1 to run at 1MHz ----
    ; bits:     7        6      5        4        3        2       1      0
    ;        ICNC1 - ICES1 - NA - WGM13 - WGM12 - CS12 - CS11 - CS10
    ; clkIO 1/8 From Prescaler
    ;       76543210
    ldi t,0b00000010
    sts tccr1b,t

    
    ;-- Initalize variables ---

    clr t
    sts FlagGyroCalibrated,t    ;FlagGyroCalibrated = false
    sts FlagFcArmed,t            ;FlagFcArmed = false

    sts CounterFcArm,t
    sts CounterFcDisarm,t
    
    clr RxChannelsUpdatingFlag
    
    ldi xl,low(1520)        ;prime the channels
    ldi xh,high(1520)

    mov RxChannel1L,xl
    mov RxChannel1H,xh
    mov RxChannel2L,xl
    mov RxChannel2H,xh
    mov RxChannel3L,xl
    mov RxChannel3H,xh
    mov RxChannel4L,xl
    mov RxChannel4H,xh


    sei ; enable interrupt

    led0_on                ;Flash LED once, I am alive!

    ldi zl,0
    rcall waitms

    led0_off

    ldi zh,100            ;2 second delay
ca6:    ldi zl,200        ;delay of waitms = zl * 0.1ms 
    rcall waitms
    dec zh
    brne ca6





;---- Gyro direction reversing ----
;---- 1: Set roll gain pot to zero.
;---- 2: Turn on flight controller.
;---- 3: LED flashes rapidly 10 times.
;---- 4: Move the stick for the gyro you want to reverse.
;---- 5: LED will blink continually.
;---- 6: Turn off flight controller.
;---- 7: If there is more gyros to be reversed, goto step 2, else set roll gain pot back. 

    rcall ReadGainPots
    rcall ReadGainPots

    b16ldi Temp, 51
    b16cmp GainInRoll,Temp
    brlt GyroDirectionReversing            ;enter gyro direction reversing?
    rjmp ca1

GyroDirectionReversing:
    ldi counterl,10            ;yes, flash led 10 times
    rcall FlashLEDnTimes




ca5:    ldi zl,180
    rcall waitms

    rcall RxGetChannels

    b16ldi Temp, 30

    b16cmp RxInRoll,Temp        ;Roll TX stick moved?
    ldi zl,0
    brge ca8

    b16cmp RxInPitch,Temp        ;Pitch TX stick moved?
    ldi zl,1
    brge ca8

    b16cmp RxInYaw,Temp            ;Yaw TX stick moved?
    ldi zl,2
    brge ca8

    b16cmp RxInCollective,Temp    ;Throttle TX stick moved?
    ldi zl,3
    brge ca8

    rjmp ca5            ;no

ca8:    ldi zh,0            ;yes, reverse direction
    rcall ReadEeprom

    ldi xl,0x80
    eor t,xl                    

    rcall WriteEeprom        ;Store gyro direction to EEPROM

ca4:    led0_on                ;flash LED
    ldi zl,0
    rcall waitms
    led0_off
    ldi zl,0
    rcall waitms
    rjmp ca4



;---- ESC Throttle range calibration. This outputs collective input to all motor outputs ---
;---- This mode is entered by turning yaw gain pot to zero and turning on the flight controller. ---
    
ca1:    b16ldi Temp, 51
    b16cmp GainInYaw,Temp
    brge Mainloop            ;enter ESC throttle range calibration?

    ldi counterl,10            ;yes, flash led 10 times
    rcall FlashLEDnTimes

ca3:    ldi zl,180
    rcall waitms

    rcall RxGetChannels

    b16mov MotorOut1,RxInCollective        ;output collective to all ESC's
    b16mov MotorOut2,RxInCollective
    b16mov MotorOut3,RxInCollective
    b16mov MotorOut4,RxInCollective
    rcall output_motor_ppm
    rjmp ca3                 ;do until the cows come home.




    ;--- Main loop ---

Mainloop:    rcall GetGyroDirections

    rcall ReadGainPots

    rcall RxGetChannels


    ;--- Arming/Disarming ---


    lds t,FlagCollectiveZero
    tst t
    breq ma80

    b16ldi Temp, -50        ;Disarm?
    b16cmp RxInYaw,Temp
    brge ma81

    lds t,CounterFcDisarm
    inc t
    sts CounterFcDisarm,t
    cpi t, 50
    brne ma82

    clr t
    sts FlagFcArmed,t
    rjmp ma80

ma81:    b16ldi Temp, 50            ;Arm?
    b16cmp RxInYaw,Temp
    brlt ma80

    lds t,CounterFcArm
    inc t
    sts CounterFcArm,t
    cpi t, 50
    brne ma82

    ser t
    sts FlagFcArmed,t

ma80:    clr t
    sts CounterFcDisarm, t
    sts CounterFcArm,t

ma82:    


    ;--- Calibrate gyro when collective below 1% ---

    lds t,FlagCollectiveZero
    tst t
    brne ma12                    ;collective below 1% ?
    rjmp ma4

ma12:    lds t,CalibrationDelayCounter            ;yes, increase delay counter
    inc t
    breq ma50                    ;delay reached 256?
    
    sts CalibrationDelayCounter,t            ;no, skip calibration.
    rjmp ma51

ma50:    b16clr GyroZeroRoll                ;yes, set gyro zero value (average of 16 readings)
    b16clr GyroZeroPitch
    b16clr GyroZeroYaw

    ldi counterl,16

ma20:    rcall ReadGyros

    b16add GyroZeroRoll, GyroRoll
    b16add GyroZeroPitch, GyroPitch
    b16add GyroZeroYaw, GyroYaw

    dec counterl
    brne ma20

    b16load GyroZeroRoll                ;divide by 16
    ldi t,4
    rcall FastDivide
    b16store GyroZeroRoll

    b16load GyroZeroPitch                ;divide by 16
    ldi t,4
    rcall FastDivide
    b16store GyroZeroPitch

    b16load GyroZeroYaw                ;divide by 16
    ldi t,4
    rcall FastDivide
    b16store GyroZeroYaw

    ser t
    sts FlagGyroCalibrated,t            ;FlagGyroCalibrated = true
    
    sts CalibrationDelayCounter,t    

    rjmp ma51
            
ma4:    clr t                        ;no, skip calibration, reset calibration delay
    sts CalibrationDelayCounter,t

ma51:




    ;--- Read gyros ---


    rcall ReadGyros
    
    b16sub GyroRoll, GyroZeroRoll            ;remove offset from gyro output
    b16sub GyroPitch, GyroZeroPitch
    b16sub GyroYaw, GyroZeroYaw
 

;rcall ShowChannels
;rcall ShowGyros


    ;--- Start mixing by setting collective to motor input 1,2,3 and 4 ---

    b16mov MotorOut1,RxInCollective
    b16mov MotorOut2,RxInCollective
    b16mov MotorOut3,RxInCollective
    b16mov MotorOut4,RxInCollective



    ;--- Calculate roll command output ---

    b16load GainInRoll                ;scale gyro output
    ldi t, PotScaleRoll
    rcall FastDivide                ;  divide by 2^t
    
    lds t, RollGyroDirection
    tst t
    brpl ma60
    
    rcall NegateXY

ma60:    b16store Temp

    b16mul GyroRoll, Temp    

    
    b16load GainInRoll                ;scale stick output
    ldi t, StickScaleRoll
    rcall FastDivide                ;  divide by 2^t
    b16store Temp

    b16mul RxInRoll, Temp


    b16add RxInRoll, GyroRoll            ;add gyro output to stick output


    
    ;--- Add roll command output to motor 2 and 3 ---

    b16add MotorOut2, RxInRoll
    b16sub MotorOut3, RxInRoll



    ;--- Calculate pitch command output ---

    b16load GainInPitch                ;scale gyro output
    ldi t, PotScalePitch
    rcall FastDivide                ;  divide by 2^t
    
    lds t, PitchGyroDirection
    tst t
    brpl ma61
    
    rcall NegateXY

ma61:    b16store Temp

    b16mul GyroPitch, Temp    

    
    b16load GainInPitch                ;scale stick output
    ldi t, StickScalePitch
    rcall FastDivide                ;  divide by 2^t
    b16store Temp

    b16mul RxInPitch, Temp


    b16add RxInPitch, GyroPitch            ;add gyro output to stick output


    
    ;--- Add pitch command output to motor 1 and 4 ---

    b16add MotorOut1, RxInPitch
    b16sub MotorOut4, RxInPitch



    ;--- Calculate yaw command output ---

    b16load GainInYaw                ;scale gyro output
    ldi t, PotScaleYaw
    rcall FastDivide                ;  divide by 2^t
    
    lds t, YawGyroDirection
    tst t
    brpl ma62
    
    rcall NegateXY

ma62:    b16store Temp

    b16mul GyroYaw, Temp    

    
    b16load GainInYaw                ;scale stick output
    ldi t, StickScaleYaw
    rcall FastDivide                ;  divide by 2^t
    b16store Temp

    b16mul RxInYaw, Temp


    b16add RxInYaw, GyroYaw                ;add gyro output to stick output


    
    b16ldi Temp, -YawLimit                ;limit Yaw command to -YawLimit and YawLimit
    b16cmp RxInYaw, Temp
    brge ma90
    b16mov RxInYaw, Temp

ma90:    b16ldi Temp, Yawlimit
    b16cmp RxInYaw, Temp
    brlt ma91
    b16mov RxInYaw, Temp
ma91:

    ;--- Add yaw command output to motor 1,2,3 and 4 ---

    b16sub MotorOut1, RxInYaw
    b16add MotorOut2, RxInYaw
    b16add MotorOut3, RxInYaw
    b16sub MotorOut4, RxInYaw



    ;--- Limit the lowest value to avoid stopping of motor if motor value is under-saturated ---

    b16ldi Temp, 10       ;this is the motor idle level

    b16cmp MotorOut1, Temp
    brge ma40
    b16mov MotorOut1, Temp

ma40:    b16cmp MotorOut2, Temp
    brge ma41
    b16mov MotorOut2, Temp

ma41:    b16cmp MotorOut3, Temp
    brge ma42
    b16mov MotorOut3, Temp

ma42:    b16cmp MotorOut4, Temp
    brge ma43
    b16mov MotorOut4, Temp

ma43:


    ;---- Update Status LED ----

    lds xl, FlagGyroCalibrated        ;LED on if (FlagGyroCalibrated && FlagFcArmed) true
    lds yl, FlagFcArmed
    and xl, yl
    brne ma7
    led0_off
    rjmp OutputToMotorESC
ma7:    led0_on        


;--- Output to motor ESC's ---

OutputToMotorESC:


    lds t,FlagCollectiveZero        ;turn on motors if (FlagGyroCalibrated && FlagFcArmed && !FlagCollectiveZero) true
    com t
    and xl,t
    brne ma6

    b16ldi Temp, 0                ;set motor 1-4 to zero
    b16mov MotorOut1,Temp
    b16mov MotorOut2,Temp
    b16mov MotorOut3,Temp
    b16mov MotorOut4,Temp

ma6:    rcall output_motor_ppm            ;output ESC signal

    rjmp Mainloop


    ;--- End of main loop ---


    ;--- Subroutines ---


;--- Output motor ppm channels 1-4 in parallell --- 
;--- Input is 0 to 100 part #1
;--- Trim if greater than 100 ot less than 0.

output_motor_ppm:

    b16ldi Temp, 0            ;limit to zero
    b16cmp MotorOut1,Temp
    brge ou1
    b16mov MotorOut1,Temp
ou1:
    b16cmp MotorOut2,Temp
    brge ou2
    b16mov MotorOut2,Temp
ou2:
    b16cmp MotorOut3,Temp
    brge ou3
    b16mov MotorOut3,Temp
ou3:
    b16cmp MotorOut4,Temp
    brge ou4
    b16mov MotorOut4,Temp
ou4:

    b16ldi Temp, 100        ;limit to 100
    b16cmp MotorOut1,Temp
    brlt ou5
    b16mov MotorOut1,Temp
ou5:
    b16cmp MotorOut2,Temp
    brlt ou6
    b16mov MotorOut2,Temp
ou6:
    b16cmp MotorOut3,Temp
    brlt ou7
    b16mov MotorOut3,Temp
ou7:
    b16cmp MotorOut4,Temp
    brlt ou8
    b16mov MotorOut4,Temp
ou8:

    
    b16ldi Temp, 100        ;add 1ms to all channels
    b16add MotorOut1,Temp
    b16add MotorOut2,Temp
    b16add MotorOut3,Temp
    b16add MotorOut4,Temp


    b16load MotorOut1        ;scale from 0-200 to 0-800 (multiply by 4)
    ldi t,2
    rcall FastMultiply
    b16store MotorOut1

    b16load MotorOut2
    ldi t,2
    rcall FastMultiply
    b16store MotorOut2

    b16load MotorOut3
    ldi t,2
    rcall FastMultiply
    b16store MotorOut3

    b16load MotorOut4
    ldi t,2
    rcall FastMultiply
    b16store MotorOut4


    b16load MotorOut4        ;transfer to 16bit counters
    push xl
    push xh
    b16load MotorOut3        
    push xl
    push xh
    b16load MotorOut2        
    push xl
    push xh
    b16load MotorOut1        
    movw r25:r24,xh:xl

    pop r27
    pop r26

    pop r29
    pop r28

    pop r31
    pop r30

    ;--- Output Signals to ESC actual part #2
    ;--- Send "1s"  to all ESC.
    sbi motor_out_pin_1        ;turn on pins
    sbi motor_out_pin_2
    sbi motor_out_pin_3
    sbi motor_out_pin_4

    clr t
    ;--- This is the total length of  square pulse sent to ESC.
    ldi counterl,low(801)
    ldi counterh,high(801)

    ;--- Output Signals to ESC actual part #3
    ;--- This is the monitor and control part: it keep watching the length of the "on" signal for each motor.
    ;--- The main logic is that there is a big loop that includes all motors.
    ;--- All pins are on, and each pin "motor" has its own Registry [R25,R26,R27,R28,R29,R30,R31] that 
    ;--- defines the positive duration of the pulse "duty cycle".

lbl_MotorOutPin1:    
    subi r24,1                ;20 cycles (1ms = 400 counts)
    sbc r25,t
    brcc lbl_MotorOutPin2    ; Have we reached the end of the "on" part; no then check the next ESC.
    cbi motor_out_pin_1        ; if yes then output "0".

lbl_MotorOutPin2:    
    subi r26,1
    sbc r27,t
    brcc lbl_MotorOutPin3
    cbi motor_out_pin_2
lbl_MotorOutPin3:
    subi r28,1
    sbc r29,t
    brcc lbl_MotorOutPin4
    cbi motor_out_pin_3
lbl_MotorOutPin4:    
    subi r30,1
    sbc r31,t
    brcc lbl_MotorOutPin5
    cbi motor_out_pin_4
lbl_MotorOutPin5:    
    subi counterl,1
    sbc counterh,t
    brcc lbl_MotorOutPin1    ; hefny:L continue go to first motor.

    ret    





;--- function: Read ADC's ---
ReadGyros:
    ; Digital Input Disable Registers (DIDR)
    ; To reduce power by diable Digital Inpurt Register.
    ; Correspondent digital pins will read Zero when diable DIR.
    ; Bits:  7 - 6  -   5    -    4    -    3    -    2    -    1    -    0
    ;        NA - NA - ADC5D - ADC4D - ADC3D - ADC2D - ADC1D - ADC0D
    ;       76543210
    ldi t,0b00111111
    sts didr0,t ; Digital Input Disable Registers (DIDR)

    ; ADCSRB: ADC Control & Status Register B
    ; Enable ADC0
    ; Bits:        7  -   6  -  5 - 4 -  3  -   2   -   1   -   0
    ;            NA - ACME - NA - NA - NA - ADTS2 - ADTS1 - ADTS0
    ;     ACME [0]: select the negative input of the Analog Comparator.
    ;    ADTS2 - ADTS1 - ADTS0 [0,0,0]: means Free Running Mode.
    ;                                 however [ADATE] is clearned so they have no effect here.
    ;       76543210
    ldi t,0b00000000
    sts adcsrb,t

    ; Enable ADC2
    ;       76543210    ;read roll
    ldi t,0b00000010
    rcall read_adc
    b16store GyroRoll
    
    ; Enable ADC1
    ;       76543210    ;read pitch
    ldi t,0b00000001
    rcall read_adc
    b16store GyroPitch

    ; Enable ADC0
    ;       76543210    ;read yaw
    ldi t,0b00000000
    rcall read_adc
    b16store GyroYaw

    ret



ReadGainPots:

    ldi zl,3        ;get PotReverser from EEPROM
    ldi zh,0
    rcall ReadEeprom
    sts PotReverser,t


    ; Digital Input Disable Registers (DIDR)
    ; To reduce power by diable Digital Inpurt Register.
    ; Correspondent digital pins will read Zero when diable DIR.
    ; Bits:  7 - 6  -   5    -    4    -    3    -    2    -    1    -    0
    ;        NA - NA - ADC5D - ADC4D - ADC3D - ADC2D - ADC1D - ADC0D
    ;       76543210
    ldi t,0b00111111
    sts didr0,t

    ; ADCSRB: ADC Control & Status Register B
    ; Enable ADC0
    ; Bits:        7  -   6  -  5 - 4 -  3  -   2   -   1   -   0
    ;            NA - ACME - NA - NA - NA - ADTS2 - ADTS1 - ADTS0
    ;     ACME [0]: select the negative input of the Analog Comparator.
    ;    ADTS2 - ADTS1 - ADTS0 [0,0,0]: means Free Running Mode.
    ;                                 however [ADATE] is clearned so they have no effect here.
    ;       76543210
    ldi t,0b00000000
    sts adcsrb,t


    ; Enable ADC3
    ;       76543210    ;read roll gain
    ldi t,0b00000011
    rcall read_adc

    lds t,PotReverser
    tst t
    brmi ga1
    rcall invert
ga1:    b16store GainInRoll

    ;       76543210    ;read pitch gain
    ldi t,0b00000100
    rcall read_adc

    lds t,PotReverser
    tst t
    brmi ga2
    rcall invert
ga2:    b16store GainInPitch

    ;       76543210    ;read yaw gain
    ldi t,0b00000101
    rcall read_adc

    lds t,PotReverser
    tst t
    brmi ga3
    rcall invert
ga3:    b16store GainInYaw
    
    ret

invert:    ldi t,0x03        ;Invert X
    eor xh,t
    ldi t,0xff
    eor xl,t
    ret


read_adc:            ;x = adc    y = 0
    ; ADMUX: ADC Multiplexer Selection Register.
    ; Bits:
    ;        REFS1 - REFS0 - ADLAR - NA - MUX3 - MUX2 - MUX1 - MUX0
    ; for REFS1 - REFS0 [0,0]: means use AREF, Internal Vref & AVCC both are turned off.
    ; MUX2..0: selects ADC number.
    sts admux,t        ;set ADC channel
    
    ; ADCSRA: ADC Control & Status Register.
    ; Bits:
    ;    ADEN - ADSC - ADATE - ADIF - ADIE - ADPES2 - ADPS1 - ADPS0
    ;   ADCEnable : on
    ;    ADSC: on - start reading . if done with ADEN then it takes 25 cycle instead of 13 
    ;          and first conversion pefromes initialization of the ADC.
    ;    ADATE "auto trigger enables" : off 
    ;   ADIF "ADC interrupt Flag" : off
    ;   ADIE "ADC Interrupt Enable" : off
    ;   ADPS2..0: "ADC Prescale Select Bits" : division factor = 64.
    ;       76543210
    ldi t,0b11000110    ;start ADC
    sts adcsra,t

re1:    
    ; ADSC turns back to zero when reading is finished.
    ; Setting ADEN to zero while this loop will terminate the conversion process.
    lds t,adcsra        ;wait for ADC to complete.
    sbrc t,6            ;is ADSC is ZERO
    rjmp re1

    lds xl,adcl        ;read ADC Data High & Low ADC Data Register.
    lds xh,adch

    clr yl
    clr yh

    ret





;--- function: wait for n ms.
;--- wait time = zl * 0.1 ms
waitms:            ;wait zl * 0.1ms
    ldi t,199
wa1:    nop
    dec t
    brne wa1
    dec zl
    brne waitms
    ret





    ;--- Read RX channels 1-4, pin change interrupt driven ---
    ;these have to be fast as possible to minimize jitter in loop timed events

RxChannel1:
;led2_on
    ser RxChannelsUpdatingFlag

    in tisp, sreg

    sbis pind,1                ;rising or falling?
    rjmp rx1m1


    lds RxChannel1StartL, tcnt1l        ;rising, store the start value
    lds RxChannel1StartH, tcnt1h

    clr RxChannelsUpdatingFlag

    out sreg,tisp                ;exit    
;led2_off
    reti


rx1m1:    lds RxChannel1L, tcnt1l            ;falling, calculate the pulse length
    lds RxChannel1H, tcnt1h

    sub RxChannel1L, RxChannel1StartL
    sbc RxChannel1H, RxChannel1StartH

    clr RxChannelsUpdatingFlag

    out sreg,tisp                ;exit    
;led2_off
    reti


RxChannel2:
;led2_on
    ser RxChannelsUpdatingFlag

    in tisp, sreg

    sbis pind,2                ;rising or falling?
    rjmp rx2m1


    lds RxChannel2StartL, tcnt1l        ;rising, store the start value
    lds RxChannel2StartH, tcnt1h

    clr RxChannelsUpdatingFlag

    out sreg,tisp                ;exit    
;led2_off
    reti


rx2m1:    lds RxChannel2L, tcnt1l            ;falling, calculate the pulse length
    lds RxChannel2H, tcnt1h

    sub RxChannel2L, RxChannel2StartL
    sbc RxChannel2H, RxChannel2StartH

    clr RxChannelsUpdatingFlag

    out sreg,tisp                ;exit    
;led2_off
    reti



RxChannel3:
;led2_on
    ser RxChannelsUpdatingFlag

    in tisp, sreg

    sbis pind,3                ;rising or falling?
    rjmp rx3m1


    lds RxChannel3StartL, tcnt1l        ;rising, store the start value
    lds RxChannel3StartH, tcnt1h

    clr RxChannelsUpdatingFlag

    out sreg,tisp                ;exit    
;led2_off
    reti


rx3m1:    lds RxChannel3L, tcnt1l            ;falling, calculate the pulse length
    lds RxChannel3H, tcnt1h

    sub RxChannel3L, RxChannel3StartL
    sbc RxChannel3H, RxChannel3StartH

    clr RxChannelsUpdatingFlag

    out sreg,tisp                ;exit    
;led2_off
    reti



RxChannel4:
;led2_on
    ser RxChannelsUpdatingFlag

    in tisp, sreg

    sbis pinb,7                ;rising or falling?
    rjmp rx4m1


    lds RxChannel4StartL, tcnt1l        ;rising, store the start value
    lds RxChannel4StartH, tcnt1h

    clr RxChannelsUpdatingFlag

    out sreg,tisp                ;exit    
;led2_off
    reti

; ------------------------------------- EOF Interrupt Handling Functions


rx4m1:    lds RxChannel4L, tcnt1l            ;falling, calculate the pulse length
    lds RxChannel4H, tcnt1h

    sub RxChannel4L, RxChannel4StartL
    sbc RxChannel4H, RxChannel4StartH

    clr RxChannelsUpdatingFlag

    out sreg,tisp                ;exit    
;led2_off
    reti




    
    ;--- Get and scale RX channel inputs ---


RxGetChannels:
    tst RxChannelsUpdatingFlag    ;channel 1 (Roll)    
    brne RxGetChannels

    mov xl,RxChannel1L
    mov xh,RxChannel1H

    rcall XAbs

    clr yl
    clr yh
    b16store RxChannel

    rcall ScaleMinus100To100

    b16mov RxInRoll,RxChannel



c2:    tst RxChannelsUpdatingFlag    ;channel 2 (Pitch)    
    brne c2

    mov xl,RxChannel2L
    mov xh,RxChannel2H

    rcall XAbs

    clr yl
    clr yh
    b16store RxChannel

    rcall ScaleMinus100To100

    b16mov RxInPitch,RxChannel



c3:    tst RxChannelsUpdatingFlag    ;channel 3 (Collective)    
    brne c3

    mov xl,RxChannel3L
    mov xh,RxChannel3H

    rcall XAbs

    clr yl
    clr yh
    b16store RxChannel

    rcall Scale0To100

    b16mov RxInCollective,RxChannel



c4:    tst RxChannelsUpdatingFlag    ;channel 4 (Yaw)    
    brne c4

    mov xl,RxChannel4L
    mov xh,RxChannel4H

    rcall XAbs

    clr yl
    clr yh
    b16store RxChannel

    rcall ScaleMinus100To100

    b16mov RxInYaw,RxChannel

    

    clr t                ;Set FlagCollectiveZero true if collective is < 1
    sts FlagCollectiveZero,t

    b16ldi Temp, 1
    b16cmp RxInCollective,Temp
    brge c5

    ser t
    sts FlagCollectiveZero,t

c5:    ret



XAbs:    tst xh        ;X = ABS(X)
    brpl xa1

    ldi t,0xff
    eor xl,t
    eor xh,t
    
    ldi t,1
    add xl,t
    clr t
    adc xh,t

xa1:    ret


ScaleMinus100To100:
    b16ldi Temp, 1520
    b16sub RxChannel, Temp
    b16load RxChannel    ;divide RxChannel by 4
    ldi t,2
    rcall FastDivide
    b16store RxChannel
    ret


Scale0To100:
    b16ldi Temp, 1120
    b16sub RxChannel, Temp
    brcc sc1
    b16ldi RxChannel, 0
sc1:    b16load RxChannel    ;divide RxChannel by 8
    ldi t,3
    rcall FastDivide
    b16store RxChannel
    
    ret



FastDivide:
    asr xh        ;X.Y Fast divide by 2n
    ror xl
    ror yh
    ror yl
    dec t
    brne Fastdivide
    ret


FastMultiply:
    lsl yl
    rol yh
    rol xl
    rol xh
    dec t
    brne FastMultiply
    ret



ReadEeprom:
    out eearl,zl    ;(Z) -> t
    out 0x22,zh

    ldi t,0x01
    out eecr,t

    in t, eedr
    ret


WriteEeprom:
    cli        ;t -> (Z)

wr1:    sbic eecr,1
    rjmp wr1

    out eearl,zl
    out 0x22,zh

    out eedr,t

    ;       76543210
    ldi t,0b00000100
    out eecr,t

    ;       76543210
    ldi t,0b00000010
    out eecr,t

    sei

    ret


GetGyroDirections:
    clr zl                ;Get roll gyro directions from EEPROM
    clr zh
    rcall ReadEeprom
    sts RollGyroDirection,t

    ldi zl,1            ;Get pitch gyro directions from EEPROM
    clr zh
    rcall ReadEeprom
    sts PitchGyroDirection,t

    ldi zl,2            ;Get yaw gyro directions from EEPROM
    clr zh
    rcall ReadEeprom
    sts YawGyroDirection,t

    ret


NegateXY: clr t                ;Negate X:Y
    sub t, yl
    mov yl, t

    clr t
    sbc t, yh
    mov yh, t

    clr t
    sbc t, xl
    mov xl, t

    clr t
    sbc t, xh
    mov xh, t

    ret

; -- function: flash LED n times.    
; -- counterl is number of flashes
FlashLEDnTimes:

lblFlashLEDnTimes:    
    led0_on
    ldi zl,0
    rcall waitms
    led0_off
    ldi zl,0
    rcall waitms
    dec counterl
    brne lblFlashLEDnTimes
    ret

    ;--- Debug: ----
/*
ShowGyros:
    ldi xl,0x0d
    rcall SerbyteOut

    ldi xl,0x0a
    rcall SerbyteOut


    b16load GyroRoll
    rcall B1616Out
    ldi xl,' '
    rcall SerByteOut

    b16load GyroPitch
    rcall B1616Out
    ldi xl,' '
    rcall SerByteOut

    b16load GyroYaw
    rcall B1616Out
    ldi xl,' '
    rcall SerByteOut

    ret



ShowChannels:
    
    ldi xl,0x0d
    rcall SerbyteOut

    ldi xl,0x0a
    rcall SerbyteOut


    b16load RxInRoll
    rcall B1616Out
    ldi xl,' '
    rcall SerByteOut

    b16load RxInPitch
    rcall B1616Out
    ldi xl,' '
    rcall SerByteOut

    b16load RxInCollective
    rcall B1616Out
    ldi xl,' '
    rcall SerByteOut

    b16load RxInYaw
    rcall B1616Out
    ldi xl,' '
    rcall SerByteOut

    ret




B1616Out:
    push yl
    push yh
    push xl

    mov xl,xh
    rcall SerByteAsciiOut
    pop xl
    rcall SerByteAsciiOut
    ldi xl,'.'
    rcall SerByteOut
    pop xl
    rcall SerByteAsciiOut
    pop xl
    rcall SerByteAsciiOut
    ret


B16Out:
    push xl

    mov xl,xh
    rcall SerByteAsciiOut
    pop xl
    rcall SerByteAsciiOut
    ret



    ;--- Debug: Output byte xl (ASCII) to serial port pin at 115200 8N1 ----

SerByteAsciiOut:


    push xl
    swap xl
    rcall su1        ;high nibble
    pop xl
    rjmp su1        ;low nibble

su1:    andi xl,0x0f
    ldi zl,low(su2*2)    ;output one nibble in ASCII
    add zl,xl
    ldi zh,high(su2*2)
    clr xl
    adc zh,xl
    lpm xl,z
    rjmp SerByteOut

su2:    .db "0123456789ABCDEF"


        



    ;--- Debug: Output byte xl (binary) to serial port pin at 115200 8N1 ----

SerByteOut:
    led3_off        ;startbit
    nop
    nop
    nop

    rcall BaudRateDelay    

    ldi xh,8        ;databits

se3:    ror xl

    brcc se1
    nop
    led3_on
    rjmp se2
se1:    led3_off
    nop
    nop

se2:    rcall BaudRateDelay

    dec xh
    brne se3

    nop
    nop
    nop
    nop

    led3_on            ;stopbit
    nop 
    nop
    nop
    rcall BaudRateDelay

    ret

BaudRatedelay:

    ldi t,17
ba1:    dec t
    brne ba1
    ret
    

*/


.undef t

.include "1616mathlib_ram_v1.asm"

I hope you find this is useful.

Months Later I built my second -and first truly flying- quadcopter I also wrote a new code with a new features that allows you to switch between +Quad & X-Quad using your remote control, without the need to recompile the code or orient the board.