quadrotor program

7/27/2019 Quadrotor Program

1/26

AVR C PROGRAM CODE FOR QUADROTOR

View from above

Forward

;;M1 CW *\ /* M2 CCW

\ /

+

/ \

;M4 CCW */ \* M3 CW;;

;******************* SETTINGS **************************; This one determines the range of the gain potmeters..equ PotScaleRoll =10.equ PotScalePitch =10.equ PotScaleYaw = 10

; This one determines the stick sensitivity..equ StickScaleRoll = 11.equ StickScalePitch =11.equ StickScaleYaw =11; This one determines the maximum Yaw command applied, in percent..equ YawLimit = 30

.include "m168def.inc";*******************************************************

1


2/26

[Type text]

;--- 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 RxChannel1L =r82


3/26

[Type text].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 istrue).def tisp=r23 ;ISR temporary register

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

3


4/26

[Type text].macro led0_on ;macros for the LED's

sbi portb,6.endmacro.macro led0_off

cbi portb,6.endmacro

#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

#message "jmp"jmp resetjmp RxChannel2

jmp RxChannel3jmp RxChannel4jmp unused jmp

RxChannel1 jmpunused jmp

unused jmpunused jmp

unused jmpunused jmpunused jmp

unused jmpunused jmpunused jmpunused jmp

unused jmpunused jmpunused jmp

unused jmpunused jmpunused jmpunused jmp

unused jmpunused jmp

unused

4


5/26

[Type text]unused: retireset: ldi t,low(ramend);initalize stack pointer

out spl,tldi t,high(ramend)out sph,t

;--- setup IO ---ldi t,0b01111111out ddrb,tldi t,0b11000000out ddrc,tldi t,0b11110001out ddrd,t

;---- Setup timer1 to run at 1MHz ----ldi t,0b00000010sts tccr1b,t

;---- Initalize variables ---clr tsts FlagGyroCalibrated,t ;FlagGyroCalibrated = falsests FlagFcArmed,t ;FlagFcArmed = falsests CounterFcArm,tsts CounterFcDisarm,tclr RxChannelsUpdatingFlag

ldi xl,low(1520) ;prime the channelsldi xh,high(1520)mov RxChannel1L,xlmov RxChannel1H,xh

mov RxChannel2L,xlmov RxChannel2H,xhmov RxChannel3L,xlmov RxChannel3H,xhmov RxChannel4L,xlmov RxChannel4H,xh

;----5


6/26

[Type text]seiled0_on ;Flash LED once, I am alive!ldi zl,0rcall waitmsled0_offldi zh,100 ;2 second delay

ca6: ldi zl,200rcall waitmsdec zhbrne ca6

;---- Gyro direction reversing ----rcall ReadGainPotsrcall ReadGainPotsb16ldi Temp, 51b16cmp GainInRoll,Tempbrlt ca9 ;enter gyro direction reversing?rjmp ca1

ca9: ldi counterl,10 ;yes, flash led 10 timesca7: led0_on

ldi zl,0rcall waitmsled0_offldi zl,0rcall waitmsdec counterlbrne ca7

ca5: ldi zl,180rcall waitmsrcall RxGetChannelsb16ldi Temp, 30b16cmp RxInRoll,Temp ;Roll TX stick moved?ldi zl,0

6


7/26

[Type text]brge ca8b16cmp RxInPitch,Temp ;Pitch TX stick moved?ldi zl,1brge ca8b16cmp RxInYaw,Temp ;Yaw TX stick moved?ldi zl,2brge ca8b16cmp RxInCollective,Temp ;Throttle TX stick moved?ldi zl,3brge ca8rjmp ca5 ;no

ca8: ldi zh,0 ;yes, reverse directionrcall ReadEepromldi xl,0x80eor t,xlrcall WriteEeprom ;Store gyro direction to EEPROM

ca4: led0_on ;flash LEDldi zl,0rcall waitmsled0_offldi zl,0rcall waitmsrjmp ca4

