Showing posts with label stabilization. Show all posts

Sunday, June 30, 2013

KK2 v1.5 Firmware Explained Part#1

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

HefnyCopter-II Firmware for KK2 Board

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

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

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

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

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

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

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

Imu: ;--- Get Sensor Data ---

Step#1: Read Gyro & Acc values and refer them to Zero

call AdcRead ;Calculate gyro output

;---M.Hefny

;--- Gyro = Gyro - GyroZero for Roll,Pitch & YAW

b16sub GyroRoll, GyroRoll, GyroRollZero

b16sub GyroPitch, GyroPitch, GyroPitchZero

b16sub GyroYaw, GyroYaw, GyroYawZero

;---M.Hefny

;--- Acc = Acc - AccZero for X Y & Z

b16sub AccX, AccX, AccXZero ;remove offset from Acc

b16sub AccY, AccY, AccYZero

b16sub AccZ, AccZ, AccZZero

;---M.Hefny

;--- AccX = AccX + AccTrimPitch

b16add AccX, AccX, AccTrimPitch ;add trim

b16add AccY, AccY, AccTrimRoll

Step#2: Apply complementary filter to Acc to remove noise.

;---M.Hefny

;--- Low Pass Filter

b16ldi Temper, 0.03 ;SF LP filter the accerellometers

;---M.Hefny

;--- AccXfilter = AccXfilter + Temper * (AccX - AccXFilter)

b16sub Error, AccX, AccXfilter

b16mul Error, Error, Temper

b16add AccXfilter, AccXfilter, Error

;---M.Hefny

;--- AccYfilter = AccYfilter + Temper * (AccY - AccYFilter)

b16sub Error, AccY, AccYfilter

b16mul Error, Error, Temper

b16add AccYfilter, AccYfilter, Error

;---M.Hefny

;--- AccZfilter = AccZfilter + Temper * (AccZ - AccZFilter)

b16sub Error, AccZ, AccZfilter

b16mul Error, Error, Temper

b16add AccZfilter, AccZfilter, Error

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

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

b16ldi Temp, 0.7

b16mul AccAngleRoll, AccYfilter, Temp

b16mul AccAnglePitch, AccXfilter, Temp

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

b16ldi Temp, 20 ;skip correction at angles greater than +-20

b16cmp AccAnglePitch, Temp

longbrge im40

b16cmp AccAngleRoll, Temp

longbrge im40 ;M.Hefny: Skip correction

b16neg Temp ;M.Hefny compare -20

b16cmp AccAnglePitch, Temp

longbrlt im40 ;M.Hefny: Skip correction

b16cmp AccAngleRoll, Temp

longbrlt im40 ;M.Hefny: Skip correction

b16ldi Temp, 60 ;skip correction if vertical acceleration is outside 0.5 to 1.5 G

b16cmp AccZfilter, Temp

longbrge im40 ;M.Hefny: Skip correction

b16neg Temp

b16cmp AccZfilter, Temp

longbrlt im40 ;M.Hefny: Skip correction

;---- M.Hefny

;--- GyroRoll = GyroRoll + (EulerAngleRoll - AccAngleRoll) / 2;

b16sub Temp, EulerAngleRoll, AccAngleRoll ;add roll correction

b16fdiv Temp, 2

b16add GyroRoll, GyroRoll, Temp

;---- M.Hefny

;--- GyroPitch = GyroPitch + (EulerAnglePitch - AccAnglePitch) / 2;b16sub Temp, EulerAnglePitch, AccAnglePitch ;add pitch correction

b16fdiv Temp, 2

b16add GyroPitch, GyroPitch, Temp

;rvsetflagtrue flagDebugBuzzerOn

rjmp im41

im40: ;rvsetflagfalse flagDebugBuzzerOn

;---- M.Hefny

;--- im40 skipping to im40 means skipping correction.

Step#4: Calculate Gyro-Based calculated angles.

im41:

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

call Rotate3dVector

call Lenght3dVector

call ExtractEulerAngles

;--debug

;--- Calculate Stick and Gyro ---

rvbrflagfalse flagThrottleZero, im7 ;reset integrals if throttle closed

b16clr IntegralRoll

b16clr IntegralPitch

b16clr IntegralYaw

im7:

b16fdiv RxRoll, 4 ;Right align to the 16.4 multiply usable bit limit.

b16fdiv RxPitch, 4

b16fdiv RxYaw, 4

b16mul RxRoll, RxRoll, StickScaleRoll ;scale Stick input.

b16mul RxPitch, RxPitch, StickScalePitch

b16mul RxYaw, RxYaw, StickScaleYaw

