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