;---- ESC Throttle range calibration. This outputs collective input toall motor outputs ---ca1: b16ldi Temp, 51

b16cmp GainInYaw,Tempbrge ma1 ;enter ESC throttle range calibration?ldi counterl,10 ;yes, flash led 10 times

ca2: led0_onldi zl,0rcall waitmsled0_offldi zl,0rcall waitmsdec counterl

7


8/26

[Type text]brne ca2

ca3: ldi zl,180rcall waitmsrcall RxGetChannelsb16mov MotorOut1,RxInCollective ;output collective to all

ESC'sb16mov MotorOut2,RxInCollectiveb16mov MotorOut3,RxInCollectiveb16mov MotorOut4,RxInCollectivercall output_motor_ppmrjmp ca3 ;do until the cows come home.

;--- Main loop ---ma1: rcall GetGyroDirections

rcall ReadGainPots

rcall RxGetChannels;--- Arming/Disarming ---lds t,FlagCollectiveZerotst tbreq ma80b16ldi Temp, -50 ;Disarm?b16cmp RxInYaw,Tempbrge ma81lds t,CounterFcDisarminc tsts CounterFcDisarm,tcpi t, 50

brne ma82clr tsts FlagFcArmed,trjmp ma80

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

8


9/26

[Type text]brlt ma80lds t,CounterFcArminc tsts CounterFcArm,tcpi t, 50brne ma82ser tsts FlagFcArmed,t

ma80: clr tsts CounterFcDisarm, tsts CounterFcArm,t

ma82:

ma4: clr t ;no, skip calibration, resetcalibration delay

sts CalibrationDelayCounter,tma51:

;--- Read gyros ---rcall ReadGyrosb16sub GyroRoll, GyroZeroRoll ;remove offset from gyro

outputb16sub GyroPitch, GyroZeroPitchb16sub GyroYaw, GyroZeroYaw

;rcall ShowChannels;rcall ShowGyros

;--- Start mixing by setting collective to motor input 1,2,3 and 4 ---b16mov MotorOut1,RxInCollectiveb16mov MotorOut2,RxInCollective

b16mov MotorOut3,RxInCollectiveb16mov MotorOut4,RxInCollective

9


10/26

[Type text]

;--- Calculate roll command output ---b16load GainInRoll ;scale gyro outputldi t, PotScaleRollrcall FastDivide ; divide by 2^tlds t, RollGyroDirectiontst tbrpl ma60rcall NegateXY

ma60: b16store Tempb16mul GyroRoll, Tempb16load GainInRoll ;scale stick outputldi t, StickScaleRollrcall FastDivide ; divide by 2^tb16store Tempb16mul RxInRoll, Tempb16add RxInRoll, GyroRoll ;add gyro output to stick

output

;--- Add roll command output to motor 1,2,3,4 --- Minsoo Kim(2010.09.09)

b16ldi Temp, 1.5 ;RxInRoll = RxInRoll * 1.5b16mul RxInRoll, Tempb16sub MotorOut2, RxInRollb16sub MotorOut3, RxInRollb16add MotorOut1, RxInRollb16add MotorOut4, RxInRoll;--- Calculate pitch command output ---b16load GainInPitch ;scale gyro outputldi t, PotScalePitchrcall FastDivide ; divide by 2^tlds t, PitchGyroDirection

10


11/26

[Type text]tst tbrpl ma61rcall NegateXY

ma61: b16store Tempb16mul GyroPitch, Tempb16load GainInPitch ;scale stick outputldi t, StickScalePitchrcall FastDivide ; divide by 2^tb16store Tempb16mul RxInPitch, Tempb16add RxInPitch, GyroPitch ;add gyro output to stick

output

;--- Add pitch command output to motor 1,2,3,4 --- Minsoo Kim(2010.09.09)