b16mul RxThrottle, RxThrottle, StickScaleThrottle

;----- Self level ----

rvbrflagtrue flagSelflevelOn, im31 ;skip if false

rjmp im30

im31:

;--- M.Hefny

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

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

b16neg RxRoll

;--- M.Hefny

;--- RxRoll = RxRoll / 2

b16fdiv RxRoll, 1

;--- M.Hefny

;--- TargetAngleRoll = (QuadRoll - StickRollAngle) / 16

b16sub Error, EulerAngleRoll, RxRoll ;calculate error

b16fdiv Error, 4

;--- M.Hefny

;--- calculate P term of PID.

b16mul Value, Error, SelflevelPgain ;Proposjonal gain

;--- M.Hefny

;---- Limit value to SelflevelPlimit

b16mov LimitV, SelflevelPlimit ;Proposjonal limit

rcall limiter

;--- M.Hefny

;--- RxRoll = Limit(SelfLevelPgain * (EulerAngle-RXRoll)/16,SelflevelPlimit)/2

b16mov RxRoll, Value

b16fdiv RxRoll, 1

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

b16neg RxPitch

;--- M.Hefny

;--- RxPitch = RxPitch / 2

b16fdiv RxPitch, 1

;--- M.Hefny

;--- TargetAnglePitch = (QuadPitch - StickPitchAngle) / 16

b16sub Error, EulerAnglePitch, RxPitch ;calculate error

b16fdiv Error, 4

;--- M.Hefny

;--- calculate P term of PID.

b16mul Value, Error, SelflevelPgain ;Proposjonal gain

;--- M.Hefny

;---- Limit value to SelflevelPlimit

b16mov LimitV, SelflevelPlimit ;Proposjonal limit

rcall limiter

;--- M.Hefny

;--- RxPitch = Limit(SelfLevelPgain * (EulerAngle-RXPitch)/16,SelflevelPlimit)/2

b16mov RxPitch, Value

b16fdiv RxPitch, 1

;--- M.Hefny

;--- EOF leveling procedure.

im30:

;--- Roll Axis PI ---

;--- M.Hefny

;--- Calculate GYRO ... Gyro is used for stabilization only.

b16sub Error, GyroRoll, RxRoll ;calculate error

b16fdiv Error, 1

b16mul Value, Error, PgainRoll ;Proposjonal gain

b16mov LimitV, PlimitRoll ;Proposjonal limit

rcall limiter

b16mov CommandRoll, Value

b16fdiv Error, 3

b16mul Temp, Error, IgainRoll ;Integral gain

b16add Value, IntegralRoll, Temp

b16mov LimitV, IlimitRoll ;Integral limit

rcall limiter

b16mov IntegralRoll, Value

b16add CommandRoll, CommandRoll, IntegralRoll

;--- Pitch Axis PI ---

b16sub Error, RxPitch, GyroPitch ;calculate error

b16fdiv Error, 1

b16mul Value, Error, PgainPitch ;Proposjonal gain

b16mov LimitV, PlimitPitch ;Proposjonal limit

rcall limiter

b16mov CommandPitch, Value

b16fdiv Error, 3

b16mul Temp, Error, IgainPitch ;Integral gain

b16add Value, IntegralPitch, Temp

b16mov LimitV, IlimitPitch ;Integral limit

rcall limiter

b16mov IntegralPitch, Value

b16add CommandPitch, CommandPitch, IntegralPitch

;--- Yaw Axis PI ---

b16sub Error, RxYaw, GyroYaw ;calculate error

b16fdiv Error, 1

b16mul Value, Error, PgainYaw ;Proposjonal gain

b16mov LimitV, PlimitYaw ;Proposjonal limit

rcall limiter

b16mov CommandYaw, Value

b16fdiv Error, 3

b16mul Temp, Error, IgainYaw ;Integral gain

b16add Value, IntegralYaw, Temp

b16mov LimitV, IlimitYaw ;Integral limit

rcall limiter

b16mov IntegralYaw, Value

b16add CommandYaw, CommandYaw, IntegralYaw

;------

ret

;--- M.Hefny

;--- Function to limit value between high and low limits

limiter:

b16cmp Value, LimitV ;high limit

brlt lim5

b16mov Value, LimitV

lim5: b16neg LimitV ;low limit

b16cmp Value, LimitV

brge lim6

b16mov Value, LimitV

lim6: ret

Saturday, October 6, 2012

HefnyCopter2 Gyro Noise Filtering

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

Tuesday, September 4, 2012

QuadCopter Stabilization & Control System "X & Plus Configuration"

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