quadrotor program
TRANSCRIPT
-
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
-
7/27/2019 Quadrotor Program
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 RxChannel2StartL =r2
.def RxChannel2StartH =r3
.def RxChannel3StartL =r4
.def RxChannel3StartH =r5
.def RxChannel4StartL =r6
.def RxChannel4StartH =r7
.def RxChannel1L =r82
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
ou2:b16cmp MotorOut3,Tempbrge ou3b16mov MotorOut3,Temp
ou3:b16cmp MotorOut4,Tempbrge ou4b16mov MotorOut4,Temp
ou4:b16ldi Temp, 100 ;limit to 100b16cmp MotorOut1,Tempbrlt ou5b16mov MotorOut1,Temp
ou5:b16cmp MotorOut2,Tempbrlt ou6b16mov MotorOut2,Temp
ou6:b16cmp MotorOut3,Tempbrlt ou7b16mov MotorOut3,Temp
ou7:b16cmp MotorOut4,Tempbrlt ou8b16mov MotorOut4,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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
ou10:subi r28,1sbc r29,tbrcc ou11cbi motor_out_pin_3
ou11:subi r30,1sbc r31,tbrcc ou12cbi motor_out_pin_4
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
20/26
[Type text]rjmp rx2m1lds RxChannel2StartL, tcnt1l ;rising, store the start valuelds RxChannel2StartH, tcnt1hclr RxChannelsUpdatingFlag out sreg,tisp ;exit
;led2_offreti
rx2m1: lds RxChannel2L, tcnt1l ;falling, calculate the pulselength
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
rx3m1: lds RxChannel3L, tcnt1l ;falling, calculate the pulselength
20
-
7/27/2019 Quadrotor Program
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
rx4m1: lds RxChannel4L, tcnt1l ;falling, calculate the pulselength
lds RxChannel4H, tcnt1hsub RxChannel4L, RxChannel4StartLsbc RxChannel4H, RxChannel4StartHclr RxChannelsUpdatingFlagout sreg,tisp ;exit
;led2_offreti
21
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
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
-
7/27/2019 Quadrotor Program
26/26
[Type text]