b16ldi Temp, 1.5 ;RxInPitch = RxInPitch * 1.5b16mul RxInPitch, Tempb16sub MotorOut3, RxInPitchb16sub MotorOut4, RxInPitchb16add MotorOut1, RxInPitchb16add MotorOut2, RxInPitch;--- Calculate yaw command output ---b16load GainInYaw ;scale gyro outputldi t, PotScaleYawrcall FastDivide ; divide by 2^tlds t,

YawGyroDirection tst t

brpl ma62

rcall NegateXYma62: b16store Temp

b16mul GyroYaw, Temp11


12/26

[Type text]

b16load GainInYaw ;scale stick outputldi t, StickScaleYawrcall FastDivide ; divide by 2^tb16store Tempb16mul RxInYaw, Tempb16add RxInYaw, GyroYaw ;add gyro output to stick

output

b16ldi Temp, -YawLimit ;limit Yaw command to -YawLimit and YawLimit

b16cmp RxInYaw, Tempbrge ma90b16mov RxInYaw, Temp

ma90: b16ldi Temp, Yawlimitb16cmp RxInYaw, Tempbrlt ma91b16mov RxInYaw, Temp

ma91:;--- Add yaw command output to motor 1,2,3 and 4 --- Minsoo Kim

(2010.09.09)b16ldi Temp, 2.0 ;RxInYaw = RxInYaw * 2b16mul RxInYaw, Tempb16sub MotorOut1, RxInYawb16add MotorOut2, RxInYaw

b16sub MotorOut3, RxInYaw

b16add MotorOut4, RxInYaw

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

b16ldi Temp, 10 ;this is the motor idle levelb16cmp MotorOut1,Temp brge ma40b16mov MotorOut1, Temp

ma40: b16cmp MotorOut2,Temp brge ma41

12


13/26

[Type text]b16mov MotorOut2, Temp

ma41: b16cmp MotorOut3,Temp brge ma42b16mov MotorOut3, Temp

ma42: b16cmp MotorOut4,Temp brge ma43b16mov MotorOut4, Temp

ma43:;---- Update Status LED ----lds xl, FlagGyroCalibrated ;LED on if (FlagGyroCalibrated &&

FlagFcArmed) truelds yl, FlagFcArmedand xl, ylbrne ma7led0_offrjmp ma8

ma7: led0_onma8:

;--- Output to motor ESC's ---lds t,FlagCollectiveZero ;turn on motors if

(FlagGyroCalibrated && FlagFcArmed && !FlagCollectiveZero) truecom tand xl,tbrne ma6b16ldi Temp, 0 ;set motor 1-4 to zerob16mov MotorOut1,Tempb16mov MotorOut2,Tempb16mov MotorOut3,Tempb16mov MotorOut4,Temp

ma6: rcall output_motor_ppm ;output ESC signalrjmp ma1

;--- End of main loop ---;--- Subroutines ---

13


14/26

[Type text];--- Output motor ppm channels 1-4 in parallell ---; Input is 0 to 100

output_motor_ppm:b16ldi Temp, 0 ;limit to zerob16cmp MotorOut1,Tempbrge ou1b16mov MotorOut1,Temp

ou1:b16cmp MotorOut2,Tempbrge ou2b16mov MotorOut2,Temp



ou4:b16ldi Temp, 100 ;limit to 100b16cmp MotorOut1,Tempbrlt ou5b16mov MotorOut1,Temp

ou5:b16cmp MotorOut2,Tempbrlt ou6b16mov MotorOut2,Temp



ou8:b16ldi Temp, 100 ;add 1ms to all channelsb16add MotorOut1,Tempb16add MotorOut2,Tempb16add MotorOut3,Tempb16add MotorOut4,Tempb16load MotorOut1 ;scale from 0-200 to 0-800 (multiply by 4)

14


15/26

[Type text]ldi t,2rcall FastMultiplyb16store MotorOut1b16load MotorOut2ldi t,2rcall FastMultiplyb16store MotorOut2b16load MotorOut3ldi t,2rcall FastMultiplyb16store MotorOut3b16load MotorOut4ldi t,2rcall FastMultiplyb16store MotorOut4

