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
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:
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.
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 |
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.
is there any way of translating it into C programs, since assembly is confusing and drag
ReplyDeletepls chk HefnyCopter for kk v2.1 it is inspired from this code and should work the same.
Deletepls chk HefnyCopter for kk v2.1 it is inspired from this code and should work the same.
Deletethank dude ..checking on that..bytheway, could you refer a book or some thing like that, that covers every assembly language command here. and what is the variablr "t" used every where??. i have V1.5 of kk2 now but that is far advaced than this. uses Euler angles i think..could you work on that. Thank..cheers
Deleteu welcome :)
Deleteif u need to read some code in euler ... try ardu ... but I suggest to start with multi wii imu.c it is much easier.
u may also want to try my Hefnycopter-II for KK2 as it supports sonar and has a ground station on windows.
Deleteat least i need to know how to change the name and title that is displayed in the start up of the board,should hack it, put new title there in KK2, v1.5.every one would be happy if they see there name in there kk2. cheers
DeleteAwesome !! you just save me a lot of time !!! thx pal.
ReplyDeleteglad to hear that.thanks and good luck
DeleteThis is code explanation of IMU for KK2 firmware.
ReplyDeletehttp://technicaladventure.blogspot.com/2013/06/kk2-v15-firmware-explained-part1.html
do u have any information regarding the kk2.1 board
ReplyDeleteI contacted HK trying to get schematic of KK2.1 still waiting for their decision.
Delete