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

No comments:

Post a Comment