b16load MotorOut4 ;transfer to 16bit counterspush xlpush xhb16load MotorOut3push xlpush xhb16load MotorOut2push xlpush xhb16load MotorOut1movw r25:r24,xh:xlpop r27pop r26pop r29pop r28pop r31pop r30sbi motor_out_pin_1

;turn on pins

sbi motor_out_pin_2sbi motor_out_pin_3sbi motor_out_pin_4clr tldi counterl,low(801)ldi counterh,high(801)

15


16/26

[Type text]

ou13: subi r24,1 ;20 cycles (1ms = 400 counts)sbc r25,tbrcc ou9cbi motor_out_pin_1

ou9:subi r26,1sbc r27,tbrcc ou10cbi motor_out_pin_2



ou12:subi counterl,1sbc counterh,tbrcc ou13ret

;--- Read ADC's ---ReadGyros:

76543210ldi t,0b00111111sts didr0,t

76543210ldi t,0b00000000

sts adcsrb,t

76543210 ;read rollldi t,0b00000010

rcall read_adcb16store GyroRoll

76543210 ;read pitch

16


17/26

[Type text]ldi t,0b00000001rcall read_adcb16store GyroPitch; 76543210 ;read yaw ldit,0b00000000rcall read_adcb16store GyroYawret

ReadGainPots:ldi zl,3 ;get PotReverser from EEPROM

ldi zh,0rcall ReadEepromsts PotReverser,t

76543210ldi t,0b00111111sts didr0,t

76543210ldi t,0b00000000sts adcsrb,t

76543210 ;read roll gainldi t,0b00000011rcall read_adc

lds t,PotReversertst tbrmi ga1rcall invert

ga1: b16store GainInRoll; 76543210 ;read pitch gain ldi

t,0b00000100

rcall read_adclds t,PotReversertst tbrmi ga2rcall invert

ga2: b16store GainInPitch17


18/26

[Type text]; 76543210 ;read yaw gain ldit,0b00000101rcall read_adclds t,PotReversertst tbrmi ga3rcall invert

ga3: b16store GainInYawret

invert: ldi t,0x03 ;Invert Xeor xh,tldi t,0xffeor xl,tret

read_adc: ;x = adc y = 0;led2_on

sts admux,t ;set ADC channel; 76543210ldi t,0b11000110 ;start ADCsts adcsra,t

re1: lds t,adcsra ;wait for ADC to completesbrc t,6rjmp re1lds xl,adcl ;read ADClds xh,adchclr ylclr yh

;led2_offret

waitms: ;wait zl * 0.1msldi t,199

wa1: nopdec tbrne wa1dec zlbrne waitms

18


19/26

[Type text]ret

;--- Read RX channels 1-4, pin change interrupt driven ---RxChannel1:;led2_on

ser RxChannelsUpdatingFlag in tisp, sregsbis pind,1 ;rising or falling?rjmp rx1m1lds RxChannel1StartL, tcnt1l ;rising, store the start valuelds RxChannel1StartH, tcnt1hclr RxChannelsUpdatingFlag out sreg,tisp ;exit

;led2_offreti

rx1m1: lds RxChannel1L, tcnt1l ;falling, calculate the pulselength

lds RxChannel1H, tcnt1hsub RxChannel1L, RxChannel1StartLsbc RxChannel1H, RxChannel1StartHclr RxChannelsUpdatingFlag out sreg,tisp ;exit

;led2_offreti

RxChannel2:;led2_on

ser RxChannelsUpdatingFlag in tisp, sregsbis pind,2 ;rising or falling?

19


20/26

[Type text]rjmp rx2m1lds RxChannel2StartL, tcnt1l ;rising, store the start valuelds RxChannel2StartH, tcnt1hclr RxChannelsUpdatingFlag out sreg,tisp ;exit

;led2_offreti


lds RxChannel2H, tcnt1hsub RxChannel2L, RxChannel2StartLsbc RxChannel2H, RxChannel2StartHclr RxChannelsUpdatingFlag out sreg,tisp ;exit

;led2_offreti

RxChannel3:;led2_on

ser RxChannelsUpdatingFlag in tisp, sregsbis pind,3 ;rising or falling?rjmp rx3m1lds RxChannel3StartL, tcnt1l ;rising, store the start valuelds RxChannel3StartH, tcnt1hclr RxChannelsUpdatingFlag

out sreg,tisp ;exit

;led2_offreti


20


21/26

[Type text]lds RxChannel3H, tcnt1hsub RxChannel3L, RxChannel3StartLsbc RxChannel3H, RxChannel3StartHclr RxChannelsUpdatingFlag out sreg,tisp ;exit

;led2_offreti

RxChannel4:;led2_on

ser RxChannelsUpdatingFlag in tisp, sregsbis pinb,7 ;rising or falling?rjmp rx4m1lds RxChannel4StartL, tcnt1l ;rising, store the start valuelds RxChannel4StartH, tcnt1hclr RxChannelsUpdatingFlag out sreg,tisp ;exit

;led2_offreti


lds RxChannel4H, tcnt1hsub RxChannel4L, RxChannel4StartLsbc RxChannel4H, RxChannel4StartHclr RxChannelsUpdatingFlagout sreg,tisp ;exit

;led2_offreti

21


22/26

[Type text];--- Get and scale RX channel inputs ---

RxGetChannels:tst RxChannelsUpdatingFlag;channel 1 (Roll)brne RxGetChannelsmov xl,RxChannel1Lmov xh,RxChannel1Hrcall XAbsclr ylclr yhb16store RxChannel

b16mov RxInRoll,RxChannel

c2: tst RxChannelsUpdatingFlag ;channel 2 (Pitch)brne c2mov xl,RxChannel2Lmov xh,RxChannel2Hrcall XAbsclr ylclr yhb16store RxChannel

b16mov RxInPitch,RxChannel

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

mov xl,RxChannel3Lmov xh,RxChannel3Hrcall XAbsclr ylclr yh

22


23/26

[Type text]b16store RxChannelb16mov RxInCollective,RxChannel

c4: tst RxChannelsUpdatingFlag ;channel 4 (Yaw)brne c4mov xl,RxChannel4Lmov xh,RxChannel4Hrcall XAbsclr ylclr yhb16store RxChannel

b16mov RxInYaw,RxChannel

clr t ;Set FlagCollectiveZero true if collective is


24/26

[Type text]

xa1: ret

FastDivide:asr xh ;X.Y Fast divide by 2n

ror xlror yhror yldec tbrne Fastdivideret

FastMultiply:lsl ylrol yhrol xlrol xhdec tbrne FastMultiplyret

ReadEeprom:out eearl,zl ;(Z) -> tout 0x22,zhldi t,0x01out eecr,tin t, eedrret

WriteEeprom:cli

;t -> (Z)

wr1: sbic eecr,1

rjmp wr1out eearl,zlout 0x22,zhout eedr,t

24


25/26

[Type text]

76543210ldi t,0b00000100out eecr,t

76543210

ldi t,0b00000010out eecr,t

sei

retGetGyroDirections:

clr zl ;Get roll gyro directions from EEPROMclr zhrcall ReadEepromsts RollGyroDirection,tldi zl,1 ;Get pitch gyro directions from EEPROMclr zhrcall ReadEepromsts PitchGyroDirection,tldi zl,2 ;Get yaw gyro directions from EEPROMclr zhrcall ReadEepromsts YawGyroDirection,tret

NegateXY: clr t ;Negate X:Ysub t, ylmov yl, tclr t sbct, yh movyh, tclr t sbct, xl movxl, tclr t sbct, xh movxh, tret

25


26/26

[Type text]

quadrotor program

